Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding passive (shorted) brake to FOC #561

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions lispBM/lispif_vesc_extensions.c
Original file line number Diff line number Diff line change
Expand Up @@ -1298,6 +1298,13 @@ static lbm_value ext_set_brake_rel(lbm_value *args, lbm_uint argn) {
return ENC_SYM_TRUE;
}

static lbm_value ext_set_passive_brake(lbm_value *args, lbm_uint argn) {
(void)args; (void)argn;
timeout_reset();
mc_interface_set_passive_brake();
return ENC_SYM_TRUE;
}

static lbm_value ext_set_handbrake(lbm_value *args, lbm_uint argn) {
LBM_CHECK_ARGN_NUMBER(1);
timeout_reset();
Expand Down Expand Up @@ -4471,6 +4478,7 @@ void lispif_load_vesc_extensions(void) {
lbm_add_extension("set-duty", ext_set_duty);
lbm_add_extension("set-brake", ext_set_brake);
lbm_add_extension("set-brake-rel", ext_set_brake_rel);
lbm_add_extension("set-passive-brake", ext_set_passive_brake);
lbm_add_extension("set-handbrake", ext_set_handbrake);
lbm_add_extension("set-handbrake-rel", ext_set_handbrake_rel);
lbm_add_extension("set-rpm", ext_set_rpm);
Expand Down
2 changes: 2 additions & 0 deletions motor/foc_math.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@ typedef struct {
float i_beta;
float i_abs;
float i_abs_filter;
float i_abs_passive_brake;
float i_abs_passive_brake_filtered;
float i_bus;
float v_bus;
float v_alpha;
Expand Down
15 changes: 15 additions & 0 deletions motor/mc_interface.c
Original file line number Diff line number Diff line change
Expand Up @@ -704,6 +704,20 @@ void mc_interface_set_brake_current(float current) {
events_add("set_current_brake", current);
}

void mc_interface_set_passive_brake(void) {

if (mc_interface_try_input()) {
return;
}

if (motor_now()->m_conf.motor_type == MOTOR_TYPE_FOC) {
mcpwm_foc_set_passive_brake();
}

events_add("set_passive_brake", 0.0);
}


/**
* Set current relative to the minimum and maximum current limits.
*
Expand Down Expand Up @@ -765,6 +779,7 @@ void mc_interface_set_handbrake(float current) {
events_add("set_handbrake", current);
}


/**
* Set handbrake brake current relative to the minimum current limit.
*
Expand Down
1 change: 1 addition & 0 deletions motor/mc_interface.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ void mc_interface_set_current(float current);
void mc_interface_set_brake_current(float current);
void mc_interface_set_current_rel(float val);
void mc_interface_set_brake_current_rel(float val);
void mc_interface_set_passive_brake(void);
void mc_interface_set_handbrake(float current);
void mc_interface_set_handbrake_rel(float val);
void mc_interface_set_openloop_current(float current, float rpm);
Expand Down
75 changes: 72 additions & 3 deletions motor/mcpwm_foc.c
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ static void control_current(motor_all_state_t *motor, float dt);
static void update_valpha_vbeta(motor_all_state_t *motor, float mod_alpha, float mod_beta);
static void stop_pwm_hw(motor_all_state_t *motor);
static void start_pwm_hw(motor_all_state_t *motor);

static void terminal_tmp(int argc, const char **argv);
static void terminal_plot_hfi(int argc, const char **argv);
static void timer_update(motor_all_state_t *motor, float dt);
Expand Down Expand Up @@ -779,7 +780,7 @@ void mcpwm_foc_set_current(float current) {
get_motor_now()->m_iq_set = current;
get_motor_now()->m_id_set = 0;

if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) {
if (fabsf(current) < get_motor_now()->m_conf->cc_min_current && get_motor_now()->m_state != MC_STATE_FULL_BRAKE) {
return;
}

Expand All @@ -794,6 +795,10 @@ void mcpwm_foc_release_motor(void) {
get_motor_now()->m_iq_set = 0.0;
get_motor_now()->m_id_set = 0.0;
get_motor_now()->m_motor_released = true;

if (get_motor_now()->m_state == MC_STATE_FULL_BRAKE) {
get_motor_now()->m_state = MC_STATE_RUNNING;
}
}

/**
Expand All @@ -807,7 +812,7 @@ void mcpwm_foc_set_brake_current(float current) {
get_motor_now()->m_control_mode = CONTROL_MODE_CURRENT_BRAKE;
get_motor_now()->m_iq_set = current;

if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) {
if (fabsf(current) < get_motor_now()->m_conf->cc_min_current && get_motor_now()->m_state != MC_STATE_FULL_BRAKE) {
return;
}

Expand All @@ -828,7 +833,7 @@ void mcpwm_foc_set_handbrake(float current) {
get_motor_now()->m_control_mode = CONTROL_MODE_HANDBRAKE;
get_motor_now()->m_iq_set = current;

if (fabsf(current) < get_motor_now()->m_conf->cc_min_current) {
if (fabsf(current) < get_motor_now()->m_conf->cc_min_current && get_motor_now()->m_state != MC_STATE_FULL_BRAKE) {
return;
}

Expand All @@ -838,6 +843,13 @@ void mcpwm_foc_set_handbrake(float current) {
}
}

void mcpwm_foc_set_passive_brake(void) {
get_motor_now()->m_iq_set = 0.0;
get_motor_now()->m_id_set = 0.0;
get_motor_now()->m_motor_released = false;
get_motor_now()->m_state = MC_STATE_FULL_BRAKE;
}

/**
* Produce an openloop rotating current.
*
Expand Down Expand Up @@ -2671,6 +2683,28 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
ADC_curr_norm_value[2 + norm_curr_ofs] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);
#endif

if (motor_now->m_state == MC_STATE_FULL_BRAKE) {
const float i0_abs = fabsf(ADC_curr_norm_value[0 + norm_curr_ofs]);
const float i1_abs = fabsf(ADC_curr_norm_value[1 + norm_curr_ofs]);
const float i2_abs = fabsf(ADC_curr_norm_value[2 + norm_curr_ofs]);

//use max abs current through any shunt
float i_max = i0_abs;
if (i1_abs>i_max) {
i_max = i1_abs;
}
if (i2_abs>i_max) {
i_max = i2_abs;
}
motor_now->m_motor_state.i_abs_passive_brake = i_max * FAC_CURRENT;
UTILS_LP_FAST(motor_now->m_motor_state.i_abs_passive_brake_filtered, motor_now->m_motor_state.i_abs_passive_brake, 0.1);

} else {
motor_now->m_motor_state.i_abs_passive_brake = 0;
motor_now->m_motor_state.i_abs_passive_brake_filtered = 0;
}


// Use the best current samples depending on the modulation state.
#ifdef HW_HAS_3_SHUNTS
if (conf_now->foc_sample_high_current) {
Expand Down Expand Up @@ -2784,6 +2818,36 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
motor_now->m_phase_now_encoder = DEG2RAD_f(phase_tmp);
}


if (motor_now->m_state == MC_STATE_FULL_BRAKE) {
//check current limits
if (motor_now->m_motor_state.i_abs_passive_brake > conf_now->l_abs_current_max) {
//over current fault
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT, &m_motor_1 != motor_now, true);
} else {
//short all phases through low side fets
if (motor_now == &m_motor_1) {
TIMER_UPDATE_DUTY_M1(0, 0, 0);
#ifdef HW_HAS_DUAL_PARALLEL
TIMER_UPDATE_DUTY_M2(0, 0, 0);
#endif
} else {
#ifndef HW_HAS_DUAL_PARALLEL
TIMER_UPDATE_DUTY_M2(0, 0, 0);
#endif
}

// do not allow to turn on PWM outputs if virtual motor is used
if(virtual_motor_is_connected() == false) {
if (!motor_now->m_output_on) {
start_pwm_hw(motor_now);
}
}
}
}



if (motor_now->m_state == MC_STATE_RUNNING) {
// Clarke transform assuming balanced currents
motor_now->m_motor_state.i_alpha = ia;
Expand Down Expand Up @@ -3207,7 +3271,11 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
UTILS_NAN_ZERO(motor_now->m_motor_state.mod_q_filter);
UTILS_LP_FAST(motor_now->m_motor_state.mod_q_filter, motor_now->m_motor_state.mod_q, 0.2);
utils_truncate_number_abs((float*)&motor_now->m_motor_state.mod_q_filter, 1.0);


}



// Calculate duty cycle
motor_now->m_motor_state.duty_now = SIGN(motor_now->m_motor_state.vq) *
Expand Down Expand Up @@ -4504,6 +4572,7 @@ static void stop_pwm_hw(motor_all_state_t *motor) {
}
}


static void start_pwm_hw(motor_all_state_t *motor) {
if (motor == &m_motor_1) {
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1);
Expand Down
1 change: 1 addition & 0 deletions motor/mcpwm_foc.h
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ void mcpwm_foc_set_current(float current);
void mcpwm_foc_release_motor(void);
void mcpwm_foc_set_brake_current(float current);
void mcpwm_foc_set_handbrake(float current);
void mcpwm_foc_set_passive_brake(void);
void mcpwm_foc_set_openloop_current(float current, float rpm);
void mcpwm_foc_set_openloop_phase(float current, float phase);
void mcpwm_foc_set_openloop_duty(float dutyCycle, float rpm);
Expand Down