diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp
index a193229a..601ef67b 100644
--- a/src/common/base_classes/FOCMotor.cpp
+++ b/src/common/base_classes/FOCMotor.cpp
@@ -1,5 +1,6 @@
 #include "FOCMotor.h"
 #include "../../communication/SimpleFOCDebug.h"
+#include "common/foc_utils.h"
 
 /**
  * Default constructor - setting all variabels to default values
@@ -431,6 +432,9 @@ float FOCMotor::angleOpenloop(float target_angle){
     shaft_angle = target_angle;
     shaft_velocity = 0;
   }
+  // Normalize the shaft angle after each iteration to prevent it growing indefinitely
+  // and eventually losing precision.
+  shaft_angle = _normalizeAngle(shaft_angle);
 
   // save timestamp for next call
   open_loop_timestamp = now_us;
@@ -864,7 +868,7 @@ int FOCMotor::alignSensor() {
     // move one electrical revolution forward
     for (int i = 0; i <=500; i++ ) {
       float angle = _3PI_2 + _2PI * i / 500.0f;
-      setPhaseVoltage(voltage_align, 0,  angle);
+      setPhaseVoltage(voltage_align, 0, _normalizeAngle(angle));
 	    sensor->update();
       _delay(2);
     }
@@ -874,7 +878,7 @@ int FOCMotor::alignSensor() {
     // move one electrical revolution backwards
     for (int i = 500; i >=0; i-- ) {
       float angle = _3PI_2 + _2PI * i / 500.0f ;
-      setPhaseVoltage(voltage_align, 0,  angle);
+      setPhaseVoltage(voltage_align, 0, _normalizeAngle(angle));
 	    sensor->update();
       _delay(2);
     }
@@ -914,7 +918,7 @@ int FOCMotor::alignSensor() {
     sensor->update();
     // get the current zero electric angle
     zero_electric_angle = 0;
-    zero_electric_angle = electricalAngle();
+      zero_electric_angle = _normalizeAngle(electricalAngle());
     _delay(20);
     SIMPLEFOC_MOTOR_DEBUG("Zero elec. angle: ", zero_electric_angle);
     // stop everything
diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp
index 7ae372f7..97aca4b8 100644
--- a/src/common/foc_utils.cpp
+++ b/src/common/foc_utils.cpp
@@ -75,13 +75,14 @@ __attribute__((weak)) float _atan2(float y, float x) {
 
 // normalizing radian angle to [0,2PI]
 __attribute__((weak)) float _normalizeAngle(float angle){
-  float a = fmod(angle, _2PI);
-  return a >= 0 ? a : (a + _2PI);
+  constexpr float INV_2PI = 1.f / _2PI;
+  float rotations = floorf(angle * INV_2PI);
+  return angle - (rotations * _2PI);
 }
 
 // Electrical angle calculation
 float _electricalAngle(float shaft_angle, int pole_pairs) {
-  return (shaft_angle * pole_pairs);
+  return _normalizeAngle(shaft_angle * pole_pairs);
 }
 
 // square root approximation function using