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_ */