From 2a94cb2088e2b4fb727d91b3953397fe92a3198a Mon Sep 17 00:00:00 2001
From: James Aguilar <aguilar.james@gmail.com>
Date: Sat, 25 Apr 2026 17:36:46 -0600
Subject: [PATCH] Improve _normalizeAngle speed.

Previously _normalizeAngle used fmodf, which is expensive on Cortex-M
series chips (40-100 cycles). For normalizing the angle, we can use
a trick of just subtracking 2 pi times the number of full rotations from
the angle.

This method does lose precision much faster than the fmodf approach.
There will be significant error if the total number of radians passed
in is greater than 10^5. However, we can counteract this by not passing
in such a large number of radians -- basically making sure that we are
not using an accumulating radian count in any location where the angle
will eventually need to be normalized. This should typically be the case
since accumulating radian use cases are definitionally not in need of
normalization. We should consider adding an assert here to ensure
that we catch any existing cases where we should be passing the
mechanical or electrical angle and are instead passing an accumulator.

Performance test (on STM32G474, with certain other optimizations):

Before:   foc_nanos:13737
After:    foc_nanos:12550 (approx 9% reduction in loopFOC time)
---
 src/common/base_classes/FOCMotor.cpp | 10 +++++++---
 src/common/foc_utils.cpp             |  7 ++++---
 2 files changed, 11 insertions(+), 6 deletions(-)

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