更新ects->etcs, etcs 加入init函数，设置扭矩判断是否刹车或者能量回收

Signed-off-by: huhui <huhui@sharkgulf.com>

Applications/foc/core/controller.c

@@ -12,6 +12,7 @@
 static void mot_contrl_pid(mot_contrl_t *ctrl);
 static void mot_contrl_ulimit(mot_contrl_t *ctrl);
 static void mot_contrl_rtlimit(mot_contrl_t *ctrl);
+static bool is_hw_brake_shutting_power(mot_contrl_t *ctrl);
 
 void mot_contrl_init(mot_contrl_t *ctrl) {
 	memset(ctrl, 0, sizeof(mot_contrl_t));
@@ -50,20 +51,33 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 		line_ramp_init(&ctrl->target_vq, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
 		line_ramp_init(&ctrl->target_vel, CONFIG_CRUISE_RAMP_TIME);
 		line_ramp_init(&ctrl->target_current, CONFIG_CURRENT_RAMP_TIME);
-		line_ramp_init(&ctrl->input_torque, CONFIG_DEFAULT_TORQUE_RAMP_TIME);	
+		line_ramp_init(&ctrl->input_torque, CONFIG_DEFAULT_TORQUE_RAMP_TIME);
+		etcs_init(&ctrl->etcs);
 	}
-	ctrl->mode_req = ctrl->mode_running = CTRL_MODE_OPEN;
-	ctrl->force_angle = INVALID_ANGLE;
-	ctrl->adv_angle = INVALID_ANGLE;
-	ctrl->angle_last = INVALID_ANGLE;
+	ctrl->b_ebrk_running = false;
 	ctrl->b_AutoHold = false;
 	ctrl->b_cruiseEna = false;
 	ctrl->b_mtpa_calibrate = false;
+	ctrl->b_hw_braker = false;
+
+	ctrl->mode_req = CTRL_MODE_OPEN;
+	ctrl->mode_running = CTRL_MODE_OPEN;
+
+	ctrl->force_angle = INVALID_ANGLE;
+	ctrl->adv_angle = INVALID_ANGLE;
+	ctrl->angle_last = INVALID_ANGLE;
+
 	ctrl->dc_curr_filted = 0;
+	ctrl->dc_curr_calc = 0;
 	ctrl->phase_curr_filted[0] = ctrl->phase_curr_filted[1] = 0;
 	ctrl->out_idq_filterd.d = ctrl->out_idq_filterd.q = 0;
 	ctrl->autohold_torque = 0;
 	ctrl->out_current_vec = 0;
+	ctrl->target_idq.d = 0;
+	ctrl->target_idq.q = 0;
+	ctrl->target_torque = 0;
+	ctrl->target_torque_raw = 0;
+
 	foc_init(&ctrl->foc);
 	foc_observer_init();
 	ctrl->b_start = start;
@@ -104,10 +118,10 @@ u8 mot_contrl_mode(mot_contrl_t *ctrl) {
 			line_ramp_update(&ctrl->input_torque);
 			if (preMode == CTRL_MODE_SPD) {
 				ctrl->target_torque_raw = ctrl->target_torque;
-				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque);
+				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
 			}else if (preMode == CTRL_MODE_CURRENT) {
-				ctrl->target_torque_raw = ctrl->target_torque;
-				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque);
+				ctrl->target_torque_raw = ctrl->target_current.interpolation;
+				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque_raw);
 			}
 		}else if ((preMode == CTRL_MODE_TRQ) && (ctrl->mode_running == CTRL_MODE_SPD)) {
 			PI_Controller_Reset(&ctrl->pi_vel, ctrl->target_torque);
@@ -234,6 +248,9 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 	foc_update(foc);
 
 	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US * 2;
+	if (lowpass > 1) {
+		lowpass = 1;
+	}
 	LowPass_Filter(ctrl->out_idq_filterd.d, foc->out.curr_dq.d ,lowpass);
 	LowPass_Filter(ctrl->out_idq_filterd.q, foc->out.curr_dq.q ,lowpass);
 	return true;
@@ -275,8 +292,7 @@ static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
 			ctrl->target_idq.d = 0;
 			ctrl->target_idq.q = target_current;
 		}
-	}else if ((ctrl->mode_running == CTRL_MODE_TRQ) || (ctrl->mode_running == CTRL_MODE_SPD) ||
-				(ctrl->mode_running == CTRL_MODE_EBRAKE)) {
+	}else if (ctrl->mode_running != CTRL_MODE_OPEN) {
 		motor_mpta_fw_lookup(ctrl->foc.in.mot_velocity, ctrl->target_torque, &ctrl->target_idq);
 	}
 	u32 mask = cpu_enter_critical();
@@ -329,7 +345,7 @@ static void crosszero_step_towards(float *value, float target) {
 /*called in media task */
 void mot_contrl_dq_calc(mot_contrl_t *ctrl) {
 	foc_t *foc = &ctrl->foc;
-	float etcs_out = etcs_process(&ctrl->ects);
+	float etcs_out = etcs_process(&ctrl->etcs);
 	if (ctrl->b_AutoHold) {
 		float hold_torque = min(ctrl->protlim.torque, mc_conf()->c.max_autohold_torque);
 		ctrl->pi_lock.max = hold_torque;
@@ -618,6 +634,9 @@ void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc) {
 }
 
 bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque) {
+	if (is_hw_brake_shutting_power(ctrl) && !ctrl->b_ebrk_running){
+		return false;
+	}
 	torque = fclamp(torque, -ctrl->userlim.torque, ctrl->userlim.torque);
 	line_ramp_set_target(&ctrl->input_torque, torque);
 	return true;

Applications/foc/core/controller.h

@@ -124,25 +124,14 @@ typedef struct {
 
 typedef struct{
 	bool 			b_start;
-	u8 				mode_req;
-	u8 				mode_running;
 	bool			b_ebrk_running;
 	bool			b_hw_braker;
-	foc_t 			foc;
-	etcs_t          ects;
-	PI_Controller 	pi_lock;
-	PI_Controller 	pi_power;
-	PI_Controller 	pi_vel_lim;
-	PI_Controller 	pi_vel;
-	user_limit 		userlim;
-	hw_limit   		hwlim;
-	prot_limit 		protlim;
 	bool 			b_mtpa_calibrate;
+	bool    		b_cruiseEna;
+	bool    		b_AutoHold;
 	float 			adv_angle; //dq 分配超前角
 	float 			force_angle;
 	float 			angle_last;
-	bool    		b_cruiseEna;
-	bool    		b_AutoHold;
 	float   		target_torque;
 	float           target_torque_raw; //扭矩过零点处理前的扭矩
 	dq_t			target_idq;
@@ -151,6 +140,22 @@ typedef struct{
 	u32				ebrk_ramp_time;
 	u32				torque_acc_time;
 	u32				torque_dec_time;
+	float   		dc_curr_filted;
+	float			dc_curr_calc;
+	float   		phase_curr_filted[3];
+	float           autohold_torque;
+	u8 				mode_req;
+	u8 				mode_running;
+	u8      		error;
+	foc_t 			foc;
+	etcs_t          etcs;
+	PI_Controller 	pi_lock;
+	PI_Controller 	pi_power;
+	PI_Controller 	pi_vel_lim;
+	PI_Controller 	pi_vel;
+	user_limit 		userlim;
+	hw_limit   		hwlim;
+	prot_limit 		protlim;
 	lineramp_t   	target_current;
 	lineramp_t 		cruise_vel;
 	lineramp_t 		target_vd;
@@ -160,15 +165,10 @@ typedef struct{
 	lineramp_t 		torque_lim;
 	lineramp_t 		dc_curr_lim;
 	lineramp_t		input_torque;
-	float   		dc_curr_filted;
-	float			dc_curr_calc;
-	float   		phase_curr_filted[3];
-	float           autohold_torque;
 	float 			phase_a_max;
 	float 			phase_b_max;
 	float 			phase_c_max;
 	u32 			phase_unbalance_cnt;
-	u8      		error;
 }mot_contrl_t;
 
 void mot_contrl_init(mot_contrl_t *ctrl);

Applications/foc/core/etcs.c

@@ -6,6 +6,11 @@
 #define CONFIG_ENTER_TCS_THRO  200
 #define CONFIG_EXIT_TCS_THRO  80
 
+void etcs_init(etcs_t *etcs) {
+	memset(etcs, 0, sizeof(etcs_t));
+	etcs->output = 1.0f;
+}
+
 void etcs_set_fvel(etcs_t *etcs, float vel) {
 	float vel_delta = vel - etcs->f_fvel;
 	float ts_delta = (float)get_delta_ms(etcs->n_fv_ts);

Applications/foc/core/etcs.h

@@ -12,9 +12,8 @@ typedef struct {
 	int   n_etcs_run_cnt;
 	float output;
 }etcs_t;
+void etcs_init(etcs_t *etcs);
 void etcs_set_fvel(etcs_t *etcs, float vel);//设置前轮速度
-bool etcs_is_running(etcs_t *etcs);
-void etcs_enable(etcs_t *etcs, bool enable);
 float etcs_process(etcs_t *etcs);
 
 static __INLINE bool etcs_is_running(etcs_t *etcs) {
@@ -22,7 +21,10 @@ static __INLINE bool etcs_is_running(etcs_t *etcs) {
 }
 
 static __INLINE void etcs_enable(etcs_t *etcs, bool enable) {
-	etcs->b_etcs_en = enable;
+	if (etcs->b_etcs_en != enable) {
+		etcs->b_etcs_en = enable;
+		etcs->output = 1.0f;
+	}
 }
 
 static __INLINE float etcs_get_output(etcs_t *etcs) {

Applications/foc/motor/motor.c

@@ -355,7 +355,7 @@ bool mc_start(u8 mode) {
 	target_q = 0.0f;
 #endif
 	mc_detect_vbus_mode();
-	etcs_enable(&motor.controller.ects, motor.u_set.b_tcs);
+	etcs_enable(&motor.controller.etcs, motor.u_set.b_tcs);
 	if (motor.b_lock_motor) {
 		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
@@ -562,7 +562,7 @@ void mc_enable_brkshutpower(u8 shut) {
 
 void mc_enable_tcs(bool enable) {
 	motor.u_set.b_tcs = enable;
-	etcs_enable(&motor.controller.ects, enable);
+	etcs_enable(&motor.controller.etcs, enable);
 }
 
 s16 mc_get_ebrk_torque(void) {
@@ -754,7 +754,7 @@ u16 mc_get_running_status2(void) {
 	data |= (motor.b_runStall?1:0) << 11; //是否堵转
 	data |= (mot_contrl_dccurr_is_protected(&motor.controller)?1:0) << 12; //是否欠压限制母线电流
 	data |= (mot_contrl_torque_is_protected(&motor.controller)?1:0) << 13; //是否高温限扭矩
-	data |= (etcs_is_running(&motor.controller.ects)?1:0) << 14; //电子tcs是否正在工作
+	data |= (etcs_is_running(&motor.controller.etcs)?1:0) << 14; //电子tcs是否正在工作
 	data |= (throttle_not_released_err()?1:0) << 15;
 	return data;
 }