CurrentSense.cpp

Go to the documentation of this file.

#include "CurrentSense.h"
#include "../../communication/SimpleFOCDebug.h"
 
 
// get current magnitude 
//   - absolute  - if no electrical_angle provided 
//   - signed    - if angle provided
float CurrentSense::getDCCurrent(float motor_electrical_angle){
    // read current phase currents
    PhaseCurrent_s current = getPhaseCurrents();
    
    // calculate clarke transform
    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
    // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1  
    if(motor_electrical_angle) {
        float ct;
        float st;
        _sincos(motor_electrical_angle, &st, &ct);
        sign = (ABcurrent.beta*ct - ABcurrent.alpha*st) > 0 ? 1 : -1;  
    }
    // return current magnitude
    return sign*_sqrt(ABcurrent.alpha*ABcurrent.alpha + ABcurrent.beta*ABcurrent.beta);
}
 
// function used with the foc algorithm
//   calculating DQ currents from phase currents
//   - function calculating park and clarke transform of the phase currents 
//   - 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 algorithm
//   calculating Alpha Beta currents from phase currents
//   - function calculating Clarke transform of the phase currents
ABCurrent_s CurrentSense::getABCurrents(PhaseCurrent_s current){
 
    // check if driver is an instance of StepperDriver
    // if so there is no need to Clarke transform
    if (driver_type == DriverType::Stepper){
        ABCurrent_s return_ABcurrent;
        return_ABcurrent.alpha = current.a;
        return_ABcurrent.beta = current.b;
        return return_ABcurrent;
    }
 
    if (driver_type == DriverType::Hybrid){
        ABCurrent_s return_ABcurrent;
        //ia + ib + ic = 0
        if(current.a == 0){
            return_ABcurrent.alpha = -current.c - current.b;
            return_ABcurrent.beta = current.b;
        }else if(current.b == 0){
            return_ABcurrent.alpha = current.a;
            return_ABcurrent.beta = -current.c - current.a;
        }else{
            return_ABcurrent.alpha = current.a;
            return_ABcurrent.beta = current.b;
        }
        return return_ABcurrent;
    }
 
    // otherwise it's a BLDC motor and 
    // 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 return_ABcurrent;
    return_ABcurrent.alpha = i_alpha;
    return_ABcurrent.beta = i_beta;
    return return_ABcurrent;
}
 
// function used with the foc algorithm
//   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
*/
void CurrentSense::linkDriver(FOCDriver* _driver) {
    driver = _driver;
    // save the driver type for easier access
    driver_type = driver->type();
}
 
 
void CurrentSense::enable(){
    // nothing is done here, but you can override this function
};
 
void CurrentSense::disable(){
    // nothing is done here, but you can override this function
};
 
 
// Function aligning the current sense with motor driver
// if all pins are connected well none of this is really necessary! - can be avoided
// returns flag
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
// IMPORTANT, this function can be overriden in the child class
int CurrentSense::driverAlign(float voltage, bool modulation_centered){
        
    int exit_flag = 1;
    if(skip_align) return exit_flag;
 
    if (!initialized) return 0;
 
    // check if stepper or BLDC 
    switch(driver_type){
        case DriverType::BLDC:
            return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered);
        case DriverType::Stepper:
            return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered);
        case DriverType::Hybrid:
            return alignHybridDriver(voltage, (BLDCDriver*)driver, modulation_centered);
        default:
            // driver type not supported
            SIMPLEFOC_DEBUG("CS: Cannot align driver type!");
            return 0; 
    }
}
 
 
 
// Helper function to read and average phase currents
PhaseCurrent_s CurrentSense::readAverageCurrents(int N) {
    PhaseCurrent_s c = getPhaseCurrents();
    for (int i = 0; i < N; i++) {
        PhaseCurrent_s c1 = getPhaseCurrents();
        c.a = c.a * 0.6f + 0.4f * c1.a;
        c.b = c.b * 0.6f + 0.4f * c1.b;
        c.c = c.c * 0.6f + 0.4f * c1.c;
        _delay(3);
    }
    return c;
};
 
 
 
// Function aligning the current sense with motor driver
// if all pins are connected well none of this is really necessary! - can be avoided
// returns flag
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){
        
    bool phases_switched = 0;
    bool phases_inverted = 0;
    
    float zero = 0;
    if(modulation_centered) zero = driver->voltage_limit/2.0;
 
    // set phase A active and phases B and C down
    // 300 ms of ramping
    for(int i=0; i < 100; i++){
        bldc_driver->setPwm(voltage/100.0*((float)i)+zero , zero, zero);
        _delay(3);
    }
    _delay(500);
    PhaseCurrent_s c_a = readAverageCurrents();
    bldc_driver->setPwm(zero, zero, zero);
    // 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((fabs(c_a.a) < 0.1f) && (fabs(c_a.b) < 0.1f) && (fabs(c_a.c) < 0.1f)){
            SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!");
            return 0; // measurement current too low
    }
 
    
    // 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);
                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);
                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);
            _swap(c_a.b, c_a.b);
            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);
            _swap(c_a.b, c_a.c);
            phases_switched = true; // signal that pins have been switched
        }
    }
    // 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 
 
    
    // set phase B active and phases A and C down
    // 300 ms of ramping
    for(int i=0; i < 100; i++){
        bldc_driver->setPwm(zero, voltage/100.0*((float)i)+zero, zero);
        _delay(3);
    }
    _delay(500);
    PhaseCurrent_s c_b = readAverageCurrents();
    bldc_driver->setPwm(zero, zero, zero);
 
    // 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);
            _swap(c_b.b, c_b.c);
            phases_switched = true; // signal that pins have been switched
        }
    }
    // 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)){
        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
        }
    }
 
    // 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;
}
 
 
// Function aligning the current sense with motor driver
// if all pins are connected well none of this is really necessary! - can be avoided
// returns flag
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){
    
    _UNUSED(modulation_centered);
 
    bool phases_switched = 0;
    bool phases_inverted = 0;
 
    if(!_isset(pinA) || !_isset(pinB)){
        SIMPLEFOC_DEBUG("CS: Pins A & B not specified!");
        return 0;
    }
 
    // set phase A active and phases B down
    // ramp 300ms
    for(int i=0; i < 100; i++){
        stepper_driver->setPwm(voltage/100.0*((float)i), 0);
        _delay(3);
    }
    _delay(500);
    PhaseCurrent_s c = readAverageCurrents();
    // disable the phases
    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
    }
    // align phase A
    // 1) only one phase can be measured so we first measure which ADC pin corresponds 
    // to the phase A by comparing the magnitude
    if (fabs(c.a) < fabs(c.b)){
        SIMPLEFOC_DEBUG("CS: Switch A-B");
        // switch phase A and B
        _swap(pinA, pinB);
        _swap(offset_ia, offset_ib);
        _swap(gain_a, gain_b);
        phases_switched = true; // signal that pins have been switched
    }
    // 2) check if measured current a is positive and invert if not
    if (c.a < 0){
        SIMPLEFOC_DEBUG("CS: Inv A");
        gain_a *= -1;
        phases_inverted = true; // signal that pins have been inverted
    }
 
    // at this point the driver's phase A is aligned with the ADC pinA
    // and the pin B should be the phase B
 
    // set phase B active and phases A down
    // ramp 300ms
    for(int i=0; i < 100; i++){
        stepper_driver->setPwm(0, voltage/100.0*((float)i));
        _delay(3);
    }
    _delay(500);
    c = readAverageCurrents();
    stepper_driver->setPwm(0, 0);
 
    // phase B should be aligned
    // 1) we just need to verify that it has been measured
    if (fabs(c.b) < 0.1f ){
        SIMPLEFOC_DEBUG("CS: Err too low current on B!");
        return 0; // measurement current too low
    }
    // 2) check if measured current a is positive and invert if not
    if (c.b < 0){
        SIMPLEFOC_DEBUG("CS: Inv B");
        gain_b *= -1;
        phases_inverted = true; // signal that pins have been inverted
    }
 
    // construct the return flag
    // if success and nothing changed return 1 
    // 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;
}
 
 
 
 
 
int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){
 
    _UNUSED(modulation_centered);
 
    bool phases_switched = 0;
    bool phases_inverted = 0;
 
    // first find the middle phase, which will be set to the C phase
    // set phase A active and phases B active, and C down
    // ramp 300ms
    for(int i=0; i < 100; i++){
        bldc_driver->setPwm(voltage/100.0*((float)i), voltage/100.0*((float)i), 0);
        _delay(3);
    }
    _delay(500);
    PhaseCurrent_s c = readAverageCurrents();
    // disable the phases
    bldc_driver->setPwm(0, 0, 0);
    if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f && fabs(c.c) < 0.1f ){
        SIMPLEFOC_DEBUG("CS: Err too low current!");
        return 0; // measurement current too low
    }
    // find the highest magnitude in c
    // and make sure it's around 2 (1.5 at least) times higher than the other two
    float cc[3] = {fabs(c.a), fabs(c.b), fabs(c.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(!cc[i]) continue; // current not measured
        if(cc[i] > max_c){
            max_c = cc[i];
            max_i = i;
            for(int j = 0; j < 3; j++){
                if(i == j) continue;
                if(!cc[j]) continue; // current not measured
                float ratio = max_c / cc[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
                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, c.c);
                phases_switched = true; // signal that pins have been switched
                break;
            case 1: // phase B is the max current
                SIMPLEFOC_DEBUG("CS: Switch B-C");
                // switch phase B and C
                _swap(pinB, pinC);
                _swap(offset_ib, offset_ic);
                _swap(gain_b, gain_c);
                _swap(c.b, c.c);
                phases_switched = true; // signal that pins have been switched
                break;
        }
        // check if the current is positive and invert the gain if so
        // current c should be negative
        if( _sign(c.c) > 0 ){
            SIMPLEFOC_DEBUG("CS: Inv C");
            gain_c *= -1;
            phases_inverted = true; // signal that pins have been inverted
        }
    }else{
        // c - middle phase is not measured
        SIMPLEFOC_DEBUG("CS: Middle phase not measured!");
        if(_isset(pinC)){
            // switch the missing phase with the phase C
            if(!_isset(pinA)){
                SIMPLEFOC_DEBUG("CS: Switch (A)NC-C");
                _swap(pinA, pinC);
                _swap(offset_ia, offset_ic);
                _swap(gain_a, gain_c);
                _swap(c.a, c.c);
                phases_switched = true; // signal that pins have been switched
            }else if(!_isset(pinB)){
                SIMPLEFOC_DEBUG("CS: Switch (B)NC-C");
                _swap(pinB, pinC);
                _swap(offset_ib, offset_ic);
                _swap(gain_b, gain_c);
                _swap(c.b, c.c);
                phases_switched = true; // signal that pins have been switched
            }
        }
    }
 
    // 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
 
    // Find the phase A
 
    // set phase A active and phases B down
    // ramp 300ms
    for(int i=0; i < 100; i++){
        bldc_driver->setPwm(voltage/100.0*((float)i), 0, 0);
        _delay(3);
    }
    _delay(500);
    c = readAverageCurrents();
    // disable the phases
    bldc_driver->setPwm(0, 0, 0);
 
    // check if currents are to low (lower than 100mA)
    if((fabs(c.a) < 0.1f) && (fabs(c.b) < 0.1f) && (fabs(c.c) < 0.1f)){
        SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!");
        return 0; // measurement current too low
    }
 
    // 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 on the phase C 
 
    // find the highest magnitude in A
    // and make sure it's around the same as the C current (if the phase C is measured)
 
    float ca[3] = {fabs(c.a), fabs(c.b), fabs(c.c)};
    if(c.a && c.c){
        // if a and mid-phase c measured
        // verify that they have almost the same magnitude
        if((fabs(c.a) - fabs(c.c)) > 0.1f){
            SIMPLEFOC_DEBUG("CS: Err A-C currents not equal!");
            return 0;
        }
    }else if(c.b && c.c){
        // if a and mid-phase c measured
        // verify that they have almost the same magnitude
        if((fabs(c.a) - fabs(c.c)) > 0.1f){
            SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!");
            return 0;
        }else{
            // if the current are equal 
            // switch phase A and B 
            SIMPLEFOC_DEBUG("CS: Switch A-B");
            _swap(pinA, pinB);
            _swap(offset_ia, offset_ib);
            _swap(gain_a, gain_b);
            _swap(c.a, c.b);
            phases_switched = true; // signal that pins have been switched
        }
    }else if(c.a && c.b){
        // check if one is significantly higher than the other
        if(fabs(fabs(c.a) - fabs(c.b)) < 0.1f){
            SIMPLEFOC_DEBUG("CS: Err A-B currents zero!");
            return 0;
        }else{
            // if they are not equal take the highest as A
            if (fabs(c.a) < fabs(c.b)){
                // switch phase A and B
                SIMPLEFOC_DEBUG("CS: Switch A-B");
                _swap(pinA, pinB);
                _swap(offset_ia, offset_ib);
                _swap(gain_a, gain_b);
                _swap(c.a, c.b);
                phases_switched = true; // signal that pins have been switched
 
            }
        }
    }
    // if we get here, phase A is aligned
    // check if the current is negative and invert the gain if so
    if( _sign(c.a) < 0 ){
        SIMPLEFOC_DEBUG("CS: Inv A");
        gain_a *= -1;
        phases_inverted = true; // signal that pins have been inverted
    }
    
    // at this point the driver's phase A is aligned with the ADC pinA
    // and the pin B should be the phase B
 
    // set phase B active and phases A down
    // ramp 300ms
    for(int i=0; i < 100; i++){
        bldc_driver->setPwm(0, voltage/100.0*((float)i), 0);
        _delay(3);
    }
    _delay(500);
    c = readAverageCurrents();
    bldc_driver->setPwm(0, 0, 0);
 
    // check if currents are to low (lower than 100mA)
    if((fabs(c.a) < 0.1f) && (fabs(c.b) < 0.1f) && (fabs(c.c) < 0.1f)){
        SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!");
        return 0; // measurement current too low
    }
 
    // check the phase B
    // find the highest magnitude in c
    // and make sure it's around the same as the C current (if the phase C is measured)
    float cb[3] = {fabs(c.a), fabs(c.b), fabs(c.c)};
    if(c.b && c.c){
        // if b and mid-phase c measured
        // verify that they have almost the same magnitude
        if((fabs(c.b) - fabs(c.c)) > 0.1f){
            SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!");
            return 0;
        }
    }else if(c.a && c.b){
        // check if one is significantly higher than the other
        if(fabs(fabs(c.a) - fabs(c.b)) < 0.1f){
            SIMPLEFOC_DEBUG("CS: Err A-B currents zero!");
            return 0;
        }
    }
 
    // check if b has good polarity
    if( _sign(c.b) < 0 ){
        SIMPLEFOC_DEBUG("CS: Inv B");
        gain_b *= -1;
        phases_inverted = true; // signal that pins have been inverted
    }
 
    // construct the return flag
    // if success and nothing changed return 1 
    // 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;
}