Skip to content

Commit

Permalink
new current sense alignement
Browse files Browse the repository at this point in the history
  • Loading branch information
@askuric
askuric committed Jul 14, 2024
1 parent 7d108c4 commit 842b361
Showing 1 changed file with 187 additions and 96 deletions.
283 changes: 187 additions & 96 deletions src/common/base_classes/CurrentSense.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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
Expand All @@ -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;
}
}
Expand All @@ -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;
}
}
Expand Down

0 comments on commit 842b361

Please sign in to comment.