From 4a94d0ec4c9f5f8b3df0889e9cd4ddaf1236e558 Mon Sep 17 00:00:00 2001 From: Maximiliano Cordoba Date: Sat, 6 Apr 2019 10:36:00 -0300 Subject: [PATCH] Add command to connect a virtual motor with configurable parameters This commit adds a motor model running within the vesc firmware,and from the vesc terminal a user or a test script can set the mechanical load torque, inertia, phase resistance, Lq and Ld phase inductances (this generic model includes IPM motors), flux linkage and battery voltage. Virtual motor parameters set at the command line should match with vesc configuration, for example phase resistance, inductance and flux linkage should match and have the correct observer gain. Observer works with the virtual motor, with some hiccups during startup For solid results its better to use sensored mode. If vesc is configured to use an SPI encoder the virtual motor phase angle will be injected as an encoder angle readout. For safety PWM outputs are disabled during simulation. Signed-off-by: Maximiliano Cordoba --- Makefile | 1 + conf_general.h | 2 + hwconf/hw_palta.h | 3 + mcpwm_foc.c | 66 +++++++- mcpwm_foc.h | 4 + virtual_motor.c | 395 ++++++++++++++++++++++++++++++++++++++++++++++ virtual_motor.h | 30 ++++ 7 files changed, 498 insertions(+), 3 deletions(-) create mode 100644 virtual_motor.c create mode 100644 virtual_motor.h diff --git a/Makefile b/Makefile index 22a0faa1..fbe34d40 100644 --- a/Makefile +++ b/Makefile @@ -154,6 +154,7 @@ CSRC = $(STARTUPSRC) \ confgenerator.c \ timer.c \ i2c_bb.c \ + virtual_motor.c \ $(HWSRC) \ $(APPSRC) \ $(NRFSRC) \ diff --git a/conf_general.h b/conf_general.h index 61eed550..a0cb3c0c 100644 --- a/conf_general.h +++ b/conf_general.h @@ -188,6 +188,8 @@ // Current ADC to amperes factor #define FAC_CURRENT ((V_REG / 4095.0) / (CURRENT_SHUNT_RES * CURRENT_AMP_GAIN)) +#define VOLTAGE_TO_ADC_FACTOR ( VIN_R2 / (VIN_R2 + VIN_R1) ) * ( 4096.0 / V_REG ) + // Actual voltage on 3.3V net based on internal reference //#define V_REG (1.21 / ((float)ADC_Value[ADC_IND_VREFINT] / 4095.0)) //#define V_REG 3.3 diff --git a/hwconf/hw_palta.h b/hwconf/hw_palta.h index f0f5e864..4f9616c0 100644 --- a/hwconf/hw_palta.h +++ b/hwconf/hw_palta.h @@ -140,6 +140,9 @@ #define GET_GATE_DRIVER_SUPPLY_VOLTAGE() ((float)ADC_VOLTS(ADC_IND_VOUT_GATE_DRV) * 11.0) #endif +#define ANGLE_TO_DAC_VALUE(angle) ( angle * 512.0 + 0x800 )//angle between -pi to pi +#define CURRENT_TO_DAC_VALUE(current) ( current * 70.0 + 0x800 )//current +#define VOLTAGE_TO_DAC_VALUE(voltage) ( voltage * 40.0 + 0x800 )//angle between -pi to pi // Double samples in beginning and end for positive current measurement. // Useful when the shunt sense traces have noise that causes offset. diff --git a/mcpwm_foc.c b/mcpwm_foc.c index f5ce25b1..98c8ce58 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -37,6 +37,7 @@ #include #include #include +#include "virtual_motor.h" // Private types typedef struct { @@ -234,6 +235,8 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) { memset((void*)&m_motor_state, 0, sizeof(motor_state_t)); memset((void*)&m_samples, 0, sizeof(mc_sample_t)); + virtual_motor_init(); + #ifdef HW_HAS_3_SHUNTS m_curr2_sum = 0; #endif @@ -688,6 +691,23 @@ void mcpwm_foc_set_openloop_phase(float current, float phase) { } } +/** + * Set current offsets values, + * this is used by the virtual motor to set the previously saved offsets back, + * when it is disconnected + */ +void mcpwm_foc_set_current_offsets(volatile int curr0_offset, + volatile int curr1_offset, + volatile int curr2_offset){ + m_curr0_offset = curr0_offset; + m_curr1_offset = curr1_offset; +#ifdef HW_HAS_3_SHUNTS + m_curr2_offset = curr2_offset; +#else + (void)curr2_offset; +#endif +} + /** * Produce an openloop rotating voltage. * @@ -771,6 +791,22 @@ float mcpwm_foc_get_sampling_frequency_now(void) { #endif } +/** + * Returns Ts used for virtual motor sync + */ +float mcpwm_foc_get_ts(void){ + +#ifdef HW_HAS_PHASE_SHUNTS + if (m_conf->foc_sample_v0_v7) { + return (1.0 / m_conf->foc_f_sw) ; + } else { + return (1.0 / (m_conf->foc_f_sw / 2.0)); + } +#else + return (1.0 / m_conf->foc_f_sw) ; +#endif + +} /** * Calculate the current RPM of the motor. This is a signed value and the sign * depends on the direction the motor is rotating in. Note that this value has @@ -988,6 +1024,21 @@ float mcpwm_foc_get_vq(void) { return m_motor_state.vq; } +/** + * Get current offsets, + * this is used by the virtual motor to save the current offsets, + * when it is connected + */ +void mcpwm_foc_get_current_offsets(volatile int *curr0_offset, volatile int *curr1_offset, volatile int *curr2_offset){ + *curr0_offset = m_curr0_offset; + *curr1_offset = m_curr1_offset; +#ifdef HW_HAS_3_SHUNTS + *curr2_offset = m_curr2_offset; +#else + *curr2_offset = 0; +#endif +} + /** * Measure encoder offset and direction. * @@ -1588,6 +1639,8 @@ void mcpwm_foc_tim_sample_int_handler(void) { if (m_init_done) { // Generate COM event here for synchronization TIM_GenerateEvent(TIM1, TIM_EventSource_COM); + + virtual_motor_int_handler(m_motor_state.v_alpha, m_motor_state.v_beta); } } @@ -1778,7 +1831,12 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { float enc_ang = 0; if (encoder_is_configured()) { - enc_ang = encoder_read_deg(); + if(virtual_motor_is_connected()){ + enc_ang = virtual_motor_get_angle_deg(); + }else{ + enc_ang = encoder_read_deg(); + } + float phase_tmp = enc_ang; if (m_conf->foc_encoder_inverted) { phase_tmp = 360.0 - phase_tmp; @@ -2424,8 +2482,10 @@ static void control_current(volatile motor_state_t *state_m, float dt) { svm(-mod_alpha, -mod_beta, top, &duty1, &duty2, &duty3, (uint32_t*)&state_m->svm_sector); TIMER_UPDATE_DUTY(duty1, duty2, duty3); - if (!m_output_on) { - start_pwm_hw(); + if(virtual_motor_is_connected() == false){//do not allow to turn on PWM outputs if virtual motor is used + if (!m_output_on) { + start_pwm_hw(); + } } } diff --git a/mcpwm_foc.h b/mcpwm_foc.h index 2cccba4a..64f50fff 100644 --- a/mcpwm_foc.h +++ b/mcpwm_foc.h @@ -76,6 +76,10 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind); bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table); void mcpwm_foc_print_state(void); float mcpwm_foc_get_last_adc_isr_duration(void); +void mcpwm_foc_get_current_offsets(volatile int *curr0_offset, volatile int *curr1_offset, volatile int *curr2_offset); +void mcpwm_foc_set_current_offsets(volatile int curr0_offset, volatile int curr1_offset, volatile int curr2_offset); +float mcpwm_foc_get_ts(void); +void mcpwm_foc_reset_vd_vq(void); // Interrupt handlers void mcpwm_foc_tim_sample_int_handler(void); diff --git a/virtual_motor.c b/virtual_motor.c new file mode 100644 index 00000000..b20629ad --- /dev/null +++ b/virtual_motor.c @@ -0,0 +1,395 @@ +/* + Copyright 2019 Maximiliano Cordoba mcordoba@powerdesigns.ca + + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#include "virtual_motor.h" +#include "terminal.h" +#include "mc_interface.h" +#include "mcpwm_foc.h" +#include "utils.h" +#include "math.h" +#include "stdio.h" +#include "commands.h" +#include "encoder.h" + +typedef struct{ + //constant variables + float Ts; //Sample Time in s + float Rs; //Stator Resistance Phase to Neutral in Ohms + float Ld; //Inductance in d-Direction in H + float Lq; //Inductance in q-Direction in H + float Psi; //Rotorflux coupling in Vs/rad + float J; //Rotor/Load Inertia in Nm*s^2 + int v_max_adc; //max voltage that ADC can measure + + //non constant variables + float id; //Current in d-Direction in A + float iq; //Current in q-Direction in A + float me; //Electrical Torque in Nm + float w; //Electrical Angular Velocity in rad/s + float phi; //Electrical Rotor Angle in rad + float phi_observer; + float sin_phi; + float cos_phi; + bool connected; //true => connected; false => disconnected; + float tsj; // Ts / J; + float ml; //load torque + float v_alpha; //alpha axis voltage in Volts + float v_beta; //beta axis voltage in Volts + float va; //phase a voltage in Volts + float vb; //phase b voltage in Volts + float vc; //phase c voltage in Volts + float vd; //d axis voltage in Volts + float vq; //q axis voltage in Volts + float i_alpha; //alpha axis current in Amps + float i_beta; //beta axis current in Amps + float ia; //phase a current in Amps + float ib; //phase b current in Amps + float ic; //phase c current in Amps +}virtual_motor_t; + +static volatile virtual_motor_t virtual_motor; +static volatile int m_curr0_offset_backup; +static volatile int m_curr1_offset_backup; +static volatile int m_curr2_offset_backup; + +//private functions +static void connect_virtual_motor(float ml, float J, float Ld, float Lq, + float Rs,float lambda,float Vbus); +static void disconnect_virtual_motor(void); +static inline void run_virtual_motor_electrical(float v_alpha, float v_beta); +static inline void run_virtual_motor_mechanics(float ml); +static inline void run_virtual_motor(float v_alpha, float v_beta, float ml); +static inline void run_virtual_motor_park_clark_inverse( void ); +static void terminal_cmd_connect_virtual_motor(int argc, const char **argv); +static void terminal_cmd_disconnect_virtual_motor(int argc, const char **argv); + +//Public Functions + +/** + * Virtual motor initialization + */ +void virtual_motor_init(void){ + //virtual motor variables init + virtual_motor.connected = false; //disconnected + + virtual_motor.me = 0.0; + virtual_motor.va = 0.0; + virtual_motor.vb = 0.0; + virtual_motor.vc = 0.0; + virtual_motor.ia = 0.0; + virtual_motor.ib = 0.0; + virtual_motor.ic = 0.0; + virtual_motor.w = 0.0; + virtual_motor.v_alpha = 0.0; + virtual_motor.v_beta = 0.0; + virtual_motor.i_alpha = 0.0; + virtual_motor.i_beta = 0.0; + virtual_motor.id = 0.0; + virtual_motor.iq = 0.0; + + // Register terminal callbacks used for virtual motor setup + terminal_register_command_callback( + "connect_virtual_motor", + "connects virtual motor", + "[ml][J][Ld][Lq][Rs][lambda][Vbus]", + terminal_cmd_connect_virtual_motor); + + terminal_register_command_callback( + "disconnect_virtual_motor", + "disconnect virtual motor", + 0, + terminal_cmd_disconnect_virtual_motor); +} + +/** + * Virtual motor interrupt handler + */ +void virtual_motor_int_handler(float v_alpha, float v_beta){ + if(virtual_motor.connected){ + run_virtual_motor(v_alpha, v_beta, virtual_motor.ml ); + mcpwm_foc_adc_int_handler( NULL, 0); + } +} + +bool virtual_motor_is_connected(void){ + return virtual_motor.connected; +} + +float virtual_motor_get_angle_deg(void){ + return (virtual_motor.phi * 180.0 / M_PI); +} + +//Private Functions + +/** + * void connect_virtual_motor( ) + * + * -disconnects TIM8 trigger to the ADC: + * mcpwm_foc_adc_int_handler() will be called from TIM8 interrupt + * while virtual motor is connected + * -sets virtual motor parameters + * + * @param ml : torque present at motor axis in Nm + * @param J: rotor inertia Nm*s^2 + * @param Ld: inductance at d axis in Hy + * @param Lq: inductance at q axis in Hy + * @param Rs: resistance in ohms + * @param lambda: flux linkage in Vs/rad + * @param Vbus: Bus voltage in Volts + */ +static void connect_virtual_motor(float ml , float J, float Ld, float Lq, + float Rs, float lambda, float Vbus){ + if(virtual_motor.connected == false){ + //first we send 0.0 current command to make system stop PWM outputs + mcpwm_foc_set_current(0.0); + //first we disconnect the ADC triggering from TIM8_CC1 + ADC_InitTypeDef ADC_InitStructure; + + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ScanConvMode = ENABLE; + ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; + ADC_InitStructure.ADC_ExternalTrigConv = 0; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = HW_ADC_NBR_CONV; + + ADC_Init(ADC1, &ADC_InitStructure); + + //save current offsets + mcpwm_foc_get_current_offsets(&m_curr0_offset_backup, + &m_curr1_offset_backup, + &m_curr2_offset_backup); + //set current offsets to 2048 + mcpwm_foc_set_current_offsets(2048, 2048, 2048); + + //sets virtual motor variables + ADC_Value[ ADC_IND_VIN_SENS ] = Vbus * VOLTAGE_TO_ADC_FACTOR; + ADC_Value[ ADC_IND_TEMP_MOS ] = 2048; + ADC_Value[ ADC_IND_TEMP_MOTOR ] = 2048; +#ifdef HW_VERSION_PALTA + ADC_Value[ ADC_IND_VOUT_GATE_DRV ] = 1692; + // 1692 gives 15.0 as Gate Driver Voltage + //( 15.0 = (ADC_Value[] * 11.0 * 3.3) / 4096 ) +#endif + virtual_motor.phi = mcpwm_foc_get_phase() * M_PI / 180.0;// 0.0;//m_motor_state.phase; + utils_fast_sincos_better(virtual_motor.phi, (float*)&virtual_motor.sin_phi, + (float*)&virtual_motor.cos_phi); + } + + //initialize constants + virtual_motor.Ts = mcpwm_foc_get_ts(); + virtual_motor.v_max_adc = Vbus; + virtual_motor.J = J; + virtual_motor.Ld = Ld; + virtual_motor.Lq = Lq; + virtual_motor.Psi = lambda; + virtual_motor.Rs = Rs; + virtual_motor.ml = ml; + virtual_motor.tsj = virtual_motor.Ts / virtual_motor.J; + + virtual_motor.connected = true; +} + +/** + * void disconnect_virtual_motor( ) + * + * if motor is connected: + * -stop motor + * -disconnect virtual motor + * -connects TIM8 back to the trigger of the ADC peripheral + */ +static void disconnect_virtual_motor( void ){ + + if(virtual_motor.connected){ + mcpwm_foc_set_current( 0.0 ); + + //disconnect virtual motor + virtual_motor.connected = false; + + //set current offsets back + mcpwm_foc_set_current_offsets(m_curr0_offset_backup, m_curr1_offset_backup, + m_curr2_offset_backup); + + //then we reconnect the ADC triggering to TIM8_CC1 + ADC_InitTypeDef ADC_InitStructure; + + ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; + ADC_InitStructure.ADC_ScanConvMode = ENABLE; + ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; + ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Falling; + ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T8_CC1; + ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; + ADC_InitStructure.ADC_NbrOfConversion = HW_ADC_NBR_CONV; + + ADC_Init(ADC1, &ADC_InitStructure); + } +} + +/* + * Run complete Motor Model + * @param ml externally applied load torque in Nm (adidionally to the Inertia) + */ +static inline void run_virtual_motor(float v_alpha, float v_beta, float ml){ + run_virtual_motor_electrical(v_alpha, v_beta); + run_virtual_motor_mechanics(ml); + run_virtual_motor_park_clark_inverse(); +} + +/** + * Run electrical model of the machine + * + * Takes as parameters v_alpha and v_beta, + * which are outputs from the mcpwm_foc system, + * representing which voltages the controller tried to set at last step + * + * @param v_alpha alpha axis Voltage in V + * @param v_beta beta axis Voltage in V + */ +static inline void run_virtual_motor_electrical(float v_alpha, float v_beta){ + + utils_fast_sincos_better( virtual_motor.phi , (float*)&virtual_motor.sin_phi, + (float*)&virtual_motor.cos_phi ); + + virtual_motor.vd = virtual_motor.cos_phi * v_alpha + virtual_motor.sin_phi * v_beta; + virtual_motor.vq = virtual_motor.cos_phi * v_beta - virtual_motor.sin_phi * v_alpha; + + // d axis current + virtual_motor.id += (virtual_motor.vd + + virtual_motor.w * virtual_motor.Lq * virtual_motor.iq - + virtual_motor.Rs * virtual_motor.id ) + * virtual_motor.Ts / virtual_motor.Ld; + // q axis current + virtual_motor.iq += (virtual_motor.vq - + virtual_motor.w * virtual_motor.Ld * virtual_motor.id - + virtual_motor.Rs * virtual_motor.iq - + virtual_motor.w * virtual_motor.Psi ) + * virtual_motor.Ts / virtual_motor.Lq; +} + +/** + * Run mechanical side of the machine + * @param ml externally applied load torque in Nm + */ +static inline void run_virtual_motor_mechanics(float ml){ + // precalculated constants + const float _3by2 = 3.0 / 2.0; + + // electric torque + virtual_motor.me = _3by2 * (virtual_motor.Psi * virtual_motor.iq + + (virtual_motor.Lq - virtual_motor.Ld) * + virtual_motor.id * virtual_motor.iq); + + // omega + float w_aux = virtual_motor.w + virtual_motor.tsj * (virtual_motor.me - ml); + + if( w_aux < 0.0 ){ + virtual_motor.w = 0; + }else{ + virtual_motor.w = w_aux; + } + + // phi + virtual_motor.phi += virtual_motor.w * virtual_motor.Ts; + + // phi limits + while( virtual_motor.phi > M_PI ){ + virtual_motor.phi -= ( 2 * M_PI); + } + + while( virtual_motor.phi < -1.0 * M_PI ){ + virtual_motor.phi += ( 2 * M_PI); + } + +} + +/** + * Take the id and iq calculated values and translate them into ADC_Values + */ +static inline void run_virtual_motor_park_clark_inverse( void ){ + // Park Inverse + virtual_motor.i_alpha = virtual_motor.cos_phi * virtual_motor.id - + virtual_motor.sin_phi * virtual_motor.iq; + virtual_motor.i_beta = virtual_motor.cos_phi * virtual_motor.iq + + virtual_motor.sin_phi * virtual_motor.id; + + virtual_motor.v_alpha = virtual_motor.cos_phi * virtual_motor.vd - + virtual_motor.sin_phi * virtual_motor.vq; + virtual_motor.v_beta = virtual_motor.cos_phi * virtual_motor.vq + + virtual_motor.sin_phi * virtual_motor.vd; + + // Clark Inverse + virtual_motor.ia = virtual_motor.i_alpha; + virtual_motor.ib = -0.5 * virtual_motor.i_alpha + SQRT3_BY_2 * virtual_motor.i_beta; + virtual_motor.ic = -0.5 * virtual_motor.i_alpha - SQRT3_BY_2 * virtual_motor.i_beta; + + virtual_motor.va = virtual_motor.v_alpha; + virtual_motor.vb = -0.5 * virtual_motor.v_alpha + SQRT3_BY_2 * virtual_motor.v_beta; + virtual_motor.vc = -0.5 * virtual_motor.v_alpha - SQRT3_BY_2 * virtual_motor.v_beta; + + // simulate current samples + ADC_Value[ ADC_IND_CURR1 ] = virtual_motor.ia / FAC_CURRENT + 2048; + ADC_Value[ ADC_IND_CURR2 ] = virtual_motor.ib / FAC_CURRENT + 2048; +#ifdef HW_HAS_3_SHUNTS + ADC_Value[ ADC_IND_CURR3 ] = virtual_motor.ic / FAC_CURRENT + 2048; +#endif + // simulate voltage samples + ADC_Value[ ADC_IND_SENS1 ] = virtual_motor.va * VOLTAGE_TO_ADC_FACTOR + 2048; + ADC_Value[ ADC_IND_SENS2 ] = virtual_motor.vb * VOLTAGE_TO_ADC_FACTOR + 2048; + ADC_Value[ ADC_IND_SENS3 ] = virtual_motor.vc * VOLTAGE_TO_ADC_FACTOR + 2048; +} + +/** + * connect_virtual_motor command + */ +static void terminal_cmd_connect_virtual_motor(int argc, const char **argv) { + if( argc == 8 ){ + float ml; //torque load in motor axis + float Ld; //inductance in d axis + float Lq; //inductance in q axis + float J; //rotor inertia + float Rs; //resistance of motor inductance + float lambda;//rotor flux linkage + float Vbus;//Bus voltage + + sscanf(argv[1], "%f", &ml); + sscanf(argv[2], "%f", &J); + sscanf(argv[3], "%f", &Ld); + sscanf(argv[4], "%f", &Lq); + sscanf(argv[5], "%f", &Rs); + sscanf(argv[6], "%f", &lambda); + sscanf(argv[7], "%f", &Vbus); + + connect_virtual_motor( ml , J, Ld , Lq , Rs, lambda, Vbus); + commands_printf("virtual motor connected"); + } + else{ + commands_printf("arguments should be 7" ); + } +} + +/** + * disconnect_virtual_motor command + */ +static void terminal_cmd_disconnect_virtual_motor(int argc, const char **argv) { + (void)argc; + (void)argv; + + disconnect_virtual_motor(); + commands_printf("virtual motor disconnected"); + commands_printf(" "); +} diff --git a/virtual_motor.h b/virtual_motor.h new file mode 100644 index 00000000..14cc1351 --- /dev/null +++ b/virtual_motor.h @@ -0,0 +1,30 @@ +/* + Copyright 2019 Maximiliano Cordoba mcordoba@powerdesigns.ca + + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + + +#ifndef VIRTUAL_MOTOR_H_ +#define VIRTUAL_MOTOR_H_ + +#include "datatypes.h" + +void virtual_motor_init(void); +void virtual_motor_int_handler(float v_alpha, float v_beta); +bool virtual_motor_is_connected(void); +float virtual_motor_get_angle_deg(void); +#endif /* VIRTUAL_MOTOR_H_ */