diff --git a/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp b/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp
index 2095ee8..edeafcc 100644
--- a/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp
+++ b/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp
@@ -46,38 +46,66 @@ bool SimpleFOC_CORDIC_Config(void){
     return true;
 };
 
+static bool arg2_modified = false;
 
 
 float _sin(float a) {
-    a = fmod(a, _2PI);
-    if (a>_PI) a -= _2PI;
-    if (a<-_PI) a += _2PI;
-    CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000);
-    q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result
-    q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result
-    return (float)out_sin / (float)0x80000000;
+    float s, c;
+    _sincos(a, &s, &c);
+    return s;
 }
 
 float _cos(float a) {
-    a = fmod(a, _2PI);
-    if (a>_PI) a -= _2PI;
-    if (a<-_PI) a += _2PI;    
-    CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000);
-    q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result
-    q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result
-    return (float)out_cos / (float)0x80000000;
+    float s, c;
+    _sincos(a, &s, &c);
+    return c;
 }
 
 void _sincos(float a, float* s, float* c) {
     a = fmod(a, _2PI);
     if (a>_PI) a -= _2PI;
-    if (a<-_PI) a += _2PI;    
+    if (a<-_PI) a += _2PI;
+    LL_CORDIC_SetFunction(CORDIC, LL_CORDIC_FUNCTION_COSINE);
     CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000);
+    if (arg2_modified)
+    {
+        CORDIC->WDATA = (q31_t)((1.f) * 0x80000000);
+    }
     q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result
     q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result
     *c = (float)out_cos / (float)0x80000000;
     *s = (float)out_sin / (float)0x80000000;
+    if (arg2_modified)
+    {
+        LL_CORDIC_SetNbWrite(CORDIC, LL_CORDIC_NBWRITE_1);
+        arg2_modified = false;
+    }
 }
 
+void _atan2mod(float y, float x, float* phase, float* modulus)
+{
+    float absx = abs(x), absy = abs(y);
+    float xbigger = abs(x) >= abs(y);
+    float scaler = (xbigger ? absx : absy) * 1.5f; //multiply scaler by 1.5 to avoid saturating modulus
+    LL_CORDIC_SetFunction(CORDIC, LL_CORDIC_FUNCTION_PHASE);
+    if (!arg2_modified)
+    {
+        arg2_modified = true;
+        LL_CORDIC_SetNbWrite(CORDIC, LL_CORDIC_NBWRITE_2);
+    }
+    CORDIC->WDATA = (q31_t)((x / scaler) * 0x80000000);
+    CORDIC->WDATA = (q31_t)((y / scaler) * 0x80000000);
+    q31_t out_phase = (int32_t)CORDIC->RDATA; // read phase result
+    q31_t out_modulus = (int32_t)CORDIC->RDATA; // read modulus result
+    *phase = (float)out_phase * _PI / (float)0x80000000;
+    *modulus = (float)out_modulus / (float)0x80000000 * scaler;
+}
+
+float _atan2(float y, float x)
+{
+    float phase, modulus;
+    _atan2mod(y, x, &phase, &modulus);
+    return phase;
+}
 
 #endif