From 6de173ce1ea474740bc484f7b7a672f06a3a4684 Mon Sep 17 00:00:00 2001
From: gabryelreyes <gabryel.reyes@newtec.de>
Date: Mon, 6 Nov 2023 15:09:14 +0100
Subject: [PATCH] Fixed orientation wrapping

---
 doc/architecture/README.md |  4 ++++
 lib/Service/Odometry.cpp   | 15 +--------------
 2 files changed, 5 insertions(+), 14 deletions(-)

diff --git a/doc/architecture/README.md b/doc/architecture/README.md
index 0444ab60..67b169ba 100644
--- a/doc/architecture/README.md
+++ b/doc/architecture/README.md
@@ -70,6 +70,10 @@ Base equations:
 Orientation:
 * $alpha [rad] = \frac{distanceRight [mm] - distanceLeft [mm]}{wheelBase [mm]}$
 * $orientation' [rad] = orientation [rad] + alpha [rad]$
+* $orientation' [rad] = orientation [rad]~\%~2\pi$
+* $-2\pi < Orientation < 2\pi$
+* After wrapping on the positive limit $2\pi$, the orientation remains positive and starts from 0 again.
+* After wrapping on the negative limit $2\pi$, the orientation remains negative and starts from 0 again.
 
 Position:
 * $dX [mm] = -distanceCenter [mm] \cdot sin(orientation' [rad])$ <- Approximation for performance reason
diff --git a/lib/Service/Odometry.cpp b/lib/Service/Odometry.cpp
index 3adca1ea..7f3c003f 100644
--- a/lib/Service/Odometry.cpp
+++ b/lib/Service/Odometry.cpp
@@ -197,20 +197,7 @@ int32_t Odometry::calculateOrientation(int32_t orientation, int16_t stepsLeft, i
 
     /* Calculate orientation */
     orientation += alpha;
-
-    /* -2*PI < alpha < +2*PI */
-    if (FP_2PI() < orientation)
-    {
-        orientation = orientation - FP_4PI();
-    }
-    else if (-FP_2PI() > orientation)
-    {
-        orientation = orientation + FP_4PI();
-    }
-    else
-    {
-        ;
-    }
+    orientation %= FP_2PI(); /* -2*PI < orientation < +2*PI */
 
     return orientation;
 }