From c45ef8934b80e30d5689712dca515af68c3afa1f Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Thu, 23 Apr 2020 14:35:49 +0200
Subject: [PATCH] Change wrap_pm to be based on fmodf

Before this commit, bad user input could cause the device to hang,
that is, no longer respond on USB (other side effects are likely,
though not tested for).

An example of such an input is:

odrv0.axis0.encoder.config.cpr = 10
odrv0.axis0.encoder.config.offset = 20000
---
 CHANGELOG.md                    |  2 ++
 Firmware/MotorControl/utils.hpp | 26 ++++++++++++++------------
 2 files changed, 16 insertions(+), 12 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 1918483f..13d5fa31 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -32,6 +32,8 @@ Please add a note of your changes below this heading if you make a Pull Request.
 * Some Encoder settings have been made read-only
 * Cleaned up VSCode C/C++ Configuration settings on Windows with recursive includePath
 * Now compiling with C++17
+* Fixed a firmware hang that could occur from unlikely but possible user input
+
 # Releases
 ## [0.4.11] - 2019-07-25
 ### Added
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index 574aa800..224f3716 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -69,18 +69,6 @@ static const float one_by_sqrt3 = 0.57735026919f;
 static const float two_by_sqrt3 = 1.15470053838f;
 static const float sqrt3_by_2 = 0.86602540378f;
 
-//beware of inserting large values!
-static inline float wrap_pm(float x, float pm_range) {
-    while (x >= pm_range) x -= (2.0f * pm_range);
-    while (x < -pm_range) x += (2.0f * pm_range);
-    return x;
-}
-
-//beware of inserting large angles!
-static inline float wrap_pm_pi(float theta) {
-    return wrap_pm(theta, M_PI);
-}
-
 // like fmodf, but always positive
 static inline float fmodf_pos(float x, float y) {
     float out = fmodf(x, y);
@@ -89,6 +77,20 @@ static inline float fmodf_pos(float x, float y) {
     return out;
 }
 
+/**
+ * @brief Similar to modulo operator, except that the output range is centered
+ * around zero.
+ * The returned value is always in the range [-pm_range, pm_range).
+ */
+static inline float wrap_pm(float x, float pm_range) {
+    return fmodf_pos(x + pm_range, 2.0f * pm_range) - pm_range;
+}
+
+//beware of inserting large angles!
+static inline float wrap_pm_pi(float theta) {
+    return wrap_pm(theta, M_PI);
+}
+
 // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
 // as per the magnitude invariant clarke transform
 // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2