From fdbff4e8a20504d4603d52cc061e5c0894512784 Mon Sep 17 00:00:00 2001 From: betzuka Date: Sun, 27 Nov 2022 11:45:42 +0000 Subject: [PATCH] Adding passive (shorted) brake to FOC --- lispBM/lispif_vesc_extensions.c | 8 ++++ motor/foc_math.h | 2 + motor/mc_interface.c | 15 +++++++ motor/mc_interface.h | 1 + motor/mcpwm_foc.c | 75 +++++++++++++++++++++++++++++++-- motor/mcpwm_foc.h | 1 + 6 files changed, 99 insertions(+), 3 deletions(-) diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c index 7f43fcd47..db1411bc0 100644 --- a/lispBM/lispif_vesc_extensions.c +++ b/lispBM/lispif_vesc_extensions.c @@ -1252,6 +1252,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(); @@ -4385,6 +4392,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); diff --git a/motor/foc_math.h b/motor/foc_math.h index 6de108238..c04de0b5d 100644 --- a/motor/foc_math.h +++ b/motor/foc_math.h @@ -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; diff --git a/motor/mc_interface.c b/motor/mc_interface.c index 7b497ce76..969b237d8 100644 --- a/motor/mc_interface.c +++ b/motor/mc_interface.c @@ -702,6 +702,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. * @@ -763,6 +777,7 @@ void mc_interface_set_handbrake(float current) { events_add("set_handbrake", current); } + /** * Set handbrake brake current relative to the minimum current limit. * diff --git a/motor/mc_interface.h b/motor/mc_interface.h index 4ff703794..e4207212a 100644 --- a/motor/mc_interface.h +++ b/motor/mc_interface.h @@ -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); diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c index 7aaed1707..eecfef6f4 100644 --- a/motor/mcpwm_foc.c +++ b/motor/mcpwm_foc.c @@ -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); @@ -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; } @@ -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; + } } /** @@ -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; } @@ -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; } @@ -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. * @@ -2653,6 +2665,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) { @@ -2766,6 +2800,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; @@ -3199,7 +3263,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) * @@ -4496,6 +4564,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); diff --git a/motor/mcpwm_foc.h b/motor/mcpwm_foc.h index 6e0750fb4..3de6b8e59 100644 --- a/motor/mcpwm_foc.h +++ b/motor/mcpwm_foc.h @@ -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);