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

@@ -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

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