Diff: FOC/foc.cpp

Revision:

59:8aa304768360

Parent:

57:57a108e15b52

--- a/FOC/foc.cpp	Sat Feb 27 22:57:54 2021 -0800
+++ b/FOC/foc.cpp	Sun Mar 28 01:10:30 2021 -0700
@@ -1,14 +1,12 @@
-
 #include "foc.h"
-using namespace FastMath;
-
+#include "arm_math.h"
 
 void abc( float theta, float d, float q, float *a, float *b, float *c){
     /// Inverse DQ0 Transform ///
     ///Phase current amplitude = lengh of dq vector///
     ///i.e. iq = 1, id = 0, peak phase current of 1///
-    float cf = FastCos(theta);
-    float sf = FastSin(theta);
+    float cf = arm_cos_f32(theta);
+    float sf = arm_sin_f32(theta);
     
     *a = cf*d - sf*q;                // Faster Inverse DQ0 transform
     *b = (0.86602540378f*sf-.5f*cf)*d - (-0.86602540378f*cf-.5f*sf)*q;
@@ -21,32 +19,44 @@
     ///Phase current amplitude = lengh of dq vector///
     ///i.e. iq = 1, id = 0, peak phase current of 1///
     
-    float cf = FastCos(theta);
-    float sf = FastSin(theta);
+    float cf = arm_cos_f32(theta);
+    float sf = arm_sin_f32(theta);
     
     *d = 0.6666667f*(cf*a + (0.86602540378f*sf-.5f*cf)*b + (-0.86602540378f*sf-.5f*cf)*c);   ///Faster DQ0 Transform
     *q = 0.6666667f*(-sf*a - (-0.86602540378f*cf-.5f*sf)*b - (0.86602540378f*cf-.5f*sf)*c);
        
     }
+
+void fast_dq0(float theta, float a, float b, float c, float *d, float *q) {
+    float32_t sinVal;
+    float32_t cosVal;
+    arm_sin_cos_f32(theta, &sinVal, &cosVal);
+    float32_t alpha;
+    float32_t beta;
+    arm_clarke_f32(a, b, &alpha, &beta);
+    arm_park_f32(alpha, beta, d, q, sinVal, cosVal);
+}
+
+void fast_abc(float theta, float d, float q, float *a, float *b, float *c) {
+    float32_t sinVal;
+    float32_t cosVal;
+    arm_sin_cos_f32(theta, &sinVal, &cosVal);
+    float32_t alpha;
+    float32_t beta;
+    arm_inv_park_f32(d, q, &alpha, &beta, sinVal, cosVal);
+    arm_inv_clarke_f32(alpha, beta, a, b);
+    *c = -*a - *b;
+}
     
 void svm(float v_bus, float u, float v, float w, float *dtc_u, float *dtc_v, float *dtc_w){
     /// Space Vector Modulation ///
-    /// u,v,w amplitude = v_bus for full modulation depth ///
+    // u,v,w amplitude = v_bus for full modulation depth
     
     float v_offset = (fminf3(u, v, w) + fmaxf3(u, v, w))*0.5f;
     
     *dtc_u = fminf(fmaxf(((u -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
     *dtc_v = fminf(fmaxf(((v -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);
     *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX); 
-    
-    /*
-    sinusoidal pwm
-    *dtc_u = fminf(fmaxf((u/v_bus + .5f), DTC_MIN), DTC_MAX);
-    *dtc_v = fminf(fmaxf((v/v_bus + .5f), DTC_MIN), DTC_MAX);
-    *dtc_w = fminf(fmaxf((w/v_bus + .5f), DTC_MIN), DTC_MAX);
-    */
-     
-    
     }
 
 void linearize_dtc(float *dtc){
@@ -147,8 +157,8 @@
            }
        controller->i_a = -controller->i_b - controller->i_c;       
        
-       float s = FastSin(theta); 
-       float c = FastCos(theta);                            
+        float c = arm_cos_f32(theta);
+        float s = arm_sin_f32(theta);                           
        dq0(controller->theta_elec, controller->i_a, controller->i_b, controller->i_c, &controller->i_d, &controller->i_q);    //dq0 transform on currents
        //controller->i_d = 0.6666667f*(c*controller->i_a + (0.86602540378f*s-.5f*c)*controller->i_b + (-0.86602540378f*s-.5f*c)*controller->i_c);   ///Faster DQ0 Transform
        //controller->i_q = 0.6666667f*(-s*controller->i_a - (-0.86602540378f*c-.5f*s)*controller->i_b - (0.86602540378f*c-.5f*s)*controller->i_c);