Skip to content

Commit 842b361

Browse files
askuricaskuric
committed
new current sense alignement
1 parent 7d108c4 commit 842b361

File tree

1 file changed

+187
-96
lines changed

1 file changed

+187
-96
lines changed

‎src/common/base_classes/CurrentSense.cpp‎

Lines changed: 187 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -174,113 +174,204 @@ PhaseCurrent_s CurrentSense::readAverageCurrents(int N) {
174174
// 4 - success but pins reconfigured and gains inverted
175175
int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){
176176

177-
int exit_flag = 1;
178-
if(_isset(pinA)){
179-
// set phase A active and phases B and C down
180-
bldc_driver->setPwm(voltage, 0, 0);
181-
_delay(500);
182-
PhaseCurrent_s c = readAverageCurrents();
183-
bldc_driver->setPwm(0, 0, 0);
184-
// align phase A
185-
float ab_ratio = c.b ? fabs(c.a / c.b) : 0;
186-
float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
187-
if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
188-
gain_a *= _sign(c.a);
189-
}else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
190-
gain_a *= _sign(c.a);
191-
}else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
192-
SIMPLEFOC_DEBUG("CS: Switch A-B");
193-
// switch phase A and B
177+
bool phases_switched = 0;
178+
bool phases_inverted = 0;
179+
180+
181+
// set phase A active and phases B and C down
182+
bldc_driver->setPwm(voltage, 0, 0);
183+
_delay(500);
184+
PhaseCurrent_s c_a = readAverageCurrents();
185+
// check if currents are to low (lower than 100mA)
186+
// TODO calculate the 100mA threshold from the ADC resolution
187+
// if yes throw an error and return 0
188+
// either the current sense is not connected or the current is
189+
// too low for calibration purposes (one should raise the motor.voltage_sensor_align)
190+
if((_isset(pinA) && fabs(c_a.a) < 0.1f)
191+
|| (_isset(pinB) && fabs(c_a.b) < 0.1f)
192+
|| (_isset(pinC) && fabs(c_a.c) < 0.1f)){
193+
SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!");
194+
return 0; // measurement current too low
195+
}
196+
197+
// set phase B active and phases A and C down
198+
bldc_driver->setPwm(0, voltage, 0);
199+
_delay(500);
200+
PhaseCurrent_s c_b = readAverageCurrents();
201+
bldc_driver->setPwm(0, 0, 0);
202+
203+
// now we have to determine
204+
// 1) which pin correspond to which phase of the bldc driver
205+
// 2) if the currents measured have good polarity
206+
//
207+
// > 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
208+
// and -I/2 on the phase B and I/2 on the phase C
209+
210+
// find the highest magnitude in c_a
211+
// and make sure it's around 2 (1.5 at least) times higher than the other two
212+
float ca[3] = {fabs(c_a.a), fabs(c_a.b), fabs(c_a.c)};
213+
uint8_t max_i = -1; // max index
214+
float max_c = 0; // max current
215+
float max_c_ratio = 0; // max current ratio
216+
for(int i = 0; i < 3; i++){
217+
if(!ca[i]) continue; // current not measured
218+
if(ca[i] > max_c){
219+
max_c = ca[i];
220+
max_i = i;
221+
for(int j = 0; j < 3; j++){
222+
if(i == j) continue;
223+
if(!ca[j]) continue; // current not measured
224+
float ratio = max_c / ca[j];
225+
if(ratio > max_c_ratio) max_c_ratio = ratio;
226+
}
227+
}
228+
}
229+
230+
// check the current magnitude ratios
231+
// 1) if there is one current that is approximately 2 times higher than the other two
232+
// this is the A current
233+
// 2) if the max current is not at least 1.5 times higher than the other two
234+
// we have two cases:
235+
// - either we only measure two currents and the third one is not measured - then phase A is not measured
236+
// - or the current sense is not connected properly
237+
238+
if(max_c_ratio >=1.5f){
239+
switch (max_i){
240+
case 1: // phase B is the max current
241+
SIMPLEFOC_DEBUG("CS: Switch A-B");
242+
// switch phase A and B
243+
_swap(pinA, pinB);
244+
_swap(offset_ia, offset_ib);
245+
_swap(gain_a, gain_b);
246+
_swap(c_a.b, c_a.b);
247+
_swap(c_b.a, c_b.b); // for the next phase of alignment
248+
phases_switched = true; // signal that pins have been switched
249+
break;
250+
case 2: // phase C is the max current
251+
SIMPLEFOC_DEBUG("CS: Switch A-C");
252+
// switch phase A and C
253+
_swap(pinA, pinC);
254+
_swap(offset_ia, offset_ic);
255+
_swap(gain_a, gain_c);
256+
_swap(c_a.a, c_a.c);
257+
_swap(c_b.a, c_b.c); // for the next phase of alignment
258+
phases_switched = true;// signal that pins have been switched
259+
break;
260+
}
261+
// check if the current is negative and invert the gain if so
262+
if( _sign(c_a.a) < 0 ){
263+
SIMPLEFOC_DEBUG("CS: Inv A");
264+
gain_a *= -1;
265+
phases_inverted = true; // signal that pins have been inverted
266+
}
267+
}else if(_isset(pinA) && _isset(pinB) && _isset(pinC)){
268+
// if all three currents are measured and none of them is significantly higher
269+
// we have a problem with the current sense
270+
SIMPLEFOC_DEBUG("CS: Err A - all currents same magnitude!");
271+
return 0;
272+
}else{ //phase A is not measured so put the _NC to the phase A
273+
if(_isset(pinA) && !_isset(pinB)){
274+
SIMPLEFOC_DEBUG("CS: Switch A-(B)NC");
194275
_swap(pinA, pinB);
195276
_swap(offset_ia, offset_ib);
196277
_swap(gain_a, gain_b);
197-
gain_a *= _sign(c.b);
198-
exit_flag = 2; // signal that pins have been switched
199-
}else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
200-
SIMPLEFOC_DEBUG("CS: Switch A-C");
201-
// switch phase A and C
278+
_swap(c_a.b, c_a.b);
279+
_swap(c_b.a, c_b.b); // for the next phase of alignment
280+
phases_switched = true; // signal that pins have been switched
281+
}else if(_isset(pinA) && !_isset(pinC)){
282+
SIMPLEFOC_DEBUG("CS: Switch A-(C)NC");
202283
_swap(pinA, pinC);
203284
_swap(offset_ia, offset_ic);
204285
_swap(gain_a, gain_c);
205-
gain_a *= _sign(c.c);
206-
exit_flag = 2;// signal that pins have been switched
207-
}else{
208-
SIMPLEFOC_DEBUG("CS: Err read A");
209-
// error in current sense - phase either not measured or bad connection
210-
return 0;
286+
_swap(c_a.b, c_a.c);
287+
_swap(c_b.a, c_b.c); // for the next phase of alignment
288+
phases_switched = true; // signal that pins have been switched
211289
}
212290
}
213-
214-
if(_isset(pinB)){
215-
// set phase B active and phases A and C down
216-
bldc_driver->setPwm(0, voltage, 0);
217-
_delay(500);
218-
PhaseCurrent_s c = readAverageCurrents();
219-
bldc_driver->setPwm(0, 0, 0);
220-
float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
221-
float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
222-
if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2);
223-
gain_b *= _sign(c.b);
224-
}else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
225-
gain_b *= _sign(c.b);
226-
}else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
227-
SIMPLEFOC_DEBUG("CS: Switch B-A");
228-
// switch phase A and B
229-
_swap(pinB, pinA);
230-
_swap(offset_ib, offset_ia);
231-
_swap(gain_b, gain_a);
232-
gain_b *= _sign(c.a);
233-
exit_flag = 2; // signal that pins have been switched
234-
}else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
235-
SIMPLEFOC_DEBUG("CS: Switch B-C");
291+
// at this point the current sensing on phase A can be either:
292+
// - aligned with the driver phase A
293+
// - or the phase A is not measured and the _NC is connected to the phase A
294+
//
295+
// In either case A is done, now we have to check the phase B and C
296+
297+
// check the phase B
298+
// find the highest magnitude in c_b
299+
// and make sure it's around 2 (1.5 at least) times higher than the other two
300+
float cb[3] = {fabs(c_b.a), fabs(c_b.b), fabs(c_b.c)};
301+
max_i = -1; // max index
302+
max_c = 0; // max current
303+
max_c_ratio = 0; // max current ratio
304+
for(int i = 0; i < 3; i++){
305+
if(!cb[i]) continue; // current not measured
306+
if(cb[i] > max_c){
307+
max_c = cb[i];
308+
max_i = i;
309+
for(int j = 0; j < 3; j++){
310+
if(i == j) continue;
311+
if(!cb[j]) continue; // current not measured
312+
float ratio = max_c / cb[j];
313+
if(ratio > max_c_ratio) max_c_ratio = ratio;
314+
}
315+
}
316+
}
317+
if(max_c_ratio >= 1.5f){
318+
switch (max_i){
319+
case 0: // phase A is the max current
320+
// this is an error as phase A is already aligned
321+
SIMPLEFOC_DEBUG("CS: Err align B");
322+
return 0;
323+
case 2: // phase C is the max current
324+
SIMPLEFOC_DEBUG("CS: Switch B-C");
325+
_swap(pinB, pinC);
326+
_swap(offset_ib, offset_ic);
327+
_swap(gain_b, gain_c);
328+
_swap(c_b.b, c_b.c);
329+
phases_switched = true; // signal that pins have been switched
330+
break;
331+
}
332+
// check if the current is negative and invert the gain if so
333+
if( _sign(c_b.b) < 0 ){
334+
SIMPLEFOC_DEBUG("CS: Inv B");
335+
gain_b *= -1;
336+
phases_inverted = true; // signal that pins have been inverted
337+
}
338+
}else if(_isset(pinB) && _isset(pinC)){
339+
// if all three currents are measured and none of them is significantly higher
340+
// we have a problem with the current sense
341+
SIMPLEFOC_DEBUG("CS: Err B - all currents same magnitude!");
342+
return 0;
343+
}else{ //phase B is not measured so put the _NC to the phase B
344+
if(_isset(pinB) && !_isset(pinC)){
345+
SIMPLEFOC_DEBUG("CS: Switch B-(C)NC");
236346
_swap(pinB, pinC);
237347
_swap(offset_ib, offset_ic);
238348
_swap(gain_b, gain_c);
239-
gain_b *= _sign(c.c);
240-
exit_flag = 2; // signal that pins have been switched
241-
}else{
242-
SIMPLEFOC_DEBUG("CS: Error read B");
243-
// error in current sense - phase either not measured or bad connection
244-
return 0;
245-
}
349+
_swap(c_b.b, c_b.c);
350+
phases_switched = true; // signal that pins have been switched
351+
}
246352
}
247-
248-
// if phase C measured
353+
// at this point the current sensing on phase A and B can be either:
354+
// - aligned with the driver phase A and B
355+
// - or the phase A and B are not measured and the _NC is connected to the phase A and B
356+
//
357+
// In either case A and B is done, now we have to check the phase C
358+
// phase C is also aligned if it is measured (not _NC)
359+
// we have to check if the current is negative and invert the gain if so
249360
if(_isset(pinC)){
250-
// set phase C active and phases A and B down
251-
bldc_driver->setPwm(0, 0, voltage);
252-
_delay(500);
253-
PhaseCurrent_s c = readAverageCurrents();
254-
bldc_driver->setPwm(0, 0, 0);
255-
float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
256-
float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
257-
if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
258-
gain_c *= _sign(c.c);
259-
}else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
260-
gain_c *= _sign(c.c);
261-
}else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
262-
SIMPLEFOC_DEBUG("CS: Switch C-A");
263-
// switch phase A and C
264-
_swap(pinC, pinA);
265-
_swap(offset_ic, offset_ia);
266-
_swap(gain_c, gain_a);
267-
gain_c *= _sign(c.a);
268-
exit_flag = 2; // signal that pins have been switched
269-
}else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
270-
SIMPLEFOC_DEBUG("CS: Switch C-B");
271-
_swap(pinC, pinB);
272-
_swap(offset_ic, offset_ib);
273-
_swap(gain_c, gain_b);
274-
gain_b *= _sign(c.b);
275-
exit_flag = 2; // signal that pins have been switched
276-
}else{
277-
SIMPLEFOC_DEBUG("CS: Err read C");
278-
// error in current sense - phase either not measured or bad connection
279-
return 0;
280-
}
361+
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)
362+
SIMPLEFOC_DEBUG("CS: Inv C");
363+
gain_c *= -1;
364+
phases_inverted = true; // signal that pins have been inverted
365+
}
281366
}
282-
// add 2 if pin gains negative
283-
if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
367+
368+
// construct the return flag
369+
// if the phases have been switched return 2
370+
// if the gains have been inverted return 3
371+
// if both return 4
372+
uint8_t exit_flag = 1;
373+
if(phases_switched) exit_flag += 1;
374+
if(phases_inverted) exit_flag += 2;
284375
return exit_flag;
285376
}
286377

@@ -303,7 +394,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive
303394
_delay(500);
304395
PhaseCurrent_s c = readAverageCurrents();
305396
// disable the phases
306-
stepper_driver->setPwm(0, 0);
397+
stepper_driver->setPwm(0, 0);
307398
if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
308399
SIMPLEFOC_DEBUG("CS: Err too low current!");
309400
return 0; // measurement current too low
@@ -321,7 +412,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive
321412
gain_a *= _sign(c.b);
322413
exit_flag = 2; // signal that pins have been switched
323414
}else if (c.a < 0){
324-
SIMPLEFOC_DEBUG("CS: Neg A");
415+
SIMPLEFOC_DEBUG("CS: Inv A");
325416
gain_a *= -1;
326417
}
327418
}
@@ -339,7 +430,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive
339430
// align phase A
340431
// check if measured current a is positive and invert if not
341432
if (c.b < 0){
342-
SIMPLEFOC_DEBUG("CS: Neg B");
433+
SIMPLEFOC_DEBUG("CS: Inv B");
343434
gain_b *= -1;
344435
}
345436
}

0 commit comments

Comments
 (0)