Skip to content

Commit 4a9bf52

Browse files
maxicor88maxicor88
committed
Fix virtual motor to work with latest Vesc v5.
connect_virtual_motor_command only requires 3 arguments, load torque, load inertia and battery voltage. The rest of the motor characteristics, are taken from already existant variables in VESC motor configuration. Solved issue with encoder sensor configuration. Solved fault gate driver undervoltage Improved hardware dependency defines in virtual motor.c Signed-off-by: Maximiliano Cordoba <[email protected]>
1 parent 960cd3b commit 4a9bf52

File tree

3 files changed

+122
-71
lines changed

3 files changed

+122
-71
lines changed

‎mcpwm_foc.c

+16-6
Original file line numberDiff line numberDiff line change
@@ -529,7 +529,7 @@ void mcpwm_foc_init(volatile mc_configuration *conf_m1, volatile mc_configuratio
529529
update_hfi_samples(m_motor_2.m_conf->foc_hfi_samples, &m_motor_2);
530530
#endif
531531

532-
virtual_motor_init();
532+
virtual_motor_init(conf_m1);
533533

534534
TIM_DeInit(TIM1);
535535
TIM_DeInit(TIM2);
@@ -723,6 +723,8 @@ void mcpwm_foc_set_configuration(volatile mc_configuration *configuration) {
723723
stop_pwm_hw(motor_now());
724724
update_hfi_samples(motor_now()->m_conf->foc_hfi_samples, motor_now());
725725
}
726+
727+
virtual_motor_set_configuration(configuration);
726728
}
727729

728730
mc_state mcpwm_foc_get_state(void) {
@@ -2329,14 +2331,22 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
23292331

23302332
UTILS_LP_FAST(motor_now->m_motor_state.v_bus, GET_INPUT_VOLTAGE(), 0.1);
23312333

2332-
float enc_ang = 0;
2333-
if (encoder_is_configured()) {
2334-
if (virtual_motor_is_connected()){
2334+
volatile float enc_ang = 0;
2335+
volatile bool encoder_is_being_used = false;
2336+
2337+
if(virtual_motor_is_connected()){
2338+
if(conf_now->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER ){
23352339
enc_ang = virtual_motor_get_angle_deg();
2336-
} else {
2340+
encoder_is_being_used = true;
2341+
}
2342+
}else{
2343+
if (encoder_is_configured()) {
23372344
enc_ang = encoder_read_deg();
2345+
encoder_is_being_used = true;
23382346
}
2347+
}
23392348

2349+
if(encoder_is_being_used){
23402350
float phase_tmp = enc_ang;
23412351
if (conf_now->foc_encoder_inverted) {
23422352
phase_tmp = 360.0 - phase_tmp;
@@ -2460,7 +2470,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
24602470

24612471
switch (conf_now->foc_sensor_mode) {
24622472
case FOC_SENSOR_MODE_ENCODER:
2463-
if (encoder_index_found()) {
2473+
if (encoder_index_found() || virtual_motor_is_connected()) {
24642474
motor_now->m_motor_state.phase = correct_encoder(
24652475
motor_now->m_phase_now_observer,
24662476
motor_now->m_phase_now_encoder,

‎virtual_motor.c

+104-64
Original file line numberDiff line numberDiff line change
@@ -29,14 +29,12 @@
2929
typedef struct{
3030
//constant variables
3131
float Ts; //Sample Time in s
32-
float Rs; //Stator Resistance Phase to Neutral in Ohms
33-
float Ld; //Inductance in d-Direction in H
34-
float Lq; //Inductance in q-Direction in H
35-
float flux_linkage; //Flux linkage of the permanent magnets in mWb
3632
float J; //Rotor/Load Inertia in Nm*s^2
3733
int v_max_adc; //max voltage that ADC can measure
3834
int pole_pairs; //number of pole pairs ( pole numbers / 2)
3935
float km; //constant = 1.5 * pole pairs
36+
float ld; //motor inductance in D axis in uHy
37+
float lq; //motor inductance in Q axis in uHy
4038

4139
//non constant variables
4240
float id; //Current in d-Direction in Amps
@@ -68,10 +66,10 @@ static volatile virtual_motor_t virtual_motor;
6866
static volatile int m_curr0_offset_backup;
6967
static volatile int m_curr1_offset_backup;
7068
static volatile int m_curr2_offset_backup;
69+
static volatile mc_configuration *m_conf;
7170

7271
//private functions
73-
static void connect_virtual_motor(float ml, float J, float Ld, float Lq,
74-
float Rs,float lambda,float Vbus);
72+
static void connect_virtual_motor(float ml, float J, float Vbus);
7573
static void disconnect_virtual_motor(void);
7674
static inline void run_virtual_motor_electrical(float v_alpha, float v_beta);
7775
static inline void run_virtual_motor_mechanics(float ml);
@@ -85,7 +83,10 @@ static void terminal_cmd_disconnect_virtual_motor(int argc, const char **argv);
8583
/**
8684
* Virtual motor initialization
8785
*/
88-
void virtual_motor_init(void){
86+
void virtual_motor_init(volatile mc_configuration *conf){
87+
88+
virtual_motor_set_configuration(conf);
89+
8990
//virtual motor variables init
9091
virtual_motor.connected = false; //disconnected
9192

@@ -103,15 +104,12 @@ void virtual_motor_init(void){
103104
virtual_motor.i_beta = 0.0;
104105
virtual_motor.id_int = 0.0;
105106
virtual_motor.iq = 0.0;
106-
const volatile mc_configuration *conf = mc_interface_get_configuration();
107-
virtual_motor.pole_pairs = conf->si_motor_poles / 2;
108-
virtual_motor.km = 1.5 * virtual_motor.pole_pairs;
109107

110108
// Register terminal callbacks used for virtual motor setup
111109
terminal_register_command_callback(
112110
"connect_virtual_motor",
113111
"connects virtual motor",
114-
"[ml][J][Ld][Lq][Rs][lambda][Vbus]",
112+
"[ml][J][Vbus]",
115113
terminal_cmd_connect_virtual_motor);
116114

117115
terminal_register_command_callback(
@@ -121,6 +119,31 @@ void virtual_motor_init(void){
121119
terminal_cmd_disconnect_virtual_motor);
122120
}
123121

122+
void virtual_motor_set_configuration(volatile mc_configuration *conf){
123+
m_conf = conf;
124+
125+
//recalculate constants that depend on m_conf
126+
virtual_motor.pole_pairs = m_conf->si_motor_poles / 2;
127+
virtual_motor.km = 1.5 * virtual_motor.pole_pairs;
128+
#ifdef HW_HAS_PHASE_SHUNTS
129+
if (m_conf->foc_sample_v0_v7) {
130+
virtual_motor.Ts = (1.0 / m_conf->foc_f_sw) ;
131+
} else {
132+
virtual_motor.Ts = (1.0 / (m_conf->foc_f_sw / 2.0));
133+
}
134+
#else
135+
virtual_motor.Ts = (1.0 / m_conf->foc_f_sw) ;
136+
#endif
137+
138+
if(m_conf->foc_motor_ld_lq_diff > 0.0){
139+
virtual_motor.lq = m_conf->foc_motor_l + m_conf->foc_motor_ld_lq_diff /2;
140+
virtual_motor.ld = m_conf->foc_motor_l - m_conf->foc_motor_ld_lq_diff /2;
141+
}else{
142+
virtual_motor.lq = m_conf->foc_motor_l ;
143+
virtual_motor.ld = m_conf->foc_motor_l ;
144+
}
145+
}
146+
124147
/**
125148
* Virtual motor interrupt handler
126149
*/
@@ -151,14 +174,9 @@ float virtual_motor_get_angle_deg(void){
151174
*
152175
* @param ml : torque present at motor axis in Nm
153176
* @param J: rotor inertia Nm*s^2
154-
* @param Ld: inductance at d axis in Hy
155-
* @param Lq: inductance at q axis in Hy
156-
* @param Rs: resistance in ohms
157-
* @param lambda: flux linkage in Vs/rad
158177
* @param Vbus: Bus voltage in Volts
159178
*/
160-
static void connect_virtual_motor(float ml , float J, float Ld, float Lq,
161-
float Rs, float lambda, float Vbus){
179+
static void connect_virtual_motor(float ml , float J, float Vbus){
162180
if(virtual_motor.connected == false){
163181
//first we send 0.0 current command to make system stop PWM outputs
164182
mcpwm_foc_set_current(0.0);
@@ -184,32 +202,40 @@ static void connect_virtual_motor(float ml , float J, float Ld, float Lq,
184202
mcpwm_foc_set_current_offsets(2048, 2048, 2048);
185203

186204
//sets virtual motor variables
187-
ADC_Value[ ADC_IND_VIN_SENS ] = Vbus * VOLTAGE_TO_ADC_FACTOR;
188205
ADC_Value[ ADC_IND_TEMP_MOS ] = 2048;
189206
ADC_Value[ ADC_IND_TEMP_MOTOR ] = 2048;
190-
#ifdef HW_VERSION_AXIOM
191-
ADC_Value[ ADC_IND_VOUT_GATE_DRV ] = 1692;
192-
// 1692 gives 15.0 as Gate Driver Voltage
193-
//( 15.0 = (ADC_Value[] * 11.0 * 3.3) / 4096 )
207+
#ifdef HW_HAS_GATE_DRIVER_SUPPLY_MONITOR
208+
//we load 1 to get the transfer function indirectly
209+
ADC_Value[ ADC_IND_VOUT_GATE_DRV ] = 1;
210+
211+
float tempVoltage = GET_GATE_DRIVER_SUPPLY_VOLTAGE();
212+
if(tempVoltage != 0.0){
213+
ADC_Value[ ADC_IND_VOUT_GATE_DRV ] = ( (HW_GATE_DRIVER_SUPPLY_MAX_VOLTAGE + HW_GATE_DRIVER_SUPPLY_MIN_VOLTAGE) / 2.0 ) /
214+
GET_GATE_DRIVER_SUPPLY_VOLTAGE();
215+
}
194216
#endif
195-
virtual_motor.phi = mcpwm_foc_get_phase() * M_PI / 180.0;// 0.0;//m_motor_state.phase;
217+
virtual_motor.phi = mcpwm_foc_get_phase() * M_PI / 180.0;
196218
utils_fast_sincos_better(virtual_motor.phi, (float*)&virtual_motor.sin_phi,
197219
(float*)&virtual_motor.cos_phi);
220+
221+
if(m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER){
222+
encoder_deinit();
223+
}
224+
}
225+
226+
//we load 1 to get the transfer function indirectly
227+
ADC_Value[ ADC_IND_VIN_SENS ] = 1;
228+
229+
float tempVoltage = GET_INPUT_VOLTAGE();
230+
if(tempVoltage != 0.0){
231+
ADC_Value[ ADC_IND_VIN_SENS ] = Vbus / GET_INPUT_VOLTAGE();
198232
}
199233

200234
//initialize constants
201-
virtual_motor.Ts = mcpwm_foc_get_ts();
202235
virtual_motor.v_max_adc = Vbus;
203236
virtual_motor.J = J;
204-
virtual_motor.Ld = Ld;
205-
virtual_motor.Lq = Lq;
206-
virtual_motor.flux_linkage = lambda;
207-
virtual_motor.Rs = Rs;
208-
virtual_motor.ml = ml;
209237
virtual_motor.tsj = virtual_motor.Ts / virtual_motor.J;
210-
const volatile mc_configuration *conf = mc_interface_get_configuration();
211-
virtual_motor.pole_pairs = conf->si_motor_poles / 2;
212-
virtual_motor.km = 1.5 * virtual_motor.pole_pairs;
238+
virtual_motor.ml = ml;
213239

214240
virtual_motor.connected = true;
215241
}
@@ -246,6 +272,31 @@ static void disconnect_virtual_motor( void ){
246272
ADC_InitStructure.ADC_NbrOfConversion = HW_ADC_NBR_CONV;
247273

248274
ADC_Init(ADC1, &ADC_InitStructure);
275+
276+
if(m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER){
277+
switch (m_conf->m_sensor_port_mode) {
278+
case SENSOR_PORT_MODE_ABI:
279+
encoder_init_abi(m_conf->m_encoder_counts);
280+
break;
281+
282+
case SENSOR_PORT_MODE_AS5047_SPI:
283+
encoder_init_as5047p_spi();
284+
break;
285+
286+
case SENSOR_PORT_MODE_AD2S1205:
287+
encoder_init_ad2s1205_spi();
288+
break;
289+
290+
case SENSOR_PORT_MODE_SINCOS:
291+
encoder_init_sincos(m_conf->foc_encoder_sin_gain, m_conf->foc_encoder_sin_offset,
292+
m_conf->foc_encoder_cos_gain, m_conf->foc_encoder_cos_offset,
293+
m_conf->foc_encoder_sincos_filter_constant);
294+
break;
295+
296+
default:
297+
break;
298+
}
299+
}
249300
}
250301
}
251302

@@ -271,46 +322,41 @@ static inline void run_virtual_motor(float v_alpha, float v_beta, float ml){
271322
*/
272323
static inline void run_virtual_motor_electrical(float v_alpha, float v_beta){
273324

274-
utils_fast_sincos_better( virtual_motor.phi , (float*)&virtual_motor.sin_phi,
275-
(float*)&virtual_motor.cos_phi );
276-
277325
virtual_motor.vd = virtual_motor.cos_phi * v_alpha + virtual_motor.sin_phi * v_beta;
278326
virtual_motor.vq = virtual_motor.cos_phi * v_beta - virtual_motor.sin_phi * v_alpha;
279327

280328
// d axis current
281329
virtual_motor.id_int += ((virtual_motor.vd +
282330
virtual_motor.we *
283331
virtual_motor.pole_pairs *
284-
virtual_motor.Lq * virtual_motor.iq -
285-
virtual_motor.Rs * virtual_motor.id )
286-
* virtual_motor.Ts ) / virtual_motor.Ld;
287-
virtual_motor.id = virtual_motor.id_int - virtual_motor.flux_linkage / virtual_motor.Ld;
332+
virtual_motor.lq * virtual_motor.iq -
333+
m_conf->foc_motor_r * virtual_motor.id )
334+
* virtual_motor.Ts ) / virtual_motor.ld;
335+
virtual_motor.id = virtual_motor.id_int - m_conf->foc_motor_flux_linkage / virtual_motor.ld;
288336

289337
// q axis current
290338
virtual_motor.iq += (virtual_motor.vq -
291339
virtual_motor.we *
292340
virtual_motor.pole_pairs *
293-
(virtual_motor.Ld * virtual_motor.id + virtual_motor.flux_linkage) -
294-
virtual_motor.Rs * virtual_motor.iq )
295-
* virtual_motor.Ts / virtual_motor.Lq;
341+
(virtual_motor.ld * virtual_motor.id + m_conf->foc_motor_flux_linkage) -
342+
m_conf->foc_motor_r * virtual_motor.iq )
343+
* virtual_motor.Ts / virtual_motor.lq;
344+
345+
// // limit current maximum values
346+
utils_truncate_number_abs((float *) &(virtual_motor.iq) , (2048 * FAC_CURRENT) );
347+
utils_truncate_number_abs((float *) &(virtual_motor.id) , (2048 * FAC_CURRENT) );
296348
}
297349

298350
/**
299351
* Run mechanical side of the machine
300352
* @param ml externally applied load torque in Nm
301353
*/
302354
static inline void run_virtual_motor_mechanics(float ml){
303-
virtual_motor.me = virtual_motor.km * (virtual_motor.flux_linkage +
304-
(virtual_motor.Ld - virtual_motor.Lq) *
355+
virtual_motor.me = virtual_motor.km * (m_conf->foc_motor_flux_linkage +
356+
(virtual_motor.ld - virtual_motor.lq) *
305357
virtual_motor.id ) * virtual_motor.iq;
306358
// omega
307-
float w_aux = virtual_motor.we + virtual_motor.tsj * (virtual_motor.me - ml);
308-
309-
if( w_aux < 0.0 ){
310-
virtual_motor.we = 0;
311-
}else{
312-
virtual_motor.we = w_aux;
313-
}
359+
virtual_motor.we += virtual_motor.tsj * (virtual_motor.me - ml);
314360

315361
// phi
316362
virtual_motor.phi += virtual_motor.we * virtual_motor.Ts;
@@ -323,13 +369,15 @@ static inline void run_virtual_motor_mechanics(float ml){
323369
while( virtual_motor.phi < -1.0 * M_PI ){
324370
virtual_motor.phi += ( 2 * M_PI);
325371
}
326-
327372
}
328373

329374
/**
330375
* Take the id and iq calculated values and translate them into ADC_Values
331376
*/
332377
static inline void run_virtual_motor_park_clark_inverse( void ){
378+
utils_fast_sincos_better( virtual_motor.phi , (float*)&virtual_motor.sin_phi,
379+
(float*)&virtual_motor.cos_phi );
380+
333381
// Park Inverse
334382
virtual_motor.i_alpha = virtual_motor.cos_phi * virtual_motor.id -
335383
virtual_motor.sin_phi * virtual_motor.iq;
@@ -366,28 +414,20 @@ static inline void run_virtual_motor_park_clark_inverse( void ){
366414
* connect_virtual_motor command
367415
*/
368416
static void terminal_cmd_connect_virtual_motor(int argc, const char **argv) {
369-
if( argc == 8 ){
417+
if( argc == 4 ){
370418
float ml; //torque load in motor axis
371-
float Ld; //inductance in d axis
372-
float Lq; //inductance in q axis
373419
float J; //rotor inertia
374-
float Rs; //resistance of motor inductance
375-
float lambda;//rotor flux linkage
376420
float Vbus;//Bus voltage
377421

378422
sscanf(argv[1], "%f", &ml);
379423
sscanf(argv[2], "%f", &J);
380-
sscanf(argv[3], "%f", &Ld);
381-
sscanf(argv[4], "%f", &Lq);
382-
sscanf(argv[5], "%f", &Rs);
383-
sscanf(argv[6], "%f", &lambda);
384-
sscanf(argv[7], "%f", &Vbus);
424+
sscanf(argv[3], "%f", &Vbus);
385425

386-
connect_virtual_motor( ml , J, Ld , Lq , Rs, lambda, Vbus);
426+
connect_virtual_motor( ml , J, Vbus);
387427
commands_printf("virtual motor connected");
388428
}
389429
else{
390-
commands_printf("arguments should be 7" );
430+
commands_printf("arguments should be 3" );
391431
}
392432
}
393433

‎virtual_motor.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,8 @@
2323

2424
#include "datatypes.h"
2525

26-
void virtual_motor_init(void);
26+
void virtual_motor_init(volatile mc_configuration *conf);
27+
void virtual_motor_set_configuration(volatile mc_configuration *conf);
2728
void virtual_motor_int_handler(float v_alpha, float v_beta);
2829
bool virtual_motor_is_connected(void);
2930
float virtual_motor_get_angle_deg(void);

0 commit comments

Comments
 (0)