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