Foc refactoring

src/common/base_classes/CurrentSense.cpp

@@ -7,33 +7,12 @@
 float CurrentSense::getDCCurrent(float motor_electrical_angle){
     // read current phase currents
     PhaseCurrent_s current = getPhaseCurrents();
-    // currnet sign - if motor angle not provided the magnitude is always positive
-    float sign = 1;
-
+
     // calculate clarke transform
-    float i_alpha, i_beta;
-    if(!current.c){
-        // if only two measured currents
-        i_alpha = current.a;  
-        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
-    }else if(!current.a){
-        // if only two measured currents
-        float a = -current.c - current.b;
-        i_alpha = a;  
-        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
-    }else if(!current.b){
-        // if only two measured currents
-        float b = -current.a - current.c;
-        i_alpha = current.a;  
-        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
-    }else{
-        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
-        float mid = (1.f/3) * (current.a + current.b + current.c);
-        float a = current.a - mid;
-        float b = current.b - mid;
-        i_alpha = a;
-        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
-    }
+    ABCurrent_s ABcurrent = getABCurrents(current);
+
+    // current sign - if motor angle not provided the magnitude is always positive
+    float sign = 1;
 
     // if motor angle provided function returns signed value of the current
     // determine the sign of the current
@@ -42,20 +21,34 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
         float ct;
         float st;
         _sincos(motor_electrical_angle, &st, &ct);
-        sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1;  
+        sign = (ABcurrent.beta*ct - ABcurrent.alpha*st) > 0 ? 1 : -1;  
     }
     // return current magnitude
-    return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta);
+    return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta);
 }
 
 // function used with the foc algorihtm
 //   calculating DQ currents from phase currents
 //   - function calculating park and clarke transform of the phase currents 
-//   - using getPhaseCurrents internally
+//   - using getPhaseCurrents and getABCurrents internally
 DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
     // read current phase currents
     PhaseCurrent_s current = getPhaseCurrents();
 
+    // calculate clarke transform
+    ABCurrent_s ABcurrent = getABCurrents(current);
+
+    // calculate park transform
+    DQCurrent_s return_current = getDQCurrents(ABcurrent,angle_el);
+
+    return return_current;
+}
+
+// function used with the foc algorihtm
+//   calculating Alpha Beta currents from phase currents
+//   - function calculating Clarke transform of the phase currents
+ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){
+
     // calculate clarke transform
     float i_alpha, i_beta;
     if(!current.c){
@@ -81,13 +74,23 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
         i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
     }
 
-    // calculate park transform
+    ABCurrent_s return_ABcurrent;
+    return_ABcurrent.alpha = i_alpha;
+    return_ABcurrent.beta = i_beta;
+    return return_ABcurrent;
+}
+
+// function used with the foc algorihtm
+//   calculating D and Q currents from Alpha Beta currents and electrical angle
+//   - function calculating Clarke transform of the phase currents
+DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){
+ // calculate park transform
     float ct;
     float st;
     _sincos(angle_el, &st, &ct);
     DQCurrent_s return_current;
-    return_current.d = i_alpha * ct + i_beta * st;
-    return_current.q = i_beta * ct - i_alpha * st;
+    return_current.d = current.alpha * ct + current.beta * st;
+    return_current.q = current.beta * ct - current.alpha * st;
     return return_current;
 }
 
@@ -96,4 +99,4 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
 */
 void CurrentSense::linkDriver(BLDCDriver* _driver) {
   driver = _driver;
-}
+}

src/common/base_classes/CurrentSense.h

@@ -53,7 +53,7 @@ class CurrentSense{
     virtual PhaseCurrent_s getPhaseCurrents() = 0;
     /**
      * Function reading the magnitude of the current set to the motor
-     *  It returns the abosolute or signed magnitude if possible
+     *  It returns the absolute or signed magnitude if possible
      *  It can receive the motor electrical angle to help with calculation
      *  This function is used with the current control  (not foc)
      *  
@@ -62,13 +62,31 @@ class CurrentSense{
     virtual float getDCCurrent(float angle_el = 0);
 
     /**
-     * Function used for FOC contorl, it reads the DQ currents of the motor 
+     * Function used for FOC control, it reads the DQ currents of the motor 
      *   It uses the function getPhaseCurrents internally
      * 
      * @param angle_el - motor electrical angle
      */
     DQCurrent_s getFOCCurrents(float angle_el);
 
+    /**
+     * Function used for Clarke transform in FOC control
+     *   It reads the phase currents of the motor 
+     *   It returns the alpha and beta currents
+     * 
+     * @param current - phase current
+     */
+    ABCurrent_s getABCurrents(PhaseCurrent_s current);
+
+    /**
+     * Function used for Park transform in FOC control
+     *   It reads the Alpha Beta currents and electircal angle of the motor 
+     *   It returns the D and Q currents
+     * 
+     * @param current - phase current
+     */
+    DQCurrent_s getDQCurrents(ABCurrent_s current,float angle_el);
+
 
 };
 

src/common/foc_utils.h

@@ -56,6 +56,12 @@ struct DQVoltage_s
     float d;
     float q;
 };
+// alpha beta current structure
+struct ABCurrent_s
+{
+    float alpha;
+    float beta;
+};
 
 
 /**