From 87279080e2b2d5aa25149e90df65775244eb5204 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 24 Sep 2023 17:59:32 +0200 Subject: [PATCH 1/3] refactoring of FOC --- src/common/base_classes/CurrentSense.cpp | 67 +++++++++++------------- src/common/base_classes/CurrentSense.h | 13 ++++- src/common/foc_utils.h | 6 +++ 3 files changed, 47 insertions(+), 39 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 609162c2..bbe5edc7 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/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; - }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; - }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,38 @@ 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 + float ct; + float st; + _sincos(angle_el, &st, &ct); + DQCurrent_s return_current; + return_current.d = ABcurrent.alpha * ct + ABcurrent.beta * st; + return_current.q = ABcurrent.beta * ct - ABcurrent.alpha * st; + 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,14 +78,10 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ i_beta = _1_SQRT3 * a + _2_SQRT3 * b; } - // 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 return_current; + ABCurrent_s return_ABcurrent; + return_ABcurrent.alpha = i_alpha; + return_ABcurrent.beta = i_beta; + return return_ABcurrent; } /** diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index ad9f926d..5292988b 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/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,22 @@ 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); + }; diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 0efe3b59..ac1ae458 100644 --- a/src/common/foc_utils.h +++ b/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; +}; /** From 7374f7a4f3297f51a2d67123cefd10d9bf7a2b39 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 24 Sep 2023 23:35:48 +0200 Subject: [PATCH 2/3] Add getDQCurrents function for Park transform --- src/common/base_classes/CurrentSense.cpp | 22 ++++++++++++++++------ src/common/base_classes/CurrentSense.h | 9 +++++++++ 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index bbe5edc7..c9394ad0 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -39,12 +39,8 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){ ABCurrent_s ABcurrent = getABCurrents(current); // calculate park transform - float ct; - float st; - _sincos(angle_el, &st, &ct); - DQCurrent_s return_current; - return_current.d = ABcurrent.alpha * ct + ABcurrent.beta * st; - return_current.q = ABcurrent.beta * ct - ABcurrent.alpha * st; + DQCurrent_s return_current = getDQCurrents(ABcurrent,angle_el); + return return_current; } @@ -84,6 +80,20 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ 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 = current.alpha * ct + current.beta * st; + return_current.q = current.beta * ct - current.alpha * st; + return return_current; +} + /** Driver linking to the current sense */ diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index 5292988b..d9cd8c56 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -78,6 +78,15 @@ class CurrentSense{ */ 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); + }; From 72cdd21f02d77a02e8d9e3198fc5a0ee41291f75 Mon Sep 17 00:00:00 2001 From: Candas1 Date: Sun, 19 Nov 2023 19:03:42 +0100 Subject: [PATCH 3/3] Update CurrentSense.cpp Fix Clarke transform for dual shunt --- src/common/base_classes/CurrentSense.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index c9394ad0..c57bdfe6 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -55,12 +55,12 @@ ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){ // if only two measured currents i_alpha = current.a; i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b; - }if(!current.a){ + }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; - }if(!current.b){ + }else if(!current.b){ // if only two measured currents float b = -current.a - current.c; i_alpha = current.a; @@ -99,4 +99,4 @@ DQCurrent_s CurrentSense::getDQCurrents(ABCurrent_s current, float angle_el){ */ void CurrentSense::linkDriver(BLDCDriver* _driver) { driver = _driver; -} \ No newline at end of file +}