From c0a0bfe1fcc007dc5b9111942f69568932ef95f3 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 6 May 2020 19:19:26 +0200 Subject: [PATCH] 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 --- CHANGELOG.md | 4 ++++ Firmware/MotorControl/trapTraj.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8102a27ec..a162270d4 100644 --- a/CHANGELOG.md +++ b/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. diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index f1e41aa50..ea244cd23 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/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;