Adding passive (shorted) brake to FOC

lispBM/lispif_vesc_extensions.c

@@ -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();
@@ -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);

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;

motor/mc_interface.c

@@ -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.
  *
@@ -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.
  *

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);

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.
  *
@@ -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) {
@@ -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;
@@ -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) *
@@ -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);

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);