From 82552060b21ad6c2273ab544a9d5092a345e246c Mon Sep 17 00:00:00 2001
From: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com>
Date: Fri, 22 Mar 2024 04:25:09 +0000
Subject: [PATCH] Added svm function for generating saddle duties

---
 include/math/foc.hpp |   2 +
 src/math/foc.cpp     | 102 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 104 insertions(+)

diff --git a/include/math/foc.hpp b/include/math/foc.hpp
index b750793..c97611c 100644
--- a/include/math/foc.hpp
+++ b/include/math/foc.hpp
@@ -37,4 +37,6 @@ FrameABC inverse_clarke_transform(FrameAlphaBeta X);
 
 FrameAlphaBeta inverse_park_transform(FrameDQ X, float angle);
 
+FrameABC space_vector_modulation(FrameAlphaBeta U);
+
 }  // namespace math
diff --git a/src/math/foc.cpp b/src/math/foc.cpp
index efed937..9287ce5 100644
--- a/src/math/foc.cpp
+++ b/src/math/foc.cpp
@@ -35,3 +35,105 @@ math::FrameAlphaBeta math::inverse_park_transform(math::FrameDQ X, float angle)
   Y.beta = X.d * sine + X.q * cosine;
   return Y;
 }
+
+math::FrameABC math::space_vector_modulation(math::FrameAlphaBeta U)
+{
+  math::FrameABC duty;
+
+  float Z = U.beta * 0.5F - U.alpha * 0.8660254037844386F;
+  float Y = U.alpha * 0.8660254037844386F + U.beta * 0.5F;
+  float X = U.beta;
+
+  // Sector 1
+  if ((-Y <= 0.F) && (-Z > 0.F) && (X > 0.F)) {
+    float T1 = (Z - X + 1.F) / 2;
+    float T2 = T1 + X;
+    float T3 = T2 - Z;
+    
+    duty.a = T3;
+    duty.b = T2;
+    duty.c = T1;
+  }
+  // Sector 2
+  else if ((-Y <= 0.F) && (-Z <= 0.F)) {
+    float T1 = (1.F - Y - Z) / 2;
+    float T2 = T1 + Y;
+    float T3 = T2 + Z;
+    
+    duty.a = T2;
+    duty.b = T3;
+    duty.c = T1;
+  }
+  // Sector 3
+  else if ((-Y > 0.F) && (-Z <= 0.F) && (X > 0.F)) {
+    float T1 = (Y - X + 1.F) / 2;
+    float T2 = T1 - Y;
+    float T3 = T2 + X;
+        
+    duty.a = T1;
+    duty.b = T3;
+    duty.c = T2;
+  }
+  // Sector 4
+  else if ((-Y > 0.F) && (-Z <= 0.F) && (X <= 0.F)) {
+    float T1 = (X - Z + 1.F) / 2;
+    float T2 = T1 + Z;
+    float T3 = T2 - X;
+
+    duty.a = T1;
+    duty.b = T2;
+    duty.c = T3;
+  }
+  // Sector 5
+  else if ((-Y > 0.F) && (-Z > 0.F)) {
+    float T1 = (Z + Y + 1.F) / 2;
+    float T2 = T1 - Z;
+    float T3 = T2 - Y;
+    
+    duty.a = T2;
+    duty.b = T1;
+    duty.c = T3;
+  }
+  // Sector 6
+  else if ((-Y <= 0.F) && (-Z > 0.F) && (X <= 0.F)) {
+    float T1 = (X - Y + 1.F) / 2;
+    float T2 = T1 - X;
+    float T3 = T2 + Y;
+
+    duty.a = T3;
+    duty.b = T1;
+    duty.c = T2;
+  }
+  // Default
+  else {
+    duty.a = 0.F;
+    duty.b = 0.F;
+    duty.c = 0.F;
+  }
+
+  // Saturation of Duty A
+  if (duty.a > 1.F)
+    duty.a = 1.F;
+  else if (duty.a < 0.F)
+    duty.a = 0.F;
+  else
+    duty.a = duty.a;
+
+  // Saturation of Duty B
+  if (duty.b > 1.F)
+    duty.b = 1.F;
+  else if (duty.b < 0.F)
+    duty.b = 0.F;
+  else
+    duty.b = duty.b;
+
+  // Saturation of Duty C
+  if (duty.c > 1.F)
+    duty.c = 1.F;
+  else if (duty.c < 0.F)
+    duty.c = 0.F;
+  else
+    duty.c = duty.c;
+  
+  return duty;
+}
\ No newline at end of file