mirror of https://github.com/rusefi/bldc.git
Merge pull request #82 from paltatech/virtual-motor
Add command to connect a virtual motor with configurable parameters
This commit is contained in:
Benjamin Vedder
GitHub
commit
8ec5723f89
1
Makefile
1
Makefile
|
|
@ -154,6 +154,7 @@ CSRC = $(STARTUPSRC) \
|
|||
confgenerator.c \
|
||||
timer.c \
|
||||
i2c_bb.c \
|
||||
virtual_motor.c \
|
||||
$(HWSRC) \
|
||||
$(APPSRC) \
|
||||
$(NRFSRC) \
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -144,6 +144,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.
|
||||
|
|
|
|||
66
mcpwm_foc.c
66
mcpwm_foc.c
|
|
@ -37,6 +37,7 @@
|
|||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,394 @@
|
|||
/*
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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 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(" ");
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#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_ */
|
||||
Loading…
Reference in New Issue