Completed CORDIC functions for fast sine and cosine computations

CMakeLists.txt

@@ -17,6 +17,7 @@ add_library(${PROJECT_NAME}
     src/math/functions.cpp
     src/math/foc.cpp
     src/math/operators.cpp
+    src/math/cordic.cpp
     src/observers/tracker.cpp
     src/observers/dq_update.cpp
     src/observers/bemf_observer.cpp
@@ -31,6 +32,10 @@ add_executable(bemf_observer_test tests/bemf_observer_test.cpp)
 target_link_libraries(bemf_observer_test ${PROJECT_NAME})
 add_test(NAME bemf_observer_test COMMAND "${CMAKE_SOURCE_DIR}/bin/bemf_observer_test")
 
+add_executable(cordic_algo_test tests/cordic_algo_test.cpp)
+target_link_libraries(cordic_algo_test ${PROJECT_NAME})
+add_test(NAME cordic_algo_test COMMAND "${CMAKE_SOURCE_DIR}/bin/cordic_algo_test" 45)
+
 # Include Header files
 include_directories(
     include

include/math/cordic.hpp

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <cstdint>
+
+namespace CORDIC
+{
+  struct Trig
+  {
+    int sin;
+    int cos;
+  };
+  Trig trig(int angle);
+}

src/math/cordic.cpp

@@ -0,0 +1,16 @@
+#include<math/cordic.hpp>
+
+CORDIC::Trig CORDIC::trig(int angle)
+{
+  int32_t atan2[14] = {8192, 4836, 2555, 1297, 651, 325, 162, 81, 40, 20, 10, 5, 2, 1};
+  int32_t x = 9949, y = 0, theta = 0;
+  for(int32_t i = 0; i < 14; i++)
+  {
+    int32_t sigma = (theta < angle) ? 1 : -1;
+    theta = theta + sigma * atan2[i];
+    int32_t nx = x - sigma * (y >> i);
+    y = y + sigma * (x >> i);
+    x = nx;
+  }
+  return {y, x};
+}

tests/cordic_algo_test.cpp

@@ -0,0 +1,22 @@
+#include <iostream>
+#include <cmath>
+#include <math/functions.hpp>
+#include <math/cordic.hpp>
+
+int main(int argc, char **argv)
+{
+  if(argc != 2)
+  {
+    std::cerr << "Usage: " << argv[0] << " <angle>" << std::endl;
+    return 1;
+  }
+  int angle = atoi(argv[1]);
+  int angle_cordic = (angle << 16) / 360;
+  CORDIC::Trig trig = CORDIC::trig(angle_cordic);
+  std::cout << "sin(x) in cru = " << trig.sin << " " << "cos(x) in cru = " << trig.cos << std::endl;
+  float sine = (float)trig.sin/16384.0F;
+  float cosine = (float)trig.cos/16384.0F;
+  std::cout << "std::sin(x) error = " << std::sin(angle*PI/180) - sine << " " << "std::cos(x) error = " << std::cos(angle*PI/180) - cosine << std::endl;
+  std::cout << "math::sin(x) error = " << math::sin(angle*PI/180) - sine << " " << "math::cos(x) error = " << math::cos(angle*PI/180) - cosine << std::endl;
+  return 0;
+}