diff --git a/shared/src/AngleUtils.h b/shared/src/AngleUtils.h
index 5e5a0aa2ed..d241fa3fe6 100644
--- a/shared/src/AngleUtils.h
+++ b/shared/src/AngleUtils.h
@@ -68,12 +68,12 @@ void angleHorizontalPolar(float& azimuth, float& altitude)
     if (altitude > Unit::half_pi())
     {
         altitude = Unit::pi() - altitude;
-        azimuth = -azimuth;
+        azimuth += Unit::pi();
     }
     else if (altitude < -Unit::half_pi())
     {
         altitude = -Unit::pi() - altitude;
-        azimuth = -azimuth;
+        azimuth += Unit::pi();
     }
     azimuth = angleUnsignedNormal<Unit>(azimuth);
 }