Skip to content

Commit d37f34e

Browse files
askuricaskuric
committed
initial low side added
1 parent 8ff4630 commit d37f34e

File tree

6 files changed

+350
-13
lines changed

6 files changed

+350
-13
lines changed

‎src/current_sense/InlineCurrentSense.cpp

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _
2222
// Inline sensor init function
2323
void InlineCurrentSense::init(){
2424
// configure ADC variables
25-
_configureADC(pinA,pinB,pinC);
25+
_configureADCInline(pinA,pinB,pinC);
2626
// calibrate zero offsets
2727
calibrateOffsets();
2828
}
@@ -36,9 +36,9 @@ void InlineCurrentSense::calibrateOffsets(){
3636
offset_ic = 0;
3737
// read the adc voltage 1000 times ( arbitrary number )
3838
for (int i = 0; i < calibration_rounds; i++) {
39-
offset_ia += _readADCVoltage(pinA);
40-
offset_ib += _readADCVoltage(pinB);
41-
if(_isset(pinC)) offset_ic += _readADCVoltage(pinC);
39+
offset_ia += _readADCVoltageInline(pinA);
40+
offset_ib += _readADCVoltageInline(pinB);
41+
if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC);
4242
_delay(1);
4343
}
4444
// calculate the mean offsets
@@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){
5050
// read all three phase currents (if possible 2 or 3)
5151
PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
5252
PhaseCurrent_s current;
53-
current.a = (_readADCVoltage(pinA) - offset_ia)*gain_a;// amps
54-
current.b = (_readADCVoltage(pinB) - offset_ib)*gain_b;// amps
55-
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltage(pinC) - offset_ic)*gain_c; // amps
53+
current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps
54+
current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps
55+
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps
5656
return current;
5757
}
5858
// Function synchronizing current sense with motor driver.

‎src/current_sense/LowsideCurrentSense.cpp

Lines changed: 179 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,179 @@
1+
#include "LowsideCurrentSense.h"
2+
// LowsideCurrentSensor 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+
// Lowside sensor init function
23+
void LowsideCurrentSense::init(){
24+
// configure ADC variables
25+
_configureADCLowSide(pinA,pinB,pinC);
26+
// calibrate zero offsets
27+
calibrateOffsets();
28+
}
29+
// Function finding zero offsets of the ADC
30+
void LowsideCurrentSense::calibrateOffsets(){
31+
// find adc offset = zero current voltage
32+
offset_ia =0;
33+
offset_ib= 0;
34+
offset_ic= 0;
35+
// read the adc voltage 1000 times ( arbitrary number )
36+
for (int i = 0; i < 1000; i++) {
37+
offset_ia += _readADCVoltageLowSide(pinA);
38+
offset_ib += _readADCVoltageLowSide(pinB);
39+
if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC);
40+
_delay(1);
41+
}
42+
// calculate the mean offsets
43+
offset_ia = offset_ia / 1000.0;
44+
offset_ib = offset_ib / 1000.0;
45+
if(_isset(pinC)) offset_ic = offset_ic / 1000.0;
46+
}
47+
48+
// read all three phase currents (if possible 2 or 3)
49+
PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){
50+
PhaseCurrent_s current;
51+
current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps
52+
current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps
53+
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps
54+
return current;
55+
}
56+
// Function synchronizing current sense with motor driver.
57+
// for in-line sensig no such thing is necessary
58+
int LowsideCurrentSense::driverSync(BLDCDriver *driver){
59+
_driverSyncLowSide();
60+
return 1;
61+
}
62+
63+
// Function aligning the current sense with motor driver
64+
// if all pins are connected well none of this is really necessary! - can be avoided
65+
// returns flag
66+
// 0 - fail
67+
// 1 - success and nothing changed
68+
// 2 - success but pins reconfigured
69+
// 3 - success but gains inverted
70+
// 4 - success but pins reconfigured and gains inverted
71+
int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
72+
//gain_a *= -1;
73+
//gain_b *= -1;
74+
//gain_c *= -1;
75+
76+
/*
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(2000);
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+
176+
return exit_flag;
177+
*/
178+
return 1;
179+
}

‎src/current_sense/LowsideCurrentSense.h

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

‎src/current_sense/hardware_api.h

Lines changed: 21 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
*
1010
* @param pinA - the arduino pin to be read (it has to be ADC pin)
1111
*/
12-
float _readADCVoltage(const int pinA);
12+
float _readADCVoltageInline(const int pinA);
1313

1414
/**
1515
* function reading an ADC value and returning the read voltage
@@ -18,6 +18,25 @@ float _readADCVoltage(const int pinA);
1818
* @param pinB - adc pin B
1919
* @param pinC - adc pin C
2020
*/
21-
void _configureADC(const int pinA,const int pinB,const int pinC = NOT_SET);
21+
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();
2342
#endif

‎src/current_sense/hardware_specific/esp32_mcu.cpp

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
#include "../hardware_api.h"
2+
3+
#if defined(ESP_H)
4+
5+
#include "driver/mcpwm.h"
6+
#include "soc/mcpwm_reg.h"
7+
#include "soc/mcpwm_struct.h"
8+
9+
#define _ADC_VOLTAGE 3.3
10+
#define _ADC_RESOLUTION 4095.0
11+
12+
static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1};
13+
int a1, a2, a3; //Current readings from internal current sensor amplifiers
14+
int _pinA, _pinB, _pinC;
15+
static void IRAM_ATTR isr_handler(void*);
16+
byte currentState = 1;
17+
18+
// adc counts to voltage conversion ratio
19+
// some optimizing for faster execution
20+
#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
21+
22+
// function reading an ADC value and returning the read voltage
23+
float _readADCVoltageLowSide(const int pin){
24+
uint32_t raw_adc;
25+
26+
if (pin == _pinA) raw_adc = a1;
27+
else if (pin == _pinB) raw_adc = a2;
28+
else if (pin == _pinC) raw_adc = a3;
29+
30+
return raw_adc * _ADC_CONV;
31+
}
32+
33+
34+
// function reading an ADC value and returning the read voltage
35+
void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
36+
_pinA = pinA;
37+
_pinB = pinB;
38+
if( _isset(pinC) ) _pinC = pinC;
39+
pinMode(pinA, INPUT);
40+
pinMode(pinB, INPUT);
41+
if( _isset(pinC) ) pinMode(pinC, INPUT);
42+
43+
}
44+
45+
void _driverSyncLowSide(){
46+
MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
47+
MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt
48+
//MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt
49+
mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler
50+
}
51+
52+
// Read currents when interrupt is triggered
53+
static void IRAM_ATTR isr_handler(void*){
54+
uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st;
55+
uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st;
56+
//uint32_t mcpwm_intr_status_2 = MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st;
57+
58+
if(mcpwm_intr_status_0 > 0 && currentState == 1){
59+
a1 = analogRead(_pinA);
60+
//a2 = analogRead(_pinB);
61+
currentState = 2;
62+
}
63+
else if(mcpwm_intr_status_1 > 0 && currentState == 2){
64+
a2 = analogRead(_pinB);
65+
//a3 = analogRead(_pinC);
66+
currentState = 1;
67+
}
68+
/*
69+
else if(mcpwm_intr_status_2 > 0 && currentState == 3){
70+
a3 = analogRead(_pinC);
71+
//a1 = analogRead(_pinA);
72+
currentState = 1;
73+
}*/
74+
75+
MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0;
76+
MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1;
77+
//MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2;
78+
}
79+
80+
#endif

0 commit comments

Comments
 (0)