Skip to content

Commit db7d865

Browse files
maxlemmaxlem
committed
LowSideCurrentSense: first (truly) successful DMA+ADC readings, 1.65-centered readings with the drv8305, just as intented
1 parent a0d17a0 commit db7d865

File tree

4 files changed

+500
-19
lines changed

4 files changed

+500
-19
lines changed

‎src/current_sense/LowSideCurrentSense.cpp

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
1+
#include "LowSideCurrentSense.h"
2+
// InlineCurrentSensor constructor
3+
// - shunt_resistor - shunt resistor value
4+
// - gain - current-sense op-amp gain
5+
// - phA - A phase adc pin
6+
// - phB - B phase adc pin
7+
// - phC - C phase adc pin (optional)
8+
LowSideCurrentSense::LowSideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
9+
pinA = _pinA;
10+
pinB = _pinB;
11+
pinC = _pinC;
12+
13+
shunt_resistor = _shunt_resistor;
14+
amp_gain = _gain;
15+
volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps
16+
// gains for each phase
17+
gain_a = volts_to_amps_ratio;
18+
gain_b = volts_to_amps_ratio;
19+
gain_c = volts_to_amps_ratio;
20+
}
21+
22+
// Inline sensor init function
23+
void LowSideCurrentSense::init(){
24+
// configure ADC variables
25+
_configure3PinsDMA(pinA,pinB,pinC);
26+
_start3PinsDMA(); //start next acuisition
27+
// calibrate zero offsets
28+
// calibrateOffsets();
29+
}
30+
// Function finding zero offsets of the ADC
31+
void LowSideCurrentSense::calibrateOffsets(){
32+
const int calibration_rounds = 1000;
33+
34+
// find adc offset = zero current voltage
35+
offset_ia = 0;
36+
offset_ib = 0;
37+
offset_ic = 0;
38+
// read the adc voltage 1000 times ( arbitrary number )
39+
for (int i = 0; i < calibration_rounds; i++) {
40+
offset_ia += _readADCVoltage(pinA);
41+
offset_ib += _readADCVoltage(pinB);
42+
if(_isset(pinC)) offset_ic += _readADCVoltage(pinC);
43+
_delay(1);
44+
}
45+
// calculate the mean offsets
46+
offset_ia = offset_ia / calibration_rounds;
47+
offset_ib = offset_ib / calibration_rounds;
48+
if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
49+
}
50+
51+
// read all three phase currents (if possible 2 or 3)
52+
PhaseCurrent_s LowSideCurrentSense::getPhaseCurrents(){
53+
PhaseCurrent_s current;
54+
// current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps
55+
// current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps
56+
// current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps
57+
_read3PinsDMA(pinA, pinB, pinC, current.a, current.b, current.c);
58+
_start3PinsDMA();
59+
60+
return current;
61+
}
62+
// Function synchronizing current sense with motor driver.
63+
// for in-line sensig no such thing is necessary
64+
int LowSideCurrentSense::driverSync(BLDCDriver *driver){
65+
return 1;
66+
}
67+
68+
// Function aligning the current sense with motor driver
69+
// if all pins are connected well none of this is really necessary! - can be avoided
70+
// returns flag
71+
// 0 - fail
72+
// 1 - success and nothing changed
73+
// 2 - success but pins reconfigured
74+
// 3 - success but gains inverted
75+
// 4 - success but pins reconfigured and gains inverted
76+
int LowSideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
77+
int exit_flag = 1;
78+
if(skip_align) return exit_flag;
79+
80+
// set phase A active and phases B and C down
81+
driver->setPwm(voltage, 0, 0);
82+
_delay(200);
83+
PhaseCurrent_s c = getPhaseCurrents();
84+
// read the current 100 times ( arbitrary number )
85+
for (int i = 0; i < 100; i++) {
86+
PhaseCurrent_s c1 = getPhaseCurrents();
87+
c.a = c.a*0.6 + 0.4*c1.a;
88+
c.b = c.b*0.6 + 0.4*c1.b;
89+
c.c = c.c*0.6 + 0.4*c1.c;
90+
_delay(3);
91+
}
92+
driver->setPwm(0, 0, 0);
93+
// align phase A
94+
float ab_ratio = fabs(c.a / c.b);
95+
float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
96+
if( ab_ratio > 1.5 ){ // should be ~2
97+
gain_a *= _sign(c.a);
98+
}else if( ab_ratio < 0.7 ){ // should be ~0.5
99+
// switch phase A and B
100+
int tmp_pinA = pinA;
101+
pinA = pinB;
102+
pinB = tmp_pinA;
103+
gain_a *= _sign(c.b);
104+
exit_flag = 2; // signal that pins have been switched
105+
}else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5
106+
// switch phase A and C
107+
int tmp_pinA = pinA;
108+
pinA = pinC;
109+
pinC= tmp_pinA;
110+
gain_a *= _sign(c.c);
111+
exit_flag = 2;// signal that pins have been switched
112+
}else{
113+
// error in current sense - phase either not measured or bad connection
114+
return 0;
115+
}
116+
117+
// set phase B active and phases A and C down
118+
driver->setPwm(0, voltage, 0);
119+
_delay(200);
120+
c = getPhaseCurrents();
121+
// read the current 50 times
122+
for (int i = 0; i < 100; i++) {
123+
PhaseCurrent_s c1 = getPhaseCurrents();
124+
c.a = c.a*0.6 + 0.4*c1.a;
125+
c.b = c.b*0.6 + 0.4*c1.b;
126+
c.c = c.c*0.6 + 0.4*c1.c;
127+
_delay(3);
128+
}
129+
driver->setPwm(0, 0, 0);
130+
float ba_ratio = fabs(c.b/c.a);
131+
float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
132+
if( ba_ratio > 1.5 ){ // should be ~2
133+
gain_b *= _sign(c.b);
134+
}else if( ba_ratio < 0.7 ){ // it should be ~0.5
135+
// switch phase A and B
136+
int tmp_pinB = pinB;
137+
pinB = pinA;
138+
pinA = tmp_pinB;
139+
gain_b *= _sign(c.a);
140+
exit_flag = 2; // signal that pins have been switched
141+
}else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5
142+
// switch phase A and C
143+
int tmp_pinB = pinB;
144+
pinB = pinC;
145+
pinC = tmp_pinB;
146+
gain_b *= _sign(c.c);
147+
exit_flag = 2; // signal that pins have been switched
148+
}else{
149+
// error in current sense - phase either not measured or bad connection
150+
return 0;
151+
}
152+
153+
// if phase C measured
154+
if(_isset(pinC)){
155+
// set phase B active and phases A and C down
156+
driver->setPwm(0, 0, voltage);
157+
_delay(200);
158+
c = getPhaseCurrents();
159+
// read the adc voltage 500 times ( arbitrary number )
160+
for (int i = 0; i < 50; i++) {
161+
PhaseCurrent_s c1 = getPhaseCurrents();
162+
c.c = (c.c+c1.c)/50.0;
163+
}
164+
driver->setPwm(0, 0, 0);
165+
gain_c *= _sign(c.c);
166+
}
167+
168+
if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
169+
// exit flag is either
170+
// 0 - fail
171+
// 1 - success and nothing changed
172+
// 2 - success but pins reconfigured
173+
// 3 - success but gains inverted
174+
// 4 - success but pins reconfigured and gains inverted
175+
return exit_flag;
176+
}
177+

‎src/current_sense/LowSideCurrentSense.h

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
#ifndef LOW_SIDE_CS_LIB_H
2+
#define LOW_SIDE_CS_LIB_H
3+
4+
#include "Arduino.h"
5+
#include "../common/foc_utils.h"
6+
#include "../common/time_utils.h"
7+
#include "../common/base_classes/CurrentSense.h"
8+
#include "hardware_api.h"
9+
10+
class LowSideCurrentSense: public CurrentSense{
11+
public:
12+
/**
13+
LowSideCurrentSense class constructor
14+
@param shunt_resistor shunt resistor value
15+
@param gain current-sense op-amp gain
16+
@param phA A phase adc pin
17+
@param phB B phase adc pin
18+
@param phC C phase adc pin (optional)
19+
*/
20+
LowSideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
21+
22+
// CurrentSense interface implementing functions
23+
void init() override;
24+
PhaseCurrent_s getPhaseCurrents() override;
25+
int driverSync(BLDCDriver *driver) override;
26+
int driverAlign(BLDCDriver *driver, float voltage) override;
27+
28+
// ADC measuremnet gain for each phase
29+
// support for different gains for different phases of more commonly - inverted phase currents
30+
// this should be automated later
31+
float gain_a; //!< phase A gain
32+
float gain_b; //!< phase B gain
33+
float gain_c; //!< phase C gain
34+
35+
private:
36+
37+
// hardware variables
38+
int pinA; //!< pin A analog pin for current measurement
39+
int pinB; //!< pin B analog pin for current measurement
40+
int pinC; //!< pin C analog pin for current measurement
41+
42+
// gain variables
43+
double shunt_resistor; //!< Shunt resistor value
44+
double amp_gain; //!< amp gain value
45+
double volts_to_amps_ratio; //!< Volts to amps ratio
46+
47+
/**
48+
* Function finding zero offsets of the ADC
49+
*/
50+
void calibrateOffsets();
51+
double offset_ia; //!< zero current A voltage value (center of the adc reading)
52+
double offset_ib; //!< zero current B voltage value (center of the adc reading)
53+
double offset_ic; //!< zero current C voltage value (center of the adc reading)
54+
55+
};
56+
57+
#endif

‎src/current_sense/hardware_api.h

Lines changed: 0 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -20,23 +20,4 @@ float _readADCVoltageInline(const int pinA);
2020
*/
2121
void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET);
2222

23-
/**
24-
* function reading an ADC value and returning the read voltage
25-
*
26-
* @param pinA - adc pin A
27-
* @param pinB - adc pin B
28-
* @param pinC - adc pin C
29-
*/
30-
void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET);
31-
/**
32-
* function reading an ADC value and returning the read voltage
33-
*
34-
* @param pinA - the arduino pin to be read (it has to be ADC pin)
35-
*/
36-
float _readADCVoltageLowSide(const int pinA);
37-
38-
/**
39-
* function syncing the Driver with the ADC for the LowSide Sensing
40-
*/
41-
void _driverSyncLowSide();
4223
#endif

0 commit comments

Comments
 (0)