From 842b3615eec1b74cdb4729c03a2c64d21d7135d8 Mon Sep 17 00:00:00 2001 From: askuric Date: Sun, 14 Jul 2024 13:15:36 +0200 Subject: [PATCH] new current sense alignement --- src/common/base_classes/CurrentSense.cpp | 283 +++++++++++++++-------- 1 file changed, 187 insertions(+), 96 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 4e6c3386..e354dfaf 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -174,113 +174,204 @@ PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { // 4 - success but pins reconfigured and gains inverted int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ - int exit_flag = 1; - if(_isset(pinA)){ - // set phase A active and phases B and C down - bldc_driver->setPwm(voltage, 0, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - // align phase A - float ab_ratio = c.b ? fabs(c.a / c.b) : 0; - float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2 - gain_a *= _sign(c.a); - }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch A-B"); - // switch phase A and B + bool phases_switched = 0; + bool phases_inverted = 0; + + + // set phase A active and phases B and C down + bldc_driver->setPwm(voltage, 0, 0); + _delay(500); + PhaseCurrent_s c_a = readAverageCurrents(); + // check if currents are to low (lower than 100mA) + // TODO calculate the 100mA threshold from the ADC resolution + // if yes throw an error and return 0 + // either the current sense is not connected or the current is + // too low for calibration purposes (one should raise the motor.voltage_sensor_align) + if((_isset(pinA) && fabs(c_a.a) < 0.1f) + || (_isset(pinB) && fabs(c_a.b) < 0.1f) + || (_isset(pinC) && fabs(c_a.c) < 0.1f)){ + SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!"); + return 0; // measurement current too low + } + + // set phase B active and phases A and C down + bldc_driver->setPwm(0, voltage, 0); + _delay(500); + PhaseCurrent_s c_b = readAverageCurrents(); + bldc_driver->setPwm(0, 0, 0); + + // now we have to determine + // 1) which pin correspond to which phase of the bldc driver + // 2) if the currents measured have good polarity + // + // > when we apply a voltage to a phase A of the driver what we expect to measure is the current I on the phase A + // and -I/2 on the phase B and I/2 on the phase C + + // find the highest magnitude in c_a + // and make sure it's around 2 (1.5 at least) times higher than the other two + float ca[3] = {fabs(c_a.a), fabs(c_a.b), fabs(c_a.c)}; + uint8_t max_i = -1; // max index + float max_c = 0; // max current + float max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!ca[i]) continue; // current not measured + if(ca[i] > max_c){ + max_c = ca[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!ca[j]) continue; // current not measured + float ratio = max_c / ca[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + + // check the current magnitude ratios + // 1) if there is one current that is approximately 2 times higher than the other two + // this is the A current + // 2) if the max current is not at least 1.5 times higher than the other two + // we have two cases: + // - either we only measure two currents and the third one is not measured - then phase A is not measured + // - or the current sense is not connected properly + + if(max_c_ratio >=1.5f){ + switch (max_i){ + case 1: // phase B is the max current + SIMPLEFOC_DEBUG("CS: Switch A-B"); + // switch phase A and B + _swap(pinA, pinB); + _swap(offset_ia, offset_ib); + _swap(gain_a, gain_b); + _swap(c_a.b, c_a.b); + _swap(c_b.a, c_b.b); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched + break; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch A-C"); + // switch phase A and C + _swap(pinA, pinC); + _swap(offset_ia, offset_ic); + _swap(gain_a, gain_c); + _swap(c_a.a, c_a.c); + _swap(c_b.a, c_b.c); // for the next phase of alignment + phases_switched = true;// signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_a.a) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv A"); + gain_a *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinA) && _isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err A - all currents same magnitude!"); + return 0; + }else{ //phase A is not measured so put the _NC to the phase A + if(_isset(pinA) && !_isset(pinB)){ + SIMPLEFOC_DEBUG("CS: Switch A-(B)NC"); _swap(pinA, pinB); _swap(offset_ia, offset_ib); _swap(gain_a, gain_b); - gain_a *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch A-C"); - // switch phase A and C + _swap(c_a.b, c_a.b); + _swap(c_b.a, c_b.b); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched + }else if(_isset(pinA) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch A-(C)NC"); _swap(pinA, pinC); _swap(offset_ia, offset_ic); _swap(gain_a, gain_c); - gain_a *= _sign(c.c); - exit_flag = 2;// signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Err read A"); - // error in current sense - phase either not measured or bad connection - return 0; + _swap(c_a.b, c_a.c); + _swap(c_b.a, c_b.c); // for the next phase of alignment + phases_switched = true; // signal that pins have been switched } } - - if(_isset(pinB)){ - // set phase B active and phases A and C down - bldc_driver->setPwm(0, voltage, 0); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - float ba_ratio = c.a ? fabs(c.b / c.a) : 0; - float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2); - gain_b *= _sign(c.b); - }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2 - gain_b *= _sign(c.b); - }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch B-A"); - // switch phase A and B - _swap(pinB, pinA); - _swap(offset_ib, offset_ia); - _swap(gain_b, gain_a); - gain_b *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch B-C"); + // at this point the current sensing on phase A can be either: + // - aligned with the driver phase A + // - or the phase A is not measured and the _NC is connected to the phase A + // + // In either case A is done, now we have to check the phase B and C + + // check the phase B + // find the highest magnitude in c_b + // and make sure it's around 2 (1.5 at least) times higher than the other two + float cb[3] = {fabs(c_b.a), fabs(c_b.b), fabs(c_b.c)}; + max_i = -1; // max index + max_c = 0; // max current + max_c_ratio = 0; // max current ratio + for(int i = 0; i < 3; i++){ + if(!cb[i]) continue; // current not measured + if(cb[i] > max_c){ + max_c = cb[i]; + max_i = i; + for(int j = 0; j < 3; j++){ + if(i == j) continue; + if(!cb[j]) continue; // current not measured + float ratio = max_c / cb[j]; + if(ratio > max_c_ratio) max_c_ratio = ratio; + } + } + } + if(max_c_ratio >= 1.5f){ + switch (max_i){ + case 0: // phase A is the max current + // this is an error as phase A is already aligned + SIMPLEFOC_DEBUG("CS: Err align B"); + return 0; + case 2: // phase C is the max current + SIMPLEFOC_DEBUG("CS: Switch B-C"); + _swap(pinB, pinC); + _swap(offset_ib, offset_ic); + _swap(gain_b, gain_c); + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + break; + } + // check if the current is negative and invert the gain if so + if( _sign(c_b.b) < 0 ){ + SIMPLEFOC_DEBUG("CS: Inv B"); + gain_b *= -1; + phases_inverted = true; // signal that pins have been inverted + } + }else if(_isset(pinB) && _isset(pinC)){ + // if all three currents are measured and none of them is significantly higher + // we have a problem with the current sense + SIMPLEFOC_DEBUG("CS: Err B - all currents same magnitude!"); + return 0; + }else{ //phase B is not measured so put the _NC to the phase B + if(_isset(pinB) && !_isset(pinC)){ + SIMPLEFOC_DEBUG("CS: Switch B-(C)NC"); _swap(pinB, pinC); _swap(offset_ib, offset_ic); _swap(gain_b, gain_c); - gain_b *= _sign(c.c); - exit_flag = 2; // signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Error read B"); - // error in current sense - phase either not measured or bad connection - return 0; - } + _swap(c_b.b, c_b.c); + phases_switched = true; // signal that pins have been switched + } } - - // if phase C measured + // at this point the current sensing on phase A and B can be either: + // - aligned with the driver phase A and B + // - or the phase A and B are not measured and the _NC is connected to the phase A and B + // + // In either case A and B is done, now we have to check the phase C + // phase C is also aligned if it is measured (not _NC) + // we have to check if the current is negative and invert the gain if so if(_isset(pinC)){ - // set phase C active and phases A and B down - bldc_driver->setPwm(0, 0, voltage); - _delay(500); - PhaseCurrent_s c = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); - float ca_ratio = c.a ? fabs(c.c / c.a) : 0; - float cb_ratio = c.b ? fabs(c.c / c.b) : 0; - if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2 - gain_c *= _sign(c.c); - }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch C-A"); - // switch phase A and C - _swap(pinC, pinA); - _swap(offset_ic, offset_ia); - _swap(gain_c, gain_a); - gain_c *= _sign(c.a); - exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5 - SIMPLEFOC_DEBUG("CS: Switch C-B"); - _swap(pinC, pinB); - _swap(offset_ic, offset_ib); - _swap(gain_c, gain_b); - gain_b *= _sign(c.b); - exit_flag = 2; // signal that pins have been switched - }else{ - SIMPLEFOC_DEBUG("CS: Err read C"); - // error in current sense - phase either not measured or bad connection - return 0; - } + if( _sign(c_b.c) > 0 ){ // the expected current is -I/2 (if the phase A and B are aligned and C has correct polarity) + SIMPLEFOC_DEBUG("CS: Inv C"); + gain_c *= -1; + phases_inverted = true; // signal that pins have been inverted + } } - // add 2 if pin gains negative - if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2; + + // construct the return flag + // if the phases have been switched return 2 + // if the gains have been inverted return 3 + // if both return 4 + uint8_t exit_flag = 1; + if(phases_switched) exit_flag += 1; + if(phases_inverted) exit_flag += 2; return exit_flag; } @@ -303,7 +394,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive _delay(500); PhaseCurrent_s c = readAverageCurrents(); // disable the phases - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(0, 0); if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ SIMPLEFOC_DEBUG("CS: Err too low current!"); return 0; // measurement current too low @@ -321,7 +412,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched }else if (c.a < 0){ - SIMPLEFOC_DEBUG("CS: Neg A"); + SIMPLEFOC_DEBUG("CS: Inv A"); gain_a *= -1; } } @@ -339,7 +430,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // align phase A // check if measured current a is positive and invert if not if (c.b < 0){ - SIMPLEFOC_DEBUG("CS: Neg B"); + SIMPLEFOC_DEBUG("CS: Inv B"); gain_b *= -1; } }