From b780a451b2a2361f7016b1494eb4f0551c8a2159 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Mon, 23 Oct 2023 23:07:58 +0200
Subject: [PATCH] Faster atan2

---
 src/common/foc_utils.cpp | 27 +++++++++++++++++++++++++++
 src/common/foc_utils.h   |  5 +++++
 2 files changed, 32 insertions(+)

diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp
index 4cb09863..9136dd73 100644
--- a/src/common/foc_utils.cpp
+++ b/src/common/foc_utils.cpp
@@ -44,6 +44,33 @@ __attribute__((weak)) void _sincos(float a, float* s, float* c){
   *c = _cos(a);
 }
 
+// fast_atan2 based on https://math.stackexchange.com/a/1105038/81278
+// Via Odrive project
+// https://github.com/odriverobotics/ODrive/blob/master/Firmware/MotorControl/utils.cpp
+// This function is MIT licenced, copyright Oskar Weigl/Odrive Robotics
+// The origin for Odrive atan2 is public domain. Thanks to Odrive for making
+// it easy to borrow.
+__attribute__((weak)) float _atan2(float y, float x) {
+    // a := min (|x|, |y|) / max (|x|, |y|)
+    float abs_y = fabsf(y);
+    float abs_x = fabsf(x);
+    // inject FLT_MIN in denominator to avoid division by zero
+    float a = min(abs_x, abs_y) / (max(abs_x, abs_y));
+    // s := a * a
+    float s = a * a;
+    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
+    float r =
+        ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
+    // if |y| > |x| then r := 1.57079637 - r
+    if (abs_y > abs_x) r = 1.57079637f - r;
+    // if x < 0 then r := 3.14159274 - r
+    if (x < 0.0f) r = 3.14159274f - r;
+    // if y < 0 then r := -r
+    if (y < 0.0f) r = -r;
+
+    return r;
+  }
+
 
 // normalizing radian angle to [0,2PI]
 __attribute__((weak)) float _normalizeAngle(float angle){
diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h
index 0efe3b59..abdeebf8 100644
--- a/src/common/foc_utils.h
+++ b/src/common/foc_utils.h
@@ -79,6 +79,11 @@ float _cos(float a);
  */
 void _sincos(float a, float* s, float* c);
 
+/**
+ * Function approximating atan2 
+ * 
+ */
+float _atan2(float y, float x);
 
 /**
  * normalizing radian angle to [0,2PI]