foc 层重新架构

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

Applications/app/app.c

@@ -13,7 +13,6 @@
 #include "libs/time_measure.h"
 #include "app/nv_storage.h"
 #include "foc/commands.h"
-#include "foc/core/thro_torque.h"
 #include "foc/core/F_Calc.h"
 #include "foc/motor/motor_param.h"
 #include "foc/motor/mot_params_ind.h"
@@ -127,36 +126,35 @@ int plot_type = 0;
 static void plot_smo_angle(void) {
 #if 1
 	float smo_angle = foc_observer_sensorless_angle();
-	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;
+	float delta = smo_angle - motor.controller.foc.in.mot_angle;
 	float s, c;
 	arm_sin_cos_f32(delta, &s, &c);
 	delta = fast_atan2(s, c)/PI*180.0f;
-	can_plot3(PMSM_FOC_Get()->in.s_motAngle, smo_angle, delta);
+	can_plot3(motor.controller.foc.in.mot_angle, smo_angle, delta);
 #else
 	can_plot2((s16)(foc_observer_sensorless_diff() * 100.0f), (s16)(foc_observer_sensorless_ration() * 100.0f));
 #endif
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {
-		can_plot2((s16)foc_observer_sensorless_speed(), (s16)PMSM_FOC_GetSpeed());
-		//can_plot3((s16)PMSM_FOC_GetSpeed() + 1000, (s16)PMSM_FOC_GetSpeed(), (s16)PMSM_FOC_GetSpeed() - 1000);
+		can_plot2((s16)foc_observer_sensorless_speed(), (s16)mot_contrl_get_speed(&motor.controller));
 	}else if (plot_type == 2) {
-		can_plot2(eCtrl_get_FinalTorque(), PMSM_FOC_Get()->in.s_targetTorque);
+		can_plot2(mot_contrl_get_final_torque(&motor.controller), motor.controller.target_torque);
 	}else if (plot_type == 3) {
 		plot_smo_angle();
 	}else if (plot_type == 4) {
-		can_plot2((s16)PMSM_FOC_Get()->out.s_RealIdq.d, (s16)PMSM_FOC_Get()->out.s_RealIdq.q);
+		can_plot2((s16)motor.controller.foc.out.curr_dq.d, (s16)motor.controller.foc.out.curr_dq.q);
 	}else if (plot_type == 5) {
-		can_plot2((s16)PMSM_FOC_Get()->idq_ctl[0].s_Cp, (s16)PMSM_FOC_Get()->idq_ctl[1].s_Cp);
+		can_plot2((s16)motor.controller.foc.ramp_curr_dq.d , (s16)motor.controller.foc.ramp_curr_dq.d);
 	}else if (plot_type == 6) {
 		//do it in other place
 	}else if (plot_type == 7) {
 		#ifdef CONFIG_DQ_STEP_RESPONSE
-		can_plot2((s16)(target_d*10.0f), (s16)(PMSM_FOC_Get()->out.s_RealIdq.d * 10.0f));
+		can_plot2((s16)(target_d*10.0f), (s16)(motor.controller.foc.out.curr_dq.d * 10.0f));
 		#endif
 	}else if (plot_type == 8) {
 		#ifdef CONFIG_DQ_STEP_RESPONSE
-		can_plot2((s16)(target_q*10.0f), (s16)(PMSM_FOC_Get()->out.s_RealIdq.q * 10.0f));
+		can_plot2((s16)(target_q*10.0f), (s16)(motor.controller.foc.out.curr_dq.q * 10.0f));
 		#endif
 	}else if (plot_type == 9) {
 		s16 thro_v = get_throttle_float() * 100.0f;
@@ -168,7 +166,7 @@ static u32 _app_plot_task(void * args) {
 }
 static u32 _app_low_task(void *args) {
 	wdog_reload();
-	if (!PMSM_FOC_Is_Start()) {
+	if (!mot_contrl_is_start(&motor.controller)) {
 		mc_err_block_save();
 	}
 	return 1;

Applications/app/key_process.c

@@ -4,7 +4,7 @@
 #include "libs/logger.h"
 #include "libs/utils.h"
 #include "foc/commands.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 
 #define KEY_START 0
 #define KEY_STOP  1

Applications/foc/commands.c

@@ -72,7 +72,7 @@ static void process_ext_command(foc_cmd_body_t *command) {
 		u8 p_mode = decode_8bits(b0, 0, 1);
 		if (p_mode == 1) {
 			if (!mc_start(CTRL_MODE_TRQ)) {
-				mc_crit_err_add(FOC_START_Err_Code, (s16)(throttle_get_signal()*100.0f), (s16)PMSM_FOC_GetSpeed());
+				mc_crit_err_add(FOC_START_Err_Code, (s16)(throttle_get_signal()*100.0f), (s16)mot_contrl_get_speed(&motor.controller));
 			}
 		}else if (p_mode == 2) {
 			mc_stop();
@@ -143,7 +143,7 @@ static u8 ignore_with_speed[] = {Foc_Set_Adrc_Params, Foc_Set_Gear_Limit, Foc_Co
 
 static bool _can_process_with_speed(u8 cmd) {
 	int size = ARRAY_SIZE(ignore_with_speed);
-	if (!mc_is_start() || PMSM_FOC_GetSpeed() < 0.1f) {
+	if (!mc_is_start() || mot_contrl_get_speed(&motor.controller) < 0.1f) {
 		return true;
 	}
 	for (int i = 0; i < size; i++) {
@@ -178,7 +178,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				success = mc_stop();
 			}
 			if (!success) {
-				erroCode = PMSM_FOC_GetErrCode();
+				erroCode = mot_contrl_get_errcode(&motor.controller);
 			}else {
 				if (command->len > sizeof(foc_start_cmd_t)) {
 					u8 *p = (u8 *)command->data + sizeof(foc_start_cmd_t);
@@ -232,7 +232,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		{
 			u8 enable = decode_u8(command->data);
 			if (!mc_enable_cruise(enable)) {
-				erroCode = PMSM_FOC_GetErrCode();
+				erroCode = mot_contrl_get_errcode(&motor.controller);
 			}
 			
 			break;
@@ -242,7 +242,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			u8 mode = decode_u8(command->data);
 			float rpm = (float)decode_s16((u8 *)command->data + 1);
 			if (!mc_set_cruise_speed(mode?true:false, rpm)) {
-				erroCode = PMSM_FOC_GetErrCode();
+				erroCode = mot_contrl_get_errcode(&motor.controller);
 			}
 			sys_debug("Cruise RPM %d\n", (int)rpm);
 			encode_u16(response + 3, (s16)rpm);
@@ -254,9 +254,9 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			u8 mode = decode_u8(command->data);
 			sys_debug("ctl mode = %d, len = %d\n", mode, command->len);
 			if (!mc_set_foc_mode(mode)) {
-				erroCode = PMSM_FOC_GetErrCode();
+				erroCode = mot_contrl_get_errcode(&motor.controller);
 			}
-			response[len++] = PMSM_FOC_GetCtrlMode();
+			response[len++] = motor.controller.mode_req;
 			break;
 		}
 		case Foc_Set_Gear_Limit:
@@ -283,8 +283,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Set_Speed_Limit:
 		{
 			s16 speed = decode_s16(((u8 *)command->data));
-			PMSM_FOC_SpeedLimit(speed);
-			encode_u16(response + 3, (u16)PMSM_FOC_GetSpeedLimit());
+			mot_contrl_set_vel_limit(&motor.controller ,speed);
+			encode_u16(response + 3, (u16)motor.controller.userlim.mot_vel);
 			len += 2;
 			break;
 		}
@@ -292,15 +292,15 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		{
 			u16 current = decode_u16(((u8 *)command->data));
 			mc_set_idc_limit((float)current);
-			encode_u16(response + 3, (u16)PMSM_FOC_GetDCCurrLimit());
+			encode_u16(response + 3, (u16)motor.controller.userlim.dc_curr);
 			len += 2;
 			break;
 		}
 		case Foc_Set_Phase_CurrLim:
 		{
 			s16 curr = decode_s16(((u8 *)command->data));
-			PMSM_FOC_PhaseCurrLim((float)curr);
-			encode_u16(response + 3, (u16)PMSM_FOC_GetPhaseCurrLim());
+			mot_contrl_set_torque_limit(&motor.controller, (float)curr);
+			encode_u16(response + 3, (u16)motor.controller.userlim.torque);
 			len += 2;
 			break;
 		}
@@ -334,7 +334,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			s16 vd = decode_s16(((u8 *)command->data));
 			s16 vq = decode_s16(((u8 *)command->data) + 2);
 			sys_debug("set v_q %d, %d\n", vd, vq);
-			PMSM_FOC_SetOpenVdq(vd, (vq));
+			mot_contrl_set_vdq(&motor.controller, vd, vq);
 			break;
 		}
 		case Foc_Conf_Pid:
@@ -347,7 +347,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			u8 id = decode_u8((u8 *)command->data);
 			mc_conf_decode_pid(&pid, (u8 *)command->data + 1);
 			sys_debug("set id = %d, kp = %f, ki = %f, kd = %f\n", id, pid.kp, pid.ki, pid.kd);
-			PMSM_FOC_SetPid(id, pid.kp, pid.ki, pid.kd);
+			mot_contrl_set_pid(&motor.controller, id, pid.kp, pid.ki, pid.kd);
 			mc_conf_set_pid(id, &pid);
 			break;
 		}
@@ -397,7 +397,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		{
 			bool mode = decode_u8((u8 *)command->data) == 0?false:true;
 			if (!mc_start_epm(mode)) {
-				erroCode = PMSM_FOC_GetErrCode();
+				erroCode = mot_contrl_get_errcode(&motor.controller);
 			}
 			break;
 		}
@@ -418,7 +418,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}else {
 				mc_lock_motor(false);
 			}
-			erroCode = PMSM_FOC_GetErrCode();
+			erroCode = mot_contrl_get_errcode(&motor.controller);
 			break;
 		}
 		case Foc_Auto_Hold:
@@ -429,14 +429,14 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}else {
 				mc_auto_hold(false);
 			}
-			erroCode = PMSM_FOC_GetErrCode();
+			erroCode = mot_contrl_get_errcode(&motor.controller);
 			break;
 		}
 		case Foc_Start_EPM_Move:
 		{
-			EPM_Dir_t dir = (EPM_Dir_t)decode_u8((u8 *)command->data);
+			epm_dir_t dir = (epm_dir_t)decode_u8((u8 *)command->data);
 			if(!mc_command_epm_move(dir)) {
-				erroCode = PMSM_FOC_GetErrCode();
+				erroCode = mot_contrl_get_errcode(&motor.controller);
 			}
 			break;
 		}
@@ -446,9 +446,9 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			if (start == Foc_Start) {
 				sys_debug("start mpta cali\n");
 				mc_set_foc_mode(CTRL_MODE_CURRENT);
-				PMSM_FOC_MTPA_Calibrate(true);
+				mot_contrl_mtpa_calibrate(&motor.controller, true);
 			}else {
-				PMSM_FOC_MTPA_Calibrate(false);
+				mot_contrl_mtpa_calibrate(&motor.controller, false);
 				mc_set_foc_mode(CTRL_MODE_TRQ);
 			}
 			break;
@@ -461,8 +461,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				s16 is_curr = decode_s16((u8 *)command->data);
 				s16 is_angle = decode_s16((u8 *)command->data + 2);
 				sys_debug("curr %d, angle %d\n", is_curr, is_angle);
-				PMSM_FOC_Set_Current(is_curr);
-				PMSM_FOC_Set_Dq_Angle(is_angle);
+				mot_contrl_set_current(&motor.controller, is_curr);
+				mot_contrl_set_adv_angle(&motor.controller, is_angle);
 			}
 			break;
 		}

Applications/foc/core/F_Calc.c

@@ -1,5 +1,5 @@
 #include "os/os_types.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/motor/motor.h"
 #include "libs/logger.h"
 #include "math/fast_math.h"
 #include "foc/mc_config.h"
@@ -20,7 +20,7 @@ void F_all_Calc(void) {
 		M = (float)mc_conf()->m.vehicle_w;
 		gear_ratio = (float)mc_conf()->m.gear_ratio;
 	}
-	float kmph = PMSM_FOC_GetSpeed() / gear_ratio * C * 60.0f / 1000.0f;
+	float kmph = mot_contrl_get_speed(&motor.controller) / gear_ratio * C * 60.0f / 1000.0f;
 	if (mot_vel == 0.0f) {
 		mot_vel = kmph;
 		mot_accl = 0.0f;
@@ -34,9 +34,8 @@ void F_all_Calc(void) {
 
 	F_accl = M * mot_accl;
 	F_Air = SQ(kmph) * 0.02f;
-	PMSM_FOC_Ctrl *foc = PMSM_FOC_Get();
-	F_Te = 1.5f * foc->params.n_poles * (foc->params.flux * foc->out.s_RealIdq.q + 
-					(foc->params.ld - foc->params.lq) * foc->out.s_RealIdq.q * foc->out.s_RealIdq.d) * gear_ratio / (C / (2 * PI));
+	F_Te = 1.5f * mc_conf()->m.poles * (mc_conf()->m.flux * motor.controller.foc.out.curr_dq.q + 
+					(mc_conf()->m.ld - mc_conf()->m.lq) * motor.controller.foc.out.curr_dq.q * motor.controller.foc.out.curr_dq.d) * gear_ratio / (C / (2 * PI));
 	mot_vel = kmph;
 }
 

Applications/foc/core/controller.c

@@ -0,0 +1,755 @@
+#include "controller.h"
+#include "foc/mc_config.h"
+#include "foc/foc_config.h"
+#include "foc/core/foc_observer.h"
+#include "foc/motor/motor_param.h"
+#include "foc/motor/motor.h"
+#include "foc/samples.h"
+#include "foc/core/f_calc.h"
+#include "foc/motor/current.h"
+#include "foc/mc_error.h"
+
+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);
+
+void mot_contrl_init(mot_contrl_t *ctrl) {
+	memset(ctrl, 0, sizeof(mot_contrl_t));
+	ctrl->foc.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
+	ctrl->foc.half_period = FOC_PWM_Half_Period;
+	ctrl->force_angle = INVALID_ANGLE;
+	ctrl->adv_angle = INVALID_ANGLE;
+	foc_init(&ctrl->foc);
+	ctrl->hwlim.dc_curr = CONFIG_HW_MAX_DC_CURRENT;
+	ctrl->hwlim.mot_vel = CONFIG_HW_MAX_MOTOR_RPM;
+	ctrl->hwlim.phase_curr = CONFIG_HW_MAX_PHASE_CURR;
+	ctrl->hwlim.phase_vol = CONFIG_HW_MAX_PHASE_VOL;
+	ctrl->hwlim.dc_vol      = CONFIG_HW_MAX_DC_VOLTAGE;
+	ctrl->hwlim.torque  = mc_conf()->m.max_torque; //电机的最大扭矩
+	ctrl->hwlim.fw_id = mc_conf()->m.max_fw_id;  //电池能支持的最大弱磁电流
+	ctrl->protlim.dc_curr = HW_LIMIT_NONE;
+	ctrl->protlim.torque = HW_LIMIT_NONE;
+}
+bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
+	if (ctrl->b_start == start) {
+		return true;
+	}
+	if (start) {
+		mot_contrl_pid(ctrl);
+		mot_contrl_ulimit(ctrl);
+		mot_contrl_rtlimit(ctrl);
+		line_ramp_init(&ctrl->torque_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_init(&ctrl->dc_curr_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_init(&ctrl->vel_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_init(&ctrl->cruise_vel, CONFIG_CRUISE_RAMP_TIME);
+		line_ramp_init(&ctrl->target_vd, CONFIG_FOC_VDQ_RAMP_FINAL_TIME);
+		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);	
+	}
+	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_AutoHold = false;
+	ctrl->b_cruiseEna = false;
+	ctrl->b_mtpa_calibrate = false;
+	ctrl->dc_curr_filted = 0;
+	ctrl->phase_curr_filted[0] = ctrl->phase_curr_filted[1] = 0;
+	ctrl->out_idq_filterd.d = ctrl->out_idq_filterd.q = 0;
+	foc_init(&ctrl->foc);
+	foc_observer_init();
+	ctrl->b_start = start;
+	return true;
+}
+
+bool mot_contrl_request_mode(mot_contrl_t *ctrl, u8 mode) {
+	if (mode > CTRL_MODE_EBRAKE) {
+		mot_contrl_set_error(ctrl, FOC_Param_Err);
+		return false;
+	}
+	ctrl->mode_req = mode;
+	return true;
+}
+
+u8 mot_contrl_mode(mot_contrl_t *ctrl) {	
+	u8 preMode = ctrl->mode_running;
+
+	if (!ctrl->b_start) {
+		ctrl->mode_running = CTRL_MODE_OPEN;
+	}else if (ctrl->mode_req == CTRL_MODE_OPEN) {
+		ctrl->mode_running = CTRL_MODE_OPEN;
+	}else if (ctrl->mode_req == CTRL_MODE_SPD || ctrl->b_cruiseEna){
+		ctrl->mode_running = CTRL_MODE_SPD;
+	}else if (ctrl->mode_req == CTRL_MODE_CURRENT) {
+		ctrl->mode_running = CTRL_MODE_CURRENT;
+	}else if (ctrl->mode_req == CTRL_MODE_EBRAKE) {
+		ctrl->mode_running = CTRL_MODE_EBRAKE;
+	}else {
+		if (!ctrl->b_cruiseEna) {
+			ctrl->mode_running = CTRL_MODE_TRQ;
+		}
+	}
+	if (preMode != ctrl->mode_running) {
+		if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_TRQ)) {
+			line_ramp_set_acctime(&ctrl->input_torque, ctrl->torque_acc_time);
+			line_ramp_set_dectime(&ctrl->input_torque, ctrl->torque_acc_time);
+			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);
+			}else if (preMode == CTRL_MODE_CURRENT) {
+				ctrl->target_torque_raw = ctrl->target_torque;
+				PI_Controller_Reset(&ctrl->pi_vel_lim, ctrl->target_torque);
+			}
+		}else if ((preMode == CTRL_MODE_TRQ) && (ctrl->mode_running == CTRL_MODE_SPD)) {
+			PI_Controller_Reset(&ctrl->pi_vel, ctrl->target_torque);
+		}else if ((preMode != ctrl->mode_running) && (ctrl->mode_running == CTRL_MODE_EBRAKE)) {
+			line_ramp_reset(&ctrl->input_torque, ctrl->target_torque);
+			line_ramp_set_time(&ctrl->input_torque, ctrl->ebrk_ramp_time);
+			line_ramp_set_target(&ctrl->input_torque, motor_get_ebreak_toruqe(ctrl->foc.in.mot_velocity));
+		}else if ((preMode == CTRL_MODE_EBRAKE) && (ctrl->mode_running == CTRL_MODE_SPD)) {
+			PI_Controller_Reset(&ctrl->pi_vel, F_get_air());
+		}
+	}
+	if (ctrl->mode_running == CTRL_MODE_OPEN) {
+		line_ramp_step(&ctrl->target_vd);
+		line_ramp_step(&ctrl->target_vq);
+	}
+	return ctrl->mode_running;
+}
+
+static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
+	static u32 _cycle_cnt = 0, _last_mod_cnt = 0;
+	static float a_max = 0, b_max = 0, c_max = 0;
+	static u32 _unbalance_cnt = 0;
+	static u32 _unbalance_time = 0;
+	foc_t *foc = &ctrl->foc;
+	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US / 2.0f;
+	if (lowpass > 1.0f) {
+		lowpass = 1.0f;
+	}
+	LowPass_Filter(ctrl->phase_curr_filted[0], foc->in.curr_abc[0], lowpass);
+	LowPass_Filter(ctrl->phase_curr_filted[1], foc->in.curr_abc[1], lowpass);
+	ctrl->phase_curr_filted[2] = -(ctrl->phase_curr_filted[0] + ctrl->phase_curr_filted[1]);
+	if ((ctrl->angle_last == INVALID_ANGLE) || (foc->mot_vel_radusPers < 100)) {
+		ctrl->angle_last = foc->in.mot_angle;
+		a_max = b_max = c_max = 0;
+		_unbalance_cnt = 0;
+		_unbalance_time = get_tick_ms();
+		_cycle_cnt = 0;
+		_last_mod_cnt = 0;
+		return;
+	}
+	float delta_angle = foc->in.mot_angle - ctrl->angle_last;
+	if (delta_angle > 200 || delta_angle < -200) { //one cycle
+		_cycle_cnt ++;
+	}
+	ctrl->angle_last = foc->in.mot_angle;
+	u32 mod_cnt = _cycle_cnt % CONFIG_PHASE_UNBALANCE_PEAK_CNT;
+	bool trigger = false;
+	if ((mod_cnt == 0) && (_last_mod_cnt != mod_cnt)) {
+		trigger = true;
+	}
+	_last_mod_cnt = mod_cnt;
+
+	a_max = MAX(a_max, ctrl->phase_curr_filted[0] * (2.2f));
+	b_max = MAX(b_max, ctrl->phase_curr_filted[1] * (2.2f));
+	c_max = MAX(c_max, ctrl->phase_curr_filted[2] * (2.2f));
+	if (trigger) { //经过CONFIG_PEAK_CNT个周期，已经得到peak值
+		float i_min = 1000.0f, i_max = 0;
+		if (a_max > i_max) {
+			i_max = a_max;
+		}
+		if (a_max < i_min) {
+			i_min = a_max;
+		}
+		if (b_max > i_max) {
+			i_max = b_max;
+		}
+		if (b_max < i_min) {
+			i_min = b_max;
+		}
+		if (c_max > i_max) {
+			i_max = c_max;
+		}
+		if (c_max < i_min) {
+			i_min = c_max;
+		}
+		float unbalance_r = (i_max - i_min - CONFIG_PHASE_UNBALANCE_THROLD)/(i_max + 1e-8f);
+		if (unbalance_r >= CONFIG_PHASE_UNBALANCE_R) {
+			if ((_unbalance_cnt++ >= 500) || (get_delta_ms(_unbalance_time) >= 1000*10)) {
+				if (mc_set_critical_error(FOC_CRIT_PHASE_UNBalance_Err)) {
+					mc_crit_err_add(FOC_CRIT_PHASE_UNBalance_Err, (s16)i_max, (s16)i_min);
+				}
+			}
+		}else {
+			_unbalance_cnt = 0;
+			_unbalance_time = get_tick_ms();
+		}
+		a_max = b_max = c_max = 0;
+	}
+}
+
+
+bool mot_contrl_update(mot_contrl_t *ctrl) {
+	foc_t *foc = &ctrl->foc;
+
+	phase_current_get(foc->in.curr_abc);
+	clark(foc->in.curr_abc[0], foc->in.curr_abc[1], foc->in.curr_abc[2], &foc->in.curr_ab);
+
+	foc_observer_update(foc->out.vol_albeta.a * TWO_BY_THREE, foc->out.vol_albeta.b * TWO_BY_THREE, foc->in.curr_ab.a, foc->in.curr_ab.b);
+	float enc_angle = motor_encoder_get_angle();
+	float enc_vel = motor_encoder_get_speed();
+	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
+		/* detect encoder angle error, do something here */
+		if (!foc_observer_sensorless_stable()) {
+			foc->in.mot_velocity = 0;
+			return false;
+		}
+		enc_angle = foc_observer_sensorless_angle();
+		enc_vel = foc_observer_sensorless_speed();
+	}
+	if (!ctrl->b_mtpa_calibrate && (ctrl->force_angle != INVALID_ANGLE)) {
+		foc->in.mot_angle = ctrl->force_angle;
+	}else {
+		foc->in.mot_angle = enc_angle;
+	}
+	foc->in.mot_velocity = enc_vel;
+	foc->in.dc_vol = get_vbus_float();
+	foc->in.b_openloop = ctrl->mode_running == CTRL_MODE_OPEN;
+	phase_curr_unbal_check(ctrl);
+	if (foc->in.b_openloop) {
+		foc->in.target_vol_dq.d = ctrl->target_vd.interpolation;
+		foc->in.target_vol_dq.q = ctrl->target_vq.interpolation;
+	}
+
+	foc_update(foc);
+
+	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US * 2;
+	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;
+}
+
+static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTrq) {
+	ctrl->pi_power.max = maxTrq;
+	float errRef = ctrl->dc_curr_lim.interpolation - ctrl->dc_curr_filted;
+	return PI_Controller_Run(&ctrl->pi_power, errRef);
+}
+
+static __INLINE float mot_contrl_vel_limiter(mot_contrl_t *ctrl, float maxTrq) {
+	ctrl->pi_vel_lim.max = maxTrq;
+	ctrl->pi_vel_lim.min = 0;
+
+	float err = ctrl->vel_lim.interpolation - ctrl->foc.in.mot_velocity;
+	return PI_Controller_RunVel(&ctrl->pi_vel_lim, err);
+}
+
+/* current vector or torque to dq axis current */
+static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
+	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
+		float target_current = ctrl->target_current.interpolation;
+		if (ctrl->b_mtpa_calibrate && (ctrl->adv_angle != INVALID_ANGLE)) {
+			float s, c;
+			normal_sincosf(degree_2_pi(ctrl->adv_angle + 90.0f), &s, &c);
+			ctrl->target_idq.d = target_current * c;
+			
+			if (ctrl->target_idq.d > ctrl->hwlim.fw_id) {
+				ctrl->target_idq.d = ctrl->hwlim.fw_id;
+			}else if (ctrl->target_idq.d < -ctrl->hwlim.fw_id) {
+				ctrl->target_idq.d = -ctrl->hwlim.fw_id;
+			}
+			ctrl->target_idq.q = sqrtf(SQ(target_current) - SQ(ctrl->target_idq.d));
+			if (s < 0) {
+				ctrl->target_idq.q = -ctrl->target_idq.q;
+			}
+		}else {
+			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)) {
+		motor_mpta_fw_lookup(ctrl->foc.in.mot_velocity, ctrl->target_torque, &ctrl->target_idq);
+	}
+	u32 mask = cpu_enter_critical();
+	ctrl->foc.in.target_curr_dq.d = ctrl->target_idq.d;
+	ctrl->foc.in.target_curr_dq.q = ctrl->target_idq.q;
+	cpu_exit_critical(mask);
+}
+
+static void crosszero_step_towards(float *value, float target) {
+	static float no_cro_step = CONFIG_CrossZero_NorStep;
+	float v_now = *value;
+	bool cross_zero = false;
+	float nor_step = mc_conf()->cz.normal_step;
+	float min_step = mc_conf()->cz.min_step;
+	float min_ramp_torque = mc_conf()->cz.low;
+	float high_ramp_torque = mc_conf()->cz.high;
+	if (target > 0) {
+		if (v_now < -min_ramp_torque) {
+			step_towards(value, -min_ramp_torque + 0.001f, nor_step);
+			cross_zero = true;
+		}else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
+			step_towards(value, target, min_step);
+			cross_zero = true;
+		}
+	}else if (target == 0) {
+		if (v_now > high_ramp_torque) {
+			step_towards(value, high_ramp_torque - 0.001f, nor_step);
+			cross_zero = true;
+		}else if (v_now >= min_ramp_torque && v_now <= high_ramp_torque) {
+			step_towards(value, target, min_step);
+			cross_zero = true;
+		}
+	}else {
+		if (v_now > high_ramp_torque) {
+			step_towards(value, high_ramp_torque - 0.001f, nor_step);
+			cross_zero = true;
+		}else if (v_now >= -min_ramp_torque && v_now <= high_ramp_torque) {
+			step_towards(value, target, min_step);
+			cross_zero = true;
+		}
+	}
+	if (!cross_zero) {
+		step_towards(&no_cro_step, nor_step, 0.1f);
+		step_towards(value, target, no_cro_step);
+	}else {
+		no_cro_step = 0.5f;
+	}
+}
+
+/*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);
+	if (ctrl->b_AutoHold) {
+		float hold_torque = min(ctrl->protlim.torque, mc_conf()->c.max_autohold_torque);
+		ctrl->pi_lock.max = hold_torque;
+		ctrl->pi_lock.min = -hold_torque;
+		float vel_count = motor_encoder_get_vel_count();
+		float errRef = 0 - vel_count;
+		ctrl->target_torque = PI_Controller_Run(&ctrl->pi_lock ,errRef);
+		mot_contrl_dq_assign(ctrl);
+		return;
+	}
+	if (ctrl->mode_running == CTRL_MODE_CURRENT) {
+		line_ramp_step(&ctrl->target_current);
+	}else if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
+		float maxTrq = line_ramp_step(&ctrl->input_torque);
+		if (ctrl->input_torque.target < 0.0001f && foc->in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE) {
+			maxTrq = 0;
+		}
+		crosszero_step_towards(&ctrl->target_torque, maxTrq);
+	}else if (ctrl->mode_running == CTRL_MODE_TRQ) {
+		float refTorque = line_ramp_step(&ctrl->input_torque);
+		refTorque = min(refTorque, ctrl->torque_lim.interpolation) * etcs_out;
+		float maxTrq = mot_contrl_vel_limiter(ctrl, refTorque);
+		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
+		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
+	}else if (ctrl->mode_running == CTRL_MODE_SPD){
+		float refSpeed = line_ramp_step(&ctrl->cruise_vel);
+		float maxSpeed = ctrl->target_vel.target;
+		if (ctrl->b_cruiseEna) {
+			maxSpeed = ctrl->cruise_vel.target;
+			refSpeed = ctrl->cruise_vel.interpolation;
+		}
+		float max_input = ctrl->torque_lim.interpolation * etcs_out;
+		if (maxSpeed >= 0) {
+			ctrl->pi_vel.max = max_input;
+#ifdef CONFIG_SERVO_MOTOR
+			ctrl->pi_vel.min = -max_input;
+#else
+			ctrl->pi_vel.min = -CONFIG_MAX_NEG_TORQUE;
+#endif
+		}else if (maxSpeed < 0) {
+			ctrl->pi_vel.min = -max_input;
+#ifdef CONFIG_SERVO_MOTOR
+			ctrl->pi_vel.max = max_input;
+#else
+			ctrl->pi_vel.max = CONFIG_MAX_NEG_TORQUE;
+#endif
+		}
+
+		if ((maxSpeed == 0) && (ctrl->foc.in.mot_velocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
+			ctrl->pi_vel.max = 0;
+			ctrl->pi_vel.min = 0; //防止倒转
+		}
+		float errRef = refSpeed - ctrl->foc.in.mot_velocity;
+		float maxTrq = PI_Controller_RunVel(&ctrl->pi_vel, errRef);
+		ctrl->target_torque_raw = mot_contrl_dc_curr_limiter(ctrl, maxTrq);
+		crosszero_step_towards(&ctrl->target_torque, ctrl->target_torque_raw);
+	}
+
+	mot_contrl_dq_assign(ctrl);
+}
+
+static void mot_contrl_pid(mot_contrl_t *ctrl) {
+	float slow_ctrl_ts = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+	PI_Controller_Reset(&ctrl->pi_power, 0);
+	ctrl->pi_power.kp = mc_conf()->c.pid[PID_IDCLim_ID].kp;
+	ctrl->pi_power.ki = mc_conf()->c.pid[PID_IDCLim_ID].ki;
+	ctrl->pi_power.kd = mc_conf()->c.pid[PID_IDCLim_ID].kd;
+	ctrl->pi_power.DT = slow_ctrl_ts;
+
+	PI_Controller_Reset(&ctrl->pi_lock, 0);
+	ctrl->pi_lock.kp = mc_conf()->c.pid[PID_AutoHold_ID].kp;
+	ctrl->pi_lock.ki = mc_conf()->c.pid[PID_AutoHold_ID].ki;
+	ctrl->pi_lock.kd = mc_conf()->c.pid[PID_AutoHold_ID].kd;
+	ctrl->pi_lock.DT = slow_ctrl_ts;
+
+	PI_Controller_Reset(&ctrl->pi_vel_lim, 0);
+	ctrl->pi_vel_lim.kp = mc_conf()->c.pid[PID_VelLim_ID].kp;
+	ctrl->pi_vel_lim.ki = mc_conf()->c.pid[PID_VelLim_ID].ki;
+	ctrl->pi_vel_lim.kd = mc_conf()->c.pid[PID_VelLim_ID].kd;
+	ctrl->pi_vel_lim.DT = slow_ctrl_ts;
+
+	PI_Controller_Reset(&ctrl->pi_vel, 0);
+	ctrl->pi_vel.kp = mc_conf()->c.pid[PID_Vel_ID].kp;
+	ctrl->pi_vel.ki = mc_conf()->c.pid[PID_Vel_ID].ki;
+	ctrl->pi_vel.kd = mc_conf()->c.pid[PID_Vel_ID].kd;
+	ctrl->pi_vel.DT = slow_ctrl_ts;
+}
+
+static void mot_contrl_ulimit(mot_contrl_t *ctrl) {
+	ctrl->userlim.dc_curr = min(mc_conf()->c.max_idc, ctrl->hwlim.dc_curr);
+	ctrl->userlim.mot_vel = min(mc_conf()->c.max_rpm, ctrl->hwlim.mot_vel);
+	ctrl->userlim.torque = mc_conf()->c.max_torque;//MAX_TORQUE;
+	ctrl->userlim.phase_curr = min(mc_conf()->c.max_phase_curr, ctrl->hwlim.phase_curr);
+	ctrl->userlim.dc_vol_min = mc_conf()->c.max_dc_vol;
+	ctrl->userlim.dc_vol_max = mc_conf()->c.min_dc_vol;
+	ctrl->userlim.ebrk_dc_curr = 0xFF;
+	ctrl->userlim.ebrk_torque = mc_conf()->c.max_ebrk_torque;
+}
+
+static void mot_contrl_rtlimit(mot_contrl_t *ctrl) {
+	line_ramp_reset(&ctrl->torque_lim, ctrl->userlim.torque);
+	line_ramp_reset(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
+	line_ramp_reset(&ctrl->vel_lim, ctrl->userlim.mot_vel);
+}
+
+void mot_contrl_slow_task(mot_contrl_t *ctrl) {
+	line_ramp_step(&ctrl->torque_lim);
+	line_ramp_step(&ctrl->dc_curr_lim);
+	line_ramp_step(&ctrl->vel_lim);
+	mot_contrl_dq_calc(ctrl);
+}
+
+
+u8 mot_contrl_protect(mot_contrl_t *ctrl) {
+	u8 changed = FOC_LIM_NO_CHANGE;
+	float dc_lim = (float)vbus_under_vol_limit();
+	float torque_lim = (float)min(mos_temp_high_limit(), motor_temp_high_limit());
+	
+	if (ctrl->protlim.dc_curr != dc_lim || ctrl->protlim.torque != torque_lim) {
+		if ((dc_lim > ctrl->protlim.dc_curr) || (torque_lim > ctrl->protlim.torque)) {
+			changed = FOC_LIM_CHANGE_H;
+		}else {
+			changed = FOC_LIM_CHANGE_L;
+		}
+		ctrl->protlim.dc_curr = dc_lim;
+		ctrl->protlim.torque = torque_lim;
+	}
+	return changed;
+}
+
+float mot_contrl_get_speed(mot_contrl_t *ctrl) {
+	float speed = ctrl->foc.in.mot_velocity;
+	if (!ctrl->b_start || foc_observer_is_encoder()) {
+		speed = motor_encoder_get_speed();
+	}else {
+		if (foc_observer_sensorless_stable()) {
+			speed = foc_observer_sensorless_speed();
+		}else {
+			speed = 0;
+		}
+	}
+	return speed;
+}
+
+void mot_contrl_velloop_params(mot_contrl_t *ctrl, float wcv, float b0) {
+#ifdef CONFIG_SPEED_LADRC
+	ladrc_change_b0(&gFoc_Ctrl.vel_adrc, b0);
+	ladrc_change_K(&gFoc_Ctrl.vel_adrc, wcv);
+#else
+	PI_Controller_Change_Kpi(&ctrl->pi_vel, wcv, b0);
+#endif
+}
+
+void mot_contrl_trqloop_params(mot_contrl_t *ctrl, float wcv, float b0) {
+#ifdef CONFIG_SPEED_LADRC
+	ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, b0);
+	ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, wcv);
+#else
+	PI_Controller_Change_Kpi(&ctrl->pi_vel_lim, wcv, b0);
+#endif
+}
+
+void mot_contrl_set_dccurr_limit(mot_contrl_t *ctrl, float ibusLimit) {
+	if (ibusLimit > ctrl->hwlim.dc_curr) {
+		ibusLimit = ctrl->hwlim.dc_curr;
+	}
+	if (ctrl->protlim.dc_curr != HW_LIMIT_NONE) {
+		ibusLimit = min(ibusLimit, ctrl->protlim.dc_curr);
+	}
+	ctrl->userlim.dc_curr = ibusLimit;
+	if (ABS(ctrl->dc_curr_filted) <= ibusLimit){
+		line_ramp_reset(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
+	}else {
+		line_ramp_set_target(&ctrl->dc_curr_lim, ctrl->userlim.dc_curr);
+	}
+}
+
+void mot_contrl_set_vel_limit(mot_contrl_t *ctrl, float vel) {
+	if (vel > ctrl->hwlim.mot_vel) {
+		vel = ctrl->hwlim.mot_vel;
+	}
+	ctrl->userlim.mot_vel = vel;
+	if (ABS(ctrl->foc.in.mot_velocity) <= vel) {
+		line_ramp_reset(&ctrl->vel_lim, ctrl->userlim.mot_vel);
+	}else {
+		line_ramp_set_target(&ctrl->vel_lim, ctrl->userlim.mot_vel);
+	}
+}
+
+void mot_contrl_set_torque_limit(mot_contrl_t *ctrl, float torque) {
+	if (torque > ctrl->hwlim.torque) {
+		torque = ctrl->hwlim.torque;
+	}
+	if (ctrl->protlim.torque != HW_LIMIT_NONE) {
+		torque = min(torque, ctrl->protlim.torque);
+	}
+	ctrl->userlim.torque = torque;
+	if (ABS(ctrl->target_torque) <= torque){
+		line_ramp_reset(&ctrl->torque_lim, ctrl->userlim.torque);
+	}else {
+		line_ramp_set_target(&ctrl->torque_lim, ctrl->userlim.torque);
+	}
+}
+
+
+float mot_contrl_get_ebrk_torque(mot_contrl_t *ctrl) {
+	if (!foc_observer_is_encoder()) {
+		return 0; //无感运行关闭能量回收
+	}
+	return ctrl->userlim.ebrk_torque;
+}
+
+void mot_contrl_set_ebrk_time(mot_contrl_t *ctrl, u32 time) {
+	ctrl->ebrk_ramp_time = time;
+	if (ctrl->mode_running == CTRL_MODE_EBRAKE) {
+		line_ramp_set_time(&ctrl->input_torque, time);
+		line_ramp_update(&ctrl->input_torque);
+	}
+}
+
+void mot_contrl_set_vdq(mot_contrl_t *ctrl, float vd, float vq) {
+	line_ramp_set_target(&ctrl->target_vd, vd);
+	line_ramp_set_target(&ctrl->target_vq, vq);
+}
+
+void mot_contrl_set_vdq_immediate(mot_contrl_t *ctrl, float vd, float vq) {
+	line_ramp_reset(&ctrl->target_vd, vd);
+	line_ramp_reset(&ctrl->target_vq, vq);
+}
+
+bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable) {
+	if (enable != ctrl->b_cruiseEna) {
+		float motSpd = mot_contrl_get_speed(ctrl);
+		if (enable && (motSpd < CONFIG_MIN_CRUISE_RPM)) { //
+			mot_contrl_set_error(ctrl, FOC_NowAllowed_With_Speed);
+			return false;
+		}
+		line_ramp_reset(&ctrl->cruise_vel, motSpd);
+		ctrl->b_cruiseEna = enable;
+	}
+	return true;
+}
+
+
+bool mot_contrl_resume_cruise(mot_contrl_t *ctrl) {
+	ctrl->b_cruiseEna = true;
+	line_ramp_set_time(&ctrl->cruise_vel, CONFIG_CRUISE_RAMP_TIME);
+	return true;
+}
+
+bool mot_contrl_set_cruise_speed(mot_contrl_t *ctrl, float rpm) {
+	if (ctrl->b_cruiseEna) {
+		if (rpm < CONFIG_MIN_CRUISE_RPM) {
+			rpm = CONFIG_MIN_CRUISE_RPM;
+		}
+		float vel = min(ABS(rpm),ctrl->userlim.mot_vel)*SIGN(rpm);
+		line_ramp_set_target(&ctrl->cruise_vel, vel);
+		return true;
+	}
+	mot_contrl_set_error(ctrl, FOC_NotCruiseMode);
+	return false;
+}
+
+bool mot_contrl_set_current(mot_contrl_t *ctrl, float is) {
+	is = fclamp(is, -ctrl->userlim.phase_curr, ctrl->userlim.phase_curr);
+	line_ramp_set_target(&ctrl->target_current, is);
+	return true;
+}
+
+void mot_contrl_set_torque_ramp_time(mot_contrl_t *ctrl, u32 acc, u32 dec) {
+	ctrl->torque_acc_time = acc;
+	ctrl->torque_dec_time = dec;
+	if (ctrl->mode_running == CTRL_MODE_TRQ) {
+		line_ramp_set_acctime(&ctrl->input_torque, acc);
+		line_ramp_set_dectime(&ctrl->input_torque, dec);
+		line_ramp_update(&ctrl->input_torque);
+	}
+}
+
+void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc) {
+	ctrl->torque_acc_time = acc;
+	if (ctrl->mode_running == CTRL_MODE_TRQ) {
+		line_ramp_set_acctime(&ctrl->input_torque, acc);
+		line_ramp_update(&ctrl->input_torque);
+	}
+}
+
+bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque) {
+	torque = fclamp(torque, -ctrl->userlim.torque, ctrl->userlim.torque);
+	line_ramp_set_target(&ctrl->input_torque, torque);
+	return true;
+}
+
+void mot_contrl_mtpa_calibrate(mot_contrl_t *ctrl, bool enable) {
+	if (enable) {
+		ctrl->b_mtpa_calibrate = true;
+		ctrl->adv_angle = 0;
+	}else {
+		ctrl->adv_angle = INVALID_ANGLE;
+		ctrl->b_mtpa_calibrate = false;
+	}
+}
+
+void mot_contrl_set_autohold(mot_contrl_t *ctrl, bool lock) {
+	if (ctrl->b_AutoHold != lock) {
+		motor_encoder_lock_pos(lock);
+		PI_Controller_Reset(&ctrl->pi_lock, 0);
+		if (!lock) {
+			float hold_torque = ctrl->target_torque * 1.1f;
+			if (ctrl->mode_running == CTRL_MODE_TRQ) {
+				PI_Controller_Reset(&ctrl->pi_vel_lim, hold_torque);
+			}else if (ctrl->mode_running == CTRL_MODE_SPD) {
+				PI_Controller_Reset(&ctrl->pi_vel, hold_torque);
+			}
+			line_ramp_reset(&ctrl->input_torque, hold_torque);
+			ctrl->autohold_torque = hold_torque;
+		}else {
+			ctrl->autohold_torque = 0;
+		}
+		ctrl->b_AutoHold = lock;
+	}
+}
+
+static bool is_hw_brake_shutting_power(mot_contrl_t *ctrl) {
+	return (ctrl->b_hw_braker && mc_hwbrk_can_shutpower());
+}
+
+bool mot_contrl_energy_recovery(mot_contrl_t *ctrl, bool start) {
+	bool enable = ctrl->b_ebrk_running;
+	if (mot_contrl_get_ebrk_torque(ctrl) == 0) {
+		enable = false;
+	}else if (start && ctrl->foc.in.mot_velocity >= CONFIG_MIN_RPM_FOR_EBRAKE){
+		enable = true;
+	}else if (!start && !is_hw_brake_shutting_power(ctrl)) {
+		enable = false;
+	}
+	if (enable != ctrl->b_ebrk_running) {
+		ctrl->b_ebrk_running = enable;
+		if (enable) {
+			ctrl->mode_req = CTRL_MODE_EBRAKE;
+		}else {
+			ctrl->mode_req = CTRL_MODE_TRQ;
+		}
+	}
+	return enable;
+}
+
+void mot_contrl_set_hw_brake(mot_contrl_t *ctrl, bool hw_brake) {
+	u32 mask = cpu_enter_critical();
+	if (hw_brake != ctrl->b_hw_braker) {
+		ctrl->b_hw_braker = hw_brake;
+	}
+	if (is_hw_brake_shutting_power(ctrl)) {
+		if (!ctrl->b_ebrk_running && !mot_contrl_energy_recovery(ctrl, true)) {
+			line_ramp_reset(&ctrl->input_torque, 0);
+		}
+	}
+	cpu_exit_critical(mask);
+}
+
+
+static PI_Controller *_pid(mot_contrl_t *ctrl, u8 id) {
+	PI_Controller *pi = NULL;
+	if (id == PID_ID_ID) {
+		pi = &ctrl->foc.daxis;
+	}else if (id == PID_IQ_ID) {
+		pi = &ctrl->foc.qaxis;
+	}else if (id == PID_VelLim_ID) {
+		pi = &ctrl->pi_vel_lim;
+	}else if (id == PID_Vel_ID) {
+		pi = &ctrl->pi_vel;
+	}else if (id == PID_AutoHold_ID) {
+		pi = &ctrl->pi_lock;
+	}
+	return pi;
+}
+
+void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd) {
+	if (id > PID_Max_ID) {
+		return;
+	}
+	PI_Controller *pi = _pid(ctrl, id);
+	if (pi != NULL) {
+		pi->kp = kp;
+		pi->ki = ki;
+		pi->kd = kd;
+	}
+}
+
+void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *kd) {
+	if (id > PID_Max_ID) {
+		return;
+	}
+	PI_Controller *pi = _pid(ctrl, id);
+	if (pi != NULL) {
+		*kp = pi->kp;
+		*ki = pi->ki;
+		*kd = pi->kd;
+	}
+}
+
+void mot_contrl_calc_current(mot_contrl_t *ctrl) {
+	float vd = ctrl->foc.out.vol_dq.d;
+	float vq = ctrl->foc.out.vol_dq.q;
+
+	float id = ctrl->out_idq_filterd.d;
+	float iq = ctrl->out_idq_filterd.q;
+    /*
+		根据公式（等幅值变换，功率不等）:
+		iDC x vDC = 3/2(iq x vq + id x vd);
+	*/
+	float m_pow = (vd * id + vq * iq);
+	float raw_idc = 0.0f;
+	float v_dc = get_vbus_float();
+	if (v_dc != 0.0f) {
+		raw_idc = m_pow / v_dc;
+	}
+	LowPass_Filter(ctrl->dc_curr_calc, raw_idc, 0.02f);
+
+	raw_idc = get_vbus_current();
+	if (raw_idc != NO_VALID_CURRENT) {
+		LowPass_Filter(ctrl->dc_curr_filted, raw_idc, 0.05f);
+	}else {
+		ctrl->dc_curr_filted = ctrl->dc_curr_calc;
+	}
+	ctrl->out_current_vec = sqrtf(SQ(id) + SQ(iq));
+}
+
+

Applications/foc/core/controller.h

@@ -0,0 +1,350 @@
+#ifndef _CONTROLLER_H__
+#define _CONTROLLER_H__
+#include "math/fix_math.h"
+#include "foc/core/PI_Controller.h"
+#include "foc/core/foc.h"
+#include "foc/core/etcs.h"
+#include "foc/limit.h"
+
+#define CTRL_MODE_OPEN                 ((u8)0U)
+#define CTRL_MODE_SPD                  ((u8)1U)
+#define CTRL_MODE_TRQ                  ((u8)2U)
+#define CTRL_MODE_CURRENT              ((u8)3U)
+#define CTRL_MODE_EBRAKE               ((u8)4U)
+
+#define FOC_LIM_NO_CHANGE  0
+#define FOC_LIM_CHANGE_H   1
+#define FOC_LIM_CHANGE_L   2
+
+#define SECTOR_1  0u
+#define SECTOR_2  1u
+#define SECTOR_3  2u
+#define SECTOR_4  3u
+#define SECTOR_5  4u
+#define SECTOR_6  5u
+#define SECTOR_UKNOW 0xFF
+
+typedef enum {
+	EPM_Dir_None,
+	EPM_Dir_Back,
+	EPM_Dir_Forward,
+}epm_dir_t;
+
+typedef enum {
+	FOC_Success = 0,
+	FOC_NotAllowed = 1,
+	FOC_Have_CritiCal_Err,
+	FOC_Throttle_Err, //ready的时候检测到转把信号
+	FOC_NowAllowed_With_Speed,
+	FOC_Speed_TooLow,
+	FOC_NotCruiseMode,
+	FOC_Param_Err,
+	FOC_MEM_Err,
+	FOC_CRC_Err,
+	FOC_Unknow_Cmd,
+}error_code_t;
+
+typedef enum {
+	FOC_CRIT_OV_Vol_Err,
+	FOC_CRIT_UN_Vol_Err,
+	FOC_CRIT_ACC_OV_Err,
+	FOC_CRIT_ACC_Un_Err,
+	FOC_CRIT_Phase_Err,
+	FOC_CRIT_Encoder_Err, /* 编码器错误，可能还是可以骑行，取决无感是否稳定 */
+	FOC_CRIT_Angle_Err, /* FOC 角度错误，一般发生在编码器错误，同时无感没有稳定的情况下，必须要停机 */
+	FOC_CRIT_CURR_OFF_Err,
+	FOC_CRIT_H_MOS_Err,
+	FOC_CRIT_L_MOS_Err,
+	FOC_CRIT_Phase_Conn_Err,
+	FOC_CRIT_MOTOR_TEMP_Lim,
+	FOC_CRIT_MOS_TEMP_Lim,
+	FOC_CRIT_Fan_Err,
+	FOC_CRIT_IDC_OV,
+	FOC_CRIT_THRO_Err,
+	FOC_CRIT_ENC_AB_Err,
+	FOC_CRIT_Vol_HW_Err, //17
+	FOC_CRIT_PHASE_UNBalance_Err, /* 三相不平衡错误，比如相线螺丝松了 */
+	FOC_CRIT_THRO2_Err,
+	FOC_CRIT_MOT_TEMP_Sensor,
+	FOC_CRIT_MOS_TEMP_Sensor,  //21
+	FOC_CRIT_BRK_Err,
+	FOC_CRIT_Err_Max = 32,
+}mctrl_critical_ebit_t;
+
+#define FOC_Cri_Err_Mask(err) (1<<(err))
+
+typedef enum {
+	FOC_EV_MOT_Limit_L=FOC_CRIT_Err_Max + 1,
+	FOC_EV_MOS_Limit_L,
+	FOC_EV_THRO_START_V,
+	FOC_START_Err_Code,
+}ctrl_event_r_t;
+
+
+typedef struct {
+	float mot_vel;
+	float torque;
+	float dc_curr;
+	float phase_curr; //最大相电流
+	float ebrk_dc_curr; //最大母线回收电流
+	float ebrk_torque;
+	float dc_vol_min;
+	float dc_vol_max;
+}user_limit;
+
+typedef struct {
+	float target;
+	float interpolation;
+	float step;
+	float time;
+	float time_dec;
+	bool  b_force_step;
+}lineramp_t;
+
+typedef struct {
+	lineramp_t vel;
+	lineramp_t torque;
+	lineramp_t dc_curr;
+}rt_lim_ramper;
+
+typedef struct {
+	float mot_vel;
+	float phase_curr;
+	float phase_vol;
+	float fw_id; //D轴最大退磁电流
+	float dc_curr;
+	float dc_vol;
+	float torque;
+}hw_limit;
+
+typedef struct {
+	float dc_curr;
+	float torque;
+}prot_limit;
+
+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;
+	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;
+	dq_t            out_idq_filterd;
+	float 			out_current_vec;
+	u32				ebrk_ramp_time;
+	u32				torque_acc_time;
+	u32				torque_dec_time;
+	lineramp_t   	target_current;
+	lineramp_t 		cruise_vel;
+	lineramp_t 		target_vd;
+	lineramp_t 		target_vq;
+	lineramp_t		target_vel;
+	lineramp_t 		vel_lim;
+	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);
+bool mot_contrl_enable(mot_contrl_t *ctrl, bool start);
+bool mot_contrl_update(mot_contrl_t *ctrl);
+u8   mot_contrl_mode(mot_contrl_t *ctrl);
+bool mot_contrl_request_mode(mot_contrl_t *ctrl, u8 mode);
+void mot_contrl_slow_task(mot_contrl_t *ctrl);
+u8   mot_contrl_protect(mot_contrl_t *ctrl);
+float mot_contrl_get_speed(mot_contrl_t *ctrl);
+void mot_contrl_velloop_params(mot_contrl_t *ctrl, float wcv, float b0);
+void mot_contrl_trqloop_params(mot_contrl_t *ctrl, float wcv, float b0);
+void mot_contrl_set_dccurr_limit(mot_contrl_t *ctrl, float ibusLimit);
+void mot_contrl_set_vel_limit(mot_contrl_t *ctrl, float vel);
+void mot_contrl_set_torque_limit(mot_contrl_t *ctrl, float torque);
+float mot_contrl_get_ebrk_torque(mot_contrl_t *ctrl);
+void mot_contrl_set_ebrk_time(mot_contrl_t *ctrl, u32 time);
+void mot_contrl_set_vdq(mot_contrl_t *ctrl, float vd, float vq);
+void mot_contrl_set_vdq_immediate(mot_contrl_t *ctrl, float vd, float vq);
+bool mot_contrl_set_cruise(mot_contrl_t *ctrl, bool enable);
+bool mot_contrl_resume_cruise(mot_contrl_t *ctrl);
+bool mot_contrl_set_cruise_speed(mot_contrl_t *ctrl, float rpm);
+bool mot_contrl_set_current(mot_contrl_t *ctrl, float is);
+void mot_contrl_set_torque_ramp_time(mot_contrl_t *ctrl, u32 acc, u32 dec);
+void mot_contrl_set_torque_acc_time(mot_contrl_t *ctrl, u32 acc);
+bool mot_contrl_set_torque(mot_contrl_t *ctrl, float torque);
+void mot_contrl_mtpa_calibrate(mot_contrl_t *ctrl, bool enable);
+void mot_contrl_set_autohold(mot_contrl_t *ctrl, bool lock);
+bool mot_contrl_energy_recovery(mot_contrl_t *ctrl, bool start);
+void mot_contrl_set_hw_brake(mot_contrl_t *ctrl, bool hw_brake);
+void mot_contrl_calc_current(mot_contrl_t *ctrl);
+void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *kd);
+void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd);
+
+static __INLINE void line_ramp_set_target(lineramp_t *line, float target) {
+	if (line->target != target) {
+		line->target = target;
+		if (!line->b_force_step) {
+			float delta = (line->target - line->interpolation);
+			if (delta >= 0) {
+				line->step = delta / line->time;
+			}else {
+				line->step = -delta / line->time_dec;
+			}
+		}
+	}
+}
+
+static __INLINE bool mot_contrl_start(mot_contrl_t *ctrl, u8 mode) {
+	mot_contrl_enable(ctrl, true);
+	return mot_contrl_request_mode(ctrl, mode);
+}
+
+static __INLINE bool mot_contrl_stop(mot_contrl_t *ctrl) {
+	return mot_contrl_enable(ctrl, false);
+}
+
+static __INLINE bool mot_contrl_is_start(mot_contrl_t *ctrl) {
+	return ctrl->b_start;
+}
+static __INLINE bool mot_contrl_dccurr_is_protected(mot_contrl_t *ctrl) {
+	return (ctrl->protlim.dc_curr != HW_LIMIT_NONE);
+}
+
+static __INLINE bool mot_contrl_torque_is_protected(mot_contrl_t *ctrl) {
+	return (ctrl->protlim.torque != HW_LIMIT_NONE);
+}
+
+static __INLINE void mot_contrl_set_ebrk_torquer(mot_contrl_t *ctrl, s16 torque) {
+	ctrl->userlim.ebrk_torque = torque;
+}
+
+static __INLINE void mot_contrl_set_error(mot_contrl_t *ctrl, u8 error) {
+	ctrl->error = error;
+}
+
+static __INLINE u8 mot_contrl_get_errcode(mot_contrl_t *ctrl) {
+	return ctrl->error;
+}
+
+static __INLINE void mot_contrl_pause_cruise(mot_contrl_t *ctrl) {
+	ctrl->b_cruiseEna = false;
+}
+
+static __INLINE bool mot_contrl_is_cruise_enabled(mot_contrl_t *ctrl) {
+	return (ctrl->b_cruiseEna && (ctrl->mode_running == CTRL_MODE_SPD));
+}
+
+static __INLINE bool mot_contrl_set_target_vel(mot_contrl_t *ctrl, float vel) {
+	if (ctrl->b_cruiseEna) {
+		return false;
+	}
+	line_ramp_set_target(&ctrl->target_vel ,min(ABS(vel), ctrl->userlim.mot_vel)*SIGN(vel));
+	return true;
+}
+
+static __INLINE void mot_contrl_set_angle(mot_contrl_t *ctrl, float angle) {
+	ctrl->force_angle = (angle);
+}
+
+static __INLINE void mot_contrl_set_adv_angle(mot_contrl_t *ctrl, float angle) {
+	ctrl->adv_angle = (angle);
+}
+
+static __INLINE bool mot_contrl_is_auto_holdding(mot_contrl_t *ctrl) {
+	return ctrl->b_AutoHold;
+}
+
+static __INLINE float mot_contrl_get_current_vector(mot_contrl_t *ctrl) {
+	return ctrl->out_current_vec;
+} 
+
+static __INLINE float mot_contrl_get_dc_current(mot_contrl_t *ctrl) {
+	return ctrl->dc_curr_filted;
+} 
+
+static __INLINE bool mot_contrl_ebrk_is_enabled(mot_contrl_t *ctrl) {
+	return mot_contrl_get_ebrk_torque(ctrl) != 0;
+}
+
+static __INLINE bool mot_contrl_ebrk_is_running(mot_contrl_t *ctrl) {
+	return ctrl->mode_running == CTRL_MODE_EBRAKE;
+}
+
+static __INLINE float mot_contrl_get_final_torque(mot_contrl_t *ctrl) {
+	return ctrl->input_torque.target;
+}
+static __INLINE void line_ramp_init(lineramp_t *line, float time) {
+	line->target = 0;
+	line->interpolation = 0;
+	line->step = 0;
+	line->b_force_step = false;
+	line->time_dec = line->time = time;
+}
+
+static __INLINE void line_ramp_reset(lineramp_t *line, float target) {
+	line->target = target;
+	line->interpolation = target;
+}
+
+
+static __INLINE void line_ramp_set_time(lineramp_t *line, float time) {
+	line->time_dec = line->time = time;
+	line->b_force_step = false;
+}
+
+static __INLINE void line_ramp_set_dectime(lineramp_t *line, float time) {
+	line->time_dec = time;
+	line->b_force_step = false;
+}
+
+static __INLINE void line_ramp_set_acctime(lineramp_t *line, float time) {
+	line->time = time;
+	line->b_force_step = false;
+}
+
+static __INLINE void line_ramp_set_step(lineramp_t *line, float step) {
+	line->step = step;
+	line->b_force_step = true;
+}
+
+static __INLINE void line_ramp_update(lineramp_t *line) {
+	if (!line->b_force_step) {
+		float delta = (line->target - line->interpolation);
+		if (delta >= 0) {
+			line->step = delta / line->time;
+		}else {
+			line->step = -delta / line->time_dec;
+		}
+	}
+}
+
+static __INLINE float line_ramp_step(lineramp_t *line) {
+	step_towards(&line->interpolation, line->target, line->step);
+	return line->interpolation;
+}
+
+#endif /* _CONTROLLER_H__ */
+

Applications/foc/core/etcs.c

@@ -1,69 +1,50 @@
 #include "foc/motor/motor.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "foc/core/etcs.h"
 #include "math/fast_math.h"
 
-static bool b_etcs_en = true;
-static bool b_etcs_running = false;
-static float f_fvel = 0, f_torque_ref, f_torque_tcs, f_acc = 0.0f;
-static u32   n_fv_ts = 0;
-static int  n_etcs_run_cnt = 0;
 #define CONFIG_ENTER_TCS_THRO  200
 #define CONFIG_EXIT_TCS_THRO  80
-void etcs_set_fvel(float vel) {
-	float vel_delta = vel - f_fvel;
-	float ts_delta = (float)get_delta_ms(n_fv_ts);
-	f_fvel = vel;
-	n_fv_ts = get_tick_ms();
-	float acc = vel_delta / ts_delta;
-	f_acc = LowPass_Filter(f_acc, acc, 0.2f);
-}
-void etcs_set_torque(float torque) {
-	f_torque_ref = torque;	
-}
-bool etcs_is_running(void) {
-	return b_etcs_running;
-}
-void etcs_enable(bool enable) {
-	b_etcs_en = enable;
-}
 
-void etcs_reset_torque(float torque) {
-	f_torque_ref = torque;
-	eCtrl_reset_Torque(torque);
+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);
+	etcs->f_fvel = vel;
+	etcs->n_fv_ts = get_tick_ms();
+	float acc = vel_delta / ts_delta;
+	etcs->f_acc = LowPass_Filter(etcs->f_acc, acc, 0.2f);
 }
 
-void etcs_process(void) {
-	if (!b_etcs_en) {
-		PMSM_FOC_Set_Torque(f_torque_ref);
-		return;
+float etcs_process(etcs_t *etcs) {
+	if (!etcs->b_etcs_en) {
+		etcs->n_etcs_run_cnt = 0;
+		etcs->output = 1.0f;
+		return etcs->output;
 	}
-	float mot_vel = PMSM_FOC_GetSpeed();
-	float f_vel = f_fvel + f_acc * (float)get_delta_ms(n_fv_ts);
-	if (!b_etcs_running && (mot_vel >= (f_vel + CONFIG_ENTER_TCS_THRO))) {
-		b_etcs_running = true;
-	}else if (b_etcs_running && (mot_vel < (f_vel + CONFIG_EXIT_TCS_THRO))) {
-		b_etcs_running = false;
+	float mot_vel = mot_contrl_get_speed(&motor.controller);
+	float f_vel = etcs->f_fvel + etcs->f_acc * (float)get_delta_ms(etcs->n_fv_ts);
+	if (!etcs->b_etcs_running && (mot_vel >= (f_vel + CONFIG_ENTER_TCS_THRO))) {
+		etcs->b_etcs_running = true;
+	}else if (etcs->b_etcs_running && (mot_vel < (f_vel + CONFIG_EXIT_TCS_THRO))) {
+		etcs->b_etcs_running = false;
 	}
-	if (b_etcs_running) {
-		if (f_torque_tcs > 0) {
-			n_etcs_run_cnt ++;
-			f_torque_tcs = f_torque_ref - n_etcs_run_cnt * 1.0f;
-			if (f_torque_tcs < 0) {
-				f_torque_tcs = 0;
+	if (etcs->b_etcs_running) {
+		if (etcs->output > 0) {
+			etcs->n_etcs_run_cnt ++;
+			etcs->output = etcs->output - 0.01f;
+			if (etcs->output < 0) {
+				etcs->output = 0;
 			}
 		}
 	}else {
-		if (n_etcs_run_cnt <= 0) {
-			f_torque_tcs = f_torque_ref;
-		}else {
-			n_etcs_run_cnt--;
-			f_torque_tcs = f_torque_tcs + 1.0f;
-			if (n_etcs_run_cnt <= 0 || f_torque_tcs >= f_torque_ref) {
-				n_etcs_run_cnt = 0;
-				f_torque_tcs = f_torque_ref;
-			}
+		if (etcs->n_etcs_run_cnt > 0) {
+			etcs->n_etcs_run_cnt--;
+			etcs->output = etcs->output + 0.01f;
+			if (etcs->n_etcs_run_cnt <= 0 || etcs->output >= 1.0f) {
+				etcs->n_etcs_run_cnt = 0;
+				etcs->output = 1.0f;
+			}		
 		}
 	}
-	PMSM_FOC_Set_Torque(f_torque_tcs);
+	return etcs->output;
 }

Applications/foc/core/etcs.h

@@ -1,10 +1,33 @@
 #ifndef _ETSC_H__
 #define _ETSC_H__
-void etcs_set_fvel(float vel);//设置前轮速度
-void etcs_set_torque(float torque);
-bool etcs_is_running(void);
-void etcs_enable(bool enable);
-void etcs_process(void);
-void etcs_reset_torque(float torque);
+
+typedef struct {
+	bool b_etcs_en;
+	bool b_etcs_running;
+	float f_fvel;
+	float f_torque_ref;
+	float f_torque_tcs;
+	float f_acc;
+	u32	  n_fv_ts;
+	int   n_etcs_run_cnt;
+	float output;
+}etcs_t;
+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) {
+	return etcs->b_etcs_running;
+}
+
+static __INLINE void etcs_enable(etcs_t *etcs, bool enable) {
+	etcs->b_etcs_en = enable;
+}
+
+static __INLINE float etcs_get_output(etcs_t *etcs) {
+	return etcs->output;
+}
+
 #endif /* _ETSC_H__ */
 

Applications/foc/core/foc.c

@@ -0,0 +1,134 @@
+#include "foc.h"
+#include "arm_math.h"
+#include "math/fix_math.h"
+#include "math/fast_math.h"
+#include "foc/core/svpwm.h"
+#include "libs/logger.h"
+#include "bsp/bsp_driver.h"
+#include "foc/mc_config.h"
+#include "foc/foc_config.h"
+#include "foc/motor/current.h"
+
+static __INLINE void rev_park(foc_t *foc, dq_t *dq, albt_t *alpha_beta) {
+	float c,s;
+
+	s = foc->sin;
+	c = foc->cos;
+
+	alpha_beta->a = dq->d * c - dq->q * s;
+	alpha_beta->b = dq->d * s + dq->q * c;
+}
+
+
+static __INLINE void park(foc_t *foc, albt_t *alpha_beta, dq_t *dq) {
+	float c,s;
+
+	s = foc->sin;
+	c = foc->cos;
+
+	dq->d = alpha_beta->a * c + alpha_beta->b * s;
+	dq->q = -alpha_beta->a * s + alpha_beta->b * c;
+}
+
+void foc_init(foc_t *foc) {
+	PI_Controller_Reset(&foc->daxis, 0);
+	PI_Controller_Reset(&foc->qaxis, 0);
+
+	foc->daxis.kp = mc_conf()->c.pid[PID_ID_ID].kp;
+	foc->daxis.ki = mc_conf()->c.pid[PID_ID_ID].ki;
+	foc->daxis.kd = mc_conf()->c.pid[PID_ID_ID].kd;
+	foc->daxis.DT = foc->ts;
+	foc->qaxis.kp = mc_conf()->c.pid[PID_IQ_ID].kp;
+	foc->qaxis.ki = mc_conf()->c.pid[PID_IQ_ID].ki;
+	foc->qaxis.kd = mc_conf()->c.pid[PID_IQ_ID].kd;
+	foc->qaxis.DT = foc->ts;
+	memset(&foc->in, 0, sizeof(foc_in_t));
+	memset(&foc->out, 0, sizeof(foc_out_t));
+}
+
+void foc_abc_2_dq(float a, float b, float c, float *d, float *q) {
+	float alpha = a;
+	float beta  = ONE_BY_SQRT3 * (b - c);
+	float cos,sin;
+	SinCos_Lut(0, &sin, &cos);
+	*d = alpha * cos + beta * sin;
+	*q = -alpha * sin + beta * cos;
+}
+
+
+static __INLINE float id_feedforward(foc_t *foc) {
+#ifdef CONFIG_CURRENT_LOOP_DECOUPE
+	return -(mc_conf()->m.lq * foc->out.curr_dq.q * foc->mot_vel_radusPers);
+#else
+	return 0;
+#endif
+}
+
+static __INLINE float iq_feedforward(foc_t *foc) {
+#ifdef CONFIG_CURRENT_LOOP_DECOUPE
+	return (mc_conf()->m.ld * foc->out.curr_dq.d + mc_conf()->m.flux) * foc->mot_vel_radusPers;
+#else
+	return 0;
+#endif
+}
+
+void foc_update(foc_t *foc) {
+	float max_Vdc = foc->in.dc_vol * CONFIG_SVM_MODULATION;
+	
+	LowPass_Filter(foc->mot_velocity_filterd, foc->in.mot_velocity, 0.01f);
+	foc->mot_vel_radusPers = foc->mot_velocity_filterd / 30.0f * PI * mc_conf()->m.poles;
+	SinCos_Lut(foc->in.mot_angle, &foc->sin, &foc->cos);
+	park(foc, &foc->in.curr_ab, &foc->out.curr_dq);
+	if (!foc->in.b_openloop) {
+		float d_step = (foc->in.target_curr_dq.d - foc->ramp_curr_dq.d) / foc->d_ramp_time;
+		float q_step = (foc->in.target_curr_dq.q - foc->ramp_curr_dq.q) / foc->q_ramp_time;
+		if (d_step < 0) {
+			d_step = -d_step;
+		}
+		if (q_step < 0) {
+			q_step = -q_step;
+		}
+		/* limiter Vd output for PI controller */
+		float max_vd = max_Vdc * SQRT3_BY_2;
+		foc->daxis.max = max_vd;
+		foc->daxis.min = -max_vd;
+		step_towards(&foc->ramp_curr_dq.d, foc->in.target_curr_dq.d, d_step);
+		float err = foc->ramp_curr_dq.d - foc->out.curr_dq.d;
+		float id_ff = id_feedforward(foc);
+		foc->in.target_vol_dq.d = PI_Controller_Current(&foc->daxis, err, id_ff);
+
+		/* limiter Vq output for PI controller */
+		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
+		foc->qaxis.max = max_vq;
+		foc->qaxis.min = -max_vq;
+		step_towards(&foc->ramp_curr_dq.q, foc->in.target_curr_dq.q, q_step);
+		err = foc->ramp_curr_dq.q - foc->out.curr_dq.q;
+		float iq_ff = iq_feedforward(foc);
+		foc->in.target_vol_dq.q = PI_Controller_Current(&foc->qaxis, err, iq_ff);
+	}else {
+		float max_vd = max_Vdc * SQRT3_BY_2;
+		foc->in.target_vol_dq.d = fclamp(foc->in.target_vol_dq.d, -max_vd, max_vd);
+		float max_vq = sqrtf(SQ(max_vd) - SQ(foc->in.target_vol_dq.d));
+		foc->in.target_vol_dq.q = fclamp(foc->in.target_vol_dq.q, -max_vq, max_vq);
+	}
+
+	foc->out.vol_dq.d = foc->in.target_vol_dq.d;
+	foc->out.vol_dq.q = foc->in.target_vol_dq.q;
+#ifdef CONFIG_Volvec_Delay_Comp
+	if (foc->mot_velocity_filterd >= CONFIG_Volvec_Delay_Comp_Start_Vel) {
+		float vol_dq_angle = foc->in.mot_angle + foc->mot_velocity_filterd / PI * 180.0f * (foc->ts * 0.8f);
+		rand_angle(vol_dq_angle);
+		SinCos_Lut(vol_dq_angle, &foc->sin, &foc->cos);
+	}
+#endif
+
+	rev_park(foc, &foc->out.vol_dq, &foc->out.vol_albeta);
+	
+	SVM_Duty_Fix(&foc->out.vol_albeta, foc->in.dc_vol, foc->half_period, &foc->out);
+
+	phase_current_point(&foc->out);
+	
+	pwm_update_duty(foc->out.duty[0], foc->out.duty[1], foc->out.duty[2]);
+	pwm_update_sample(foc->out.sample1, foc->out.sample2, foc->out.sample_phase);
+}
+

Applications/foc/core/foc.h

@@ -0,0 +1,75 @@
+#ifndef _FOC_H__
+#define _FOC_H__
+#include "math/fix_math.h"
+#include "foc/core/PI_Controller.h"
+
+typedef struct {
+	float a;
+	float b;
+}albt_t;
+
+typedef struct {
+	float d;
+	float q;
+}dq_t;
+
+typedef struct {
+	float 	curr_abc[3];
+	albt_t  curr_ab;
+	float   vol_abc[3]; //相对地电压
+	float   mot_velocity;   //from hall or encoder
+	float 	mot_angle; //from hall or encoder
+	dq_t    target_curr_dq;
+	dq_t    target_vol_dq;
+	float 	dc_vol;
+	bool    b_openloop;
+}foc_in_t;
+
+typedef struct {
+	u16   	duty[3];
+	u16   	low_duty;
+	u16   	mid_duty;
+	u8    	sector;
+	u8    	sample_phase;
+	u16   	sample1;
+	u16   	sample2;
+	dq_t  	vol_dq;
+	dq_t    curr_dq;
+	albt_t  vol_albeta;
+}foc_out_t;
+
+typedef struct {
+	foc_in_t in;
+	foc_out_t out;
+	dq_t    ramp_curr_dq;
+	float 	curr_abc_filter[3];
+	float   curr_abc_dt[3]; //abc phase current for deadtime compesition
+	float   mot_velocity_filterd; //电机滤波后的转速
+	float   mot_vel_radusPers; //电机的电角速度
+	float   d_ramp_time;
+	float   q_ramp_time;
+	float   ts;
+	float 	sin;
+	float 	cos;
+	u32     half_period;
+	PI_Controller daxis;
+	PI_Controller qaxis;
+}foc_t;
+
+void foc_init(foc_t *foc);
+void foc_update(foc_t *foc);
+void foc_abc_2_dq(float a, float b, float c, float *d, float *q);
+
+static __INLINE void clark(float A, float B, float C, albt_t *alpha_beta){
+	alpha_beta->a = A;
+	alpha_beta->b = ONE_BY_SQRT3 * (B - C);
+}
+
+static __INLINE void rev_clark(albt_t *alpha_beta, float *ABC){
+	ABC[0] = alpha_beta->a;
+	ABC[1] = -alpha_beta->a * 0.5f + alpha_beta->b * SQRT3_BY_2;
+	ABC[2] = -alpha_beta->a * 0.5f - alpha_beta->b * SQRT3_BY_2;
+}
+
+#endif /* _FOC_H__ */
+

Applications/foc/core/smo_observer.c

@@ -1,6 +1,6 @@
 #include "foc/mc_config.h"
 #include "math/fast_math.h"
-#include "PMSM_FOC_Core.h"
+#include "controller.h"
 #include "smo_observer.h"
 
 #define USE_ARCTAN 0

Applications/foc/core/svpwm.c

@@ -536,7 +536,7 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 #else
 #define Duty_Sat(max, t1, t2) 
 #endif
-void SVM_Duty_Fix(AB_t *alb, float vbus, u32 PWM_half_period, FOC_OutP *out) {
+void SVM_Duty_Fix(albt_t *alb, float vbus, u32 PWM_half_period, foc_out_t *out) {
 	float alpha = (alb->a);// * SQRT3_BY_2;
 	float beta  = (alb->b);//  * SQRT3_BY_2;
 	s32   PWM_Period = PWM_half_period * 2;
@@ -660,12 +660,12 @@ void SVM_Duty_Fix(AB_t *alb, float vbus, u32 PWM_half_period, FOC_OutP *out) {
 		default:
 			break;
 	}	
-	out->n_Duty[0] = sclamp(tA, 0, (s32)PWM_half_period);
-	out->n_Duty[1] = sclamp(tB, 0, (s32)PWM_half_period);
-	out->n_Duty[2] = sclamp(tC, 0, (s32)PWM_half_period);
-	out->n_lowDuty = low;
-	out->n_midDuty = midle;
-	out->n_Sector = sector;
+	out->duty[0] = sclamp(tA, 0, (s32)PWM_half_period);
+	out->duty[1] = sclamp(tB, 0, (s32)PWM_half_period);
+	out->duty[2] = sclamp(tC, 0, (s32)PWM_half_period);
+	out->low_duty = low;
+	out->mid_duty = midle;
+	out->sector = sector;
 #if 0	
 	if (_g_count++ % 10 == 0) {
 		plot_3data16(X, Y, Z);

Applications/foc/core/svpwm.h

@@ -1,13 +1,13 @@
 #ifndef _SVPWM_H__
 #define _SVPWM_H__
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #if 0
 void svpwm(alpha_beta_t *alpha_beta, float vbus, u32 PWW_half_period, phase_time_t *phase_out, u8 *sector_out);
 void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
 void SVPWM_ST(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
 void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
 #endif
-void SVM_Duty_Fix(AB_t *alb, float vbus, u32 PWM_half_period, FOC_OutP *out);
+void SVM_Duty_Fix(albt_t *alb, float vbus, u32 PWM_half_period, foc_out_t *out);
 
 #endif /* _SVPWM_H__ */
 

Applications/foc/foc_config.h

@@ -26,6 +26,10 @@
 
 #define CONFIG_CRUISE_RAMP_TIME 4000 /* 定速巡航的时候给定加减速的斜波时间 */
 
+#define CONFIG_CURRENT_RAMP_TIME 5000
+
+#define CONFIG_DEFAULT_TORQUE_RAMP_TIME 1000
+
 #define CONFIG_LIMIT_RAMP_TIME (6 * 1000) /* 过温、欠压保护的给定限流和限速的斜波时间 */
 
 #define CONFIG_SENSORLESS_RAMP_TIME (20 * 1000)
@@ -49,6 +53,10 @@
 #define CONFIG_SENSORLESS_MAX_TORQUE 100
 #define CONFIG_SENSORLESS_MAX_SPEED 4000
 
+#define CONFIG_PHASE_UNBALANCE_PEAK_CNT 3 //计算经过的电周期内的最大值(peak 峰值)
+#define CONFIG_PHASE_UNBALANCE_THROLD 4.0F
+#define CONFIG_PHASE_UNBALANCE_R      0.1F
+
 #define CONFIG_ENABLE_IAB_REC 1   // for phase current debug
 
 #ifdef CONFIG_SPEED_LADRC

Applications/foc/limit.c

@@ -1,5 +1,5 @@
 #include "foc/limit.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "foc/motor/motor.h"
 #include "foc/motor/motor_param.h"
 #include "foc/samples.h"
@@ -116,7 +116,7 @@ static u16 _motor_limit(void) {
 		if (lim_value != HW_LIMIT_NONE) {
 			if (lim_value == 0) {
 				if (mc_set_critical_error(FOC_CRIT_MOTOR_TEMP_Lim)) {
-					mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)PMSM_FOC_GetSpeed());
+					mc_crit_err_add(FOC_CRIT_MOTOR_TEMP_Lim, temp, (s16)mot_contrl_get_speed(&motor.controller));
 				}
 			}else if (_can_recovery){
 				mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Lim);
@@ -173,7 +173,7 @@ static u16 _mos_limit(void) {
 		if (lim_value != HW_LIMIT_NONE) {
 			if (lim_value == 0) {
 				if (mc_set_critical_error(FOC_CRIT_MOS_TEMP_Lim)) {
-					mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)PMSM_FOC_GetSpeed());
+					mc_crit_err_add(FOC_CRIT_MOS_TEMP_Lim, temp, (s16)mot_contrl_get_speed(&motor.controller));
 				}
 			}else if (_can_recovery){
 				mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Lim);
@@ -237,8 +237,8 @@ u16 vbus_under_vol_limit(void) {
 		u16 lim_value = _vol_limiter(vol, lim);
 		if (lim_value != HW_LIMIT_NONE) {
 			if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
-				if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
-					mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)PMSM_FOC_GetSpeed());
+				if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
+					mc_crit_err_add(FOC_CRIT_UN_Vol_Err, vol, (s16)mot_contrl_get_speed(&motor.controller));
 				}
 			}
 			return lim_value;

Applications/foc/motor/current.c

@@ -1,7 +1,7 @@
 #include <math.h>
 #include "bsp/bsp_drvier.h"
 #include "foc/motor/current.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "foc/mc_error.h"
 #include "libs/utils.h"
 #include "libs/logger.h"

Applications/foc/motor/current_ics.c

@@ -1,7 +1,7 @@
 #include <math.h>
 #include "bsp/bsp_driver.h"
 #include "foc/motor/current.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "foc/mc_error.h"
 #include "libs/utils.h"
 #include "libs/logger.h"
@@ -177,13 +177,13 @@ void phase_current_get(float *iABC){
 }
 
 void phase_current_point(void *p){
-	FOC_OutP *out = p;
+	foc_out_t *out = p;
 	current_samp_t *cs = &g_cs;
 	
 	cs->c_phases = PHASE_BC;
-	out->n_Sample1 = FOC_PWM_Half_Period - 1;
-	out->n_Sample2 = FOC_PWM_Half_Period + 1;
-	out->n_CPhases = cs->c_phases;
+	out->sample1 = FOC_PWM_Half_Period - 1;
+	out->sample2 = FOC_PWM_Half_Period + 1;
+	out->sample_phase = cs->c_phases;
 }
 
 void phase_current_adc_triger(void){

Applications/foc/motor/mot_params_ind.c

@@ -1,5 +1,5 @@
 #include "foc/motor/motor.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "math/fast_math.h"
 #include "foc/motor/mot_params_ind.h"
 #include "libs/logger.h"
@@ -31,8 +31,8 @@ void mot_params_ind_rs(float vd_max, float id_max, s32 time) {
 	rs_vd_max = vd_max;
 	rs_vd_now = 2.0f;
 	rs_meas_time = time;
-	PMSM_FOC_Set_MotAngle(0);
-	PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
+	mot_contrl_set_angle(&motor.controller, 0);
+	mot_contrl_set_vdq_immediate(&motor.controller, rs_vd_now, 0);
 
 	rs_ind_step = 1;
 	shark_timer_post(&_rs_ind_timer, 10);
@@ -47,42 +47,42 @@ void mot_params_ind_stop(void) {
 	b_ldq_ind = false;
 	b_flux_ind = false;
 	cpu_exit_critical(mask);
-	PMSM_FOC_SetOpenVdq(0, 0);
-	PMSM_FOC_Set_Current(0);
+	mot_contrl_set_vdq(&motor.controller, 0, 0);
+	mot_contrl_set_current(&motor.controller, 0);
 }
 
 static void _rs_ind_timer_handler(shark_timer_t *t) {
 	bool finish = false;
 	static int wait_iq_0_cnt = 0;
 	if (!b_rs_ind) {
-		PMSM_FOC_SetOpenVdq(0, 0);
+		mot_contrl_set_vdq(&motor.controller, 0, 0);
 		return;
 	}
 
 	switch (rs_ind_step) {
 	case 1:
-		if (PMSM_FOC_Get()->out.s_RealIdq.d < rs_id_max) {
+		if (motor.controller.foc.out.curr_dq.d < rs_id_max) {
 			rs_vd_now += 0.1f;
 			wait_iq_0_cnt = 0;
 			if (rs_vd_now >= rs_vd_max) {
-				PMSM_FOC_SetOpenVdq(0, 0);
+				mot_contrl_set_vdq(&motor.controller, 0, 0);
 				b_rs_ind = false;
-				sys_debug("id not reach max id %f\n", PMSM_FOC_Get()->out.s_RealIdq.d);
+				sys_debug("id not reach max id %f\n", motor.controller.foc.out.curr_dq.d);
 				return;
 			}
-			PMSM_FOC_SetOpenVdq_Immediate(rs_vd_now, 0);
+			mot_contrl_set_vdq_immediate(&motor.controller, rs_vd_now, 0);
 		}else {
 			rs_ind_step = 2;
 			sys_debug("id reach the set\n");
 		}
 		break;
 	case 2:
-		if (ABS(PMSM_FOC_Get()->out.s_FilterIdq.q) > 5.0f) {
+		if (ABS(motor.controller.foc.out.curr_dq.q) > 5.0f) {
 			wait_iq_0_cnt++;
 			if (wait_iq_0_cnt >= 200) {
-				PMSM_FOC_SetOpenVdq(0, 0);
+				mot_contrl_set_vdq(&motor.controller, 0, 0);
 				b_rs_ind = false;
-				sys_debug("iq is larger %f\n", PMSM_FOC_Get()->out.s_FilterIdq.q);
+				sys_debug("iq is larger %f\n", motor.controller.out_idq_filterd.q);
 				return;
 			}
 		}else {
@@ -92,12 +92,12 @@ static void _rs_ind_timer_handler(shark_timer_t *t) {
 		}
 		break;
 	case 3: {
-		float *iabc = PMSM_FOC_Get()->in.s_iABC;
+		float *iabc = motor.controller.foc.in.curr_abc;
 		float d, q;
-		PMSM_FOC_ABC2Dq(SIGN(iabc[0]), SIGN(iabc[1]), SIGN(iabc[2]), &d, &q);
-		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * PMSM_FOC_Get()->in.s_vDC * 1.5f;
+		foc_abc_2_dq(SIGN(iabc[0]), SIGN(iabc[1]), SIGN(iabc[2]), &d, &q);
+		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * motor.controller.foc.in.dc_vol * 1.5f;
 		float vd = (rs_vd_now - dtc) * TWO_BY_THREE;
-		float rs = vd / (PMSM_FOC_Get()->out.s_RealIdq.d + 0.0001f);
+		float rs = vd / (motor.controller.foc.out.curr_dq.d + 0.0001f);
 		rs_est_value = LowPass_Filter(rs_est_value, rs, 0.2f);
 		if (rs_meas_time-- <= 0) {
 			mot_params_ind_stop();
@@ -144,7 +144,7 @@ void mot_params_ind_inductance(float v, float freq, u16 l_type) {
 	hj_n = (float)FOC_PWM_FS / hj_freq;
 	hj_samples = hj_n * 50;
 	K_terms = (s32) (0.5f + hj_samples*hj_freq/(float)FOC_PWM_FS);
-	Vdead = PMSM_FOC_Get()->in.s_vDC * 0.5f * (float)CONFIG_HW_DeadTime / (float)FOC_PWM_period;
+	Vdead = motor.controller.foc.in.dc_vol * 0.5f * (float)CONFIG_HW_DeadTime / (float)FOC_PWM_period;
 	hj_w = 360.0f / hj_n;
 	sys_debug("hj %f, %f, %f, %f, %f, %f, %f\n", hj_v, hj_freq, hj_n, hj_samples, K_terms, Vdead, hj_w);
 	float fft_angle = 360.0f / hj_samples * K_terms;
@@ -211,7 +211,7 @@ void mot_params_high_freq_inject(void) {
 	}else {
 		vq = hj_v * c;
 	}
-	PMSM_FOC_SetOpenVdq_Immediate(vd, vq);
+	mot_contrl_set_vdq_immediate(&motor.controller, vd, vq);
 }
 
 bool mot_params_hj_sample_vi(float vd, float vq, float id, float iq) {
@@ -279,12 +279,12 @@ void mot_params_calc_inductance(void) {
 static float motVelRadusPers, flux_wait_cnt = 0, flux_do_cnt = 0, flux_est_value = 0;
 static bool _pending_flux_mc_stop = false;
 static void _flux_ind_timer_handler(shark_timer_t *t) {
-	float We = PMSM_FOC_Get()->in.s_motVelRadusPers;
+	float We = motor.controller.foc.mot_vel_radusPers;
 	float delta = We - motVelRadusPers;
-	motVelRadusPers = PMSM_FOC_Get()->in.s_motVelRadusPers;
+	motVelRadusPers = motor.controller.foc.mot_vel_radusPers;
 	if (We > 100 && ABS(delta) < 40) {
-		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * PMSM_FOC_Get()->in.s_vDC * 1.5f;
-		float vq = (PMSM_FOC_Get()->out.s_OutVdq.q - dtc) * TWO_BY_THREE;
+		float dtc = ((float)CONFIG_HW_DeadTime/(float)FOC_PWM_Half_Period) * motor.controller.foc.in.dc_vol * 1.5f;
+		float vq = (motor.controller.foc.out.vol_dq.q - dtc) * TWO_BY_THREE;
 		float flux = vq / We;
 		flux_est_value = LowPass_Filter(flux_est_value, flux, 0.1f);
 		flux_do_cnt ++;
@@ -300,7 +300,7 @@ static void _flux_ind_timer_handler(shark_timer_t *t) {
 			sys_debug("ind_flux finish, %f\n", flux_est_value);
 		}
 		mot_params_ind_stop();
-		if (PMSM_FOC_GetSpeed() < CONFIG_ZERO_SPEED_RPM) {
+		if (mot_contrl_get_speed(&motor.controller) < CONFIG_ZERO_SPEED_RPM) {
 			mc_ind_motor_start(false);
 		}else {
 			_pending_flux_mc_stop = true;
@@ -319,9 +319,9 @@ void mot_params_ind_flux(float id, float iq) {
 	flux_do_cnt = 0;
 	b_flux_ested = false;
 	mc_set_foc_mode(CTRL_MODE_CURRENT);
-	PMSM_FOC_Set_Current(iq);
+	mot_contrl_set_current(&motor.controller ,iq);
 	shark_timer_post(&_flux_ind_timer, 10);
-	motVelRadusPers = PMSM_FOC_Get()->in.s_motVelRadusPers;
+	motVelRadusPers = motor.controller.foc.mot_vel_radusPers;
 }
 
 float mot_params_get_est_ld(void) {
@@ -347,7 +347,7 @@ float mot_params_get_est_flux(void) {
 }
 
 void mot_params_flux_stop(void) {
-	if (_pending_flux_mc_stop && (PMSM_FOC_GetSpeed() < CONFIG_ZERO_SPEED_RPM)) {
+	if (_pending_flux_mc_stop && (mot_contrl_get_speed(&motor.controller) < CONFIG_ZERO_SPEED_RPM)) {
 		mc_ind_motor_start(false);
 		_pending_flux_mc_stop = false;
 	}

Applications/foc/motor/motor.c

@@ -8,11 +8,9 @@
 #include "libs/time_measure.h"
 #include "foc/commands.h"
 #include "libs/logger.h"
-#include "foc/core/e_ctrl.h"
 #include "foc/samples.h"
 #include "foc/motor/motor_param.h"
 #include "foc/core/foc_observer.h"
-#include "foc/core/thro_torque.h"
 #include "foc/core/F_Calc.h"
 #include "foc/core/etcs.h"
 #include "app/nv_storage.h"
@@ -38,7 +36,7 @@ static void _encoder_zero_off_timer_handler(shark_timer_t *);
 static shark_timer_t _encoder_zero_off_timer = TIMER_INIT(_encoder_zero_off_timer, _encoder_zero_off_timer_handler);
 static void _led_off_timer_handler(shark_timer_t *);
 static shark_timer_t _led_off_timer = TIMER_INIT(_led_off_timer, _led_off_timer_handler);
-static m_contrl_t motor = {
+m_contrl_t motor = {
 	.s_direction = POSITIVE,
 	.n_gear = 0,
 	.b_is96Mode = false,
@@ -222,12 +220,12 @@ static void mc_gear_vmode_changed(void) {
 	if (gears != &sensorless_gear) {
 		sensorless_gear.max_torque = gears->max_torque;
 	}else { //slowly changed
-		eRamp_set_time(&(PMSM_FOC_Get()->rtLim.rpmLimRamp), CONFIG_SENSORLESS_RAMP_TIME, CONFIG_SENSORLESS_RAMP_TIME);
-		eRamp_set_time(&(PMSM_FOC_Get()->rtLim.DCCurrLimRamp), CONFIG_SENSORLESS_RAMP_TIME, CONFIG_SENSORLESS_RAMP_TIME);
+		line_ramp_set_time(&motor.controller.vel_lim, CONFIG_SENSORLESS_RAMP_TIME);
+		line_ramp_set_time(&motor.controller.dc_curr_lim, CONFIG_SENSORLESS_RAMP_TIME);
 	}
-	PMSM_FOC_SpeedLimit((float)min(gears->max_speed, motor.u_set.rpm_lim));
-	PMSM_FOC_DCCurrLimit((float)min(gears->max_idc, motor.u_set.idc_lim));
-	PMSM_FOC_TorqueLimit((float)gears->max_torque);
+	mot_contrl_set_vel_limit(&motor.controller, (float)min(gears->max_speed, motor.u_set.rpm_lim));
+	mot_contrl_set_dccurr_limit(&motor.controller, (float)min(gears->max_idc, motor.u_set.idc_lim));
+	mot_contrl_set_torque_limit(&motor.controller, (float)gears->max_torque);
 	sys_debug("change %d, %d, %d\n", gears->max_idc, gears->max_speed, gears->max_torque);
 }
 
@@ -239,10 +237,8 @@ void mc_init(void) {
 	motor_encoder_init();
 	foc_command_init();
 	throttle_init();
-	thro_torque_init();
 	mc_detect_vbus_mode();
-	PMSM_FOC_CoreInit();
-	eCtrl_init(mc_get_gear_config()->zero_accl, mc_conf()->c.thro_dec_time);
+	mot_contrl_init(&motor.controller);
 	mc_gpio_init();
 	MC_Check_MosVbusThrottle();
 	sched_timer_enable(CONFIG_SPD_CTRL_US);
@@ -273,6 +269,41 @@ gear_t *mc_get_gear_config(void) {
 	return mc_get_gear_config_by_gear(motor.n_gear);
 }
 
+static __INLINE float gear_rpm_2_torque(u8 torque, s16 max) {
+	return (float)torque/100.0f * max;
+}
+
+float mc_get_max_torque_with_gear_vel(s16 vel, u8 gear) {
+	gear_t *_current_gear = mc_get_gear_config_by_gear(gear);
+	if (_current_gear == NULL) {
+		return 0;
+	}
+
+	if (vel > _current_gear->max_speed) {
+		vel = _current_gear->max_speed;
+	}
+	vel = ABS(vel);
+	if (vel <= 1000) {
+		return gear_rpm_2_torque(_current_gear->torque[0], _current_gear->max_torque);
+	}
+
+	for (int i = 1; i < CONFIG_GEAR_SPEED_TRQ_NUM; i++) {
+		if (vel <= 1000 * (i + 1)) { //线性插值
+			float trq1 = gear_rpm_2_torque(_current_gear->torque[i-1], _current_gear->max_torque);
+			float min_rpm = (i * 1000);
+			float trq2 = gear_rpm_2_torque(_current_gear->torque[i], _current_gear->max_torque);
+			float max_rpm = (i + 1) * 1000;
+			if (trq1 > trq2) {
+				return f_map_inv((float)vel, min_rpm, max_rpm, trq2, trq1);
+			}else {
+				return f_map((float)vel, min_rpm, max_rpm, trq1, trq2);
+			}
+		}
+	}
+	return gear_rpm_2_torque(_current_gear->torque[CONFIG_GEAR_SPEED_TRQ_NUM-1], _current_gear->max_torque);
+}
+
+
 /* 必须立即停机 */
 bool mc_critical_need_stop(void) {
 	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Vol_HW_Err);
@@ -324,27 +355,27 @@ bool mc_start(u8 mode) {
 	target_q = 0.0f;
 #endif
 	mc_detect_vbus_mode();
-	etcs_enable(motor.u_set.b_tcs);
+	etcs_enable(&motor.controller.ects, motor.u_set.b_tcs);
 	if (motor.b_lock_motor) {
-		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
 	}
 	MC_Check_MosVbusThrottle();
 
 	if (mc_unsafe_critical_error()) {
-		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
 		return false;
 	}
 	if (mode > CTRL_MODE_CURRENT) {
-		PMSM_FOC_SetErrCode(FOC_Param_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Param_Err);
 		return false;
 	}
-	if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
-		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+	if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
+		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
 		return false;
 	}
 	if (!mc_throttle_released()) {
-		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
 		return false;
 	}
 
@@ -352,12 +383,11 @@ bool mc_start(u8 mode) {
 
 	_mc_internal_init(mode, true);
 
-	thro_torque_reset();
+	throttle_torque_reset();
 	mc_gear_vmode_changed();
-	eCtrl_init(mc_get_gear_config()->zero_accl, mc_conf()->c.thro_dec_time);
+	mot_contrl_set_torque_ramp_time(&motor.controller, mc_get_gear_config()->zero_accl, mc_conf()->c.thro_dec_time);
 	motor_encoder_start(true);
-	PMSM_FOC_Start(mode);
-	PMSM_FOC_RT_LimInit();
+	mot_contrl_start(&motor.controller, mode);
 	pwm_turn_on_low_side();
 	delay_ms(10);
 	phase_current_offset_calibrate();
@@ -370,12 +400,12 @@ bool mc_start(u8 mode) {
 	adc_start_convert();
 	phase_current_calibrate_wait();
 	if (phase_curr_offset_check()) {
-		mc_set_critical_error(FOC_CRIT_CURR_OFF_Err);
+		mot_contrl_set_error(&motor.controller, FOC_CRIT_CURR_OFF_Err);
 		mc_stop();
 		return false;
 	}
 	if (mc_detect_hwbrake()) {
-		PMSM_FOC_Brake(true);
+		mot_contrl_set_hw_brake(&motor.controller, true);
 	}
 	gpio_beep(200);
 
@@ -388,16 +418,16 @@ bool mc_stop(void) {
 	}
 
 	if (motor.b_lock_motor) {
-		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
 	}
 
-	if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
-		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+	if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
+		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
 		return false;
 	}
 	if (!mc_throttle_released()) {
-		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
 		return false;
 	}
 
@@ -405,7 +435,7 @@ bool mc_stop(void) {
 	_mc_internal_init(CTRL_MODE_OPEN, false);
 	adc_stop_convert();
 	pwm_stop();
-	PMSM_FOC_Stop();
+	mot_contrl_stop(&motor.controller);
 	motor_encoder_start(false);
 	pwm_up_enable(true);
 	cpu_exit_critical(mask);
@@ -429,7 +459,7 @@ void mc_set_motor_lim_level(u8 l) {
 
 bool mc_set_gear(u8 gear) {
 	if (gear >= CONFIG_MAX_GEARS) {
-		PMSM_FOC_SetErrCode(FOC_Param_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Param_Err);
 		return false;
 	}
 	if (motor.n_gear != gear) {
@@ -464,7 +494,7 @@ bool mc_enable_cruise(bool enable) {
 	if (enable == motor.b_cruise) {
 		return true;
 	}
-	if (PMSM_FOC_EnableCruise(enable)) {
+	if (mot_contrl_set_cruise(&motor.controller, enable)) {
 		motor.b_cruise = enable;
 		motor.cruise_time = enable?shark_get_seconds():0;
 		motor.cruise_torque = 0.0f;
@@ -488,9 +518,9 @@ bool mc_is_cruise_enabled(void) {
 bool mc_set_cruise_speed(bool rpm_abs, float target_rpm) {
 	bool ret;
 	if (rpm_abs) {
-		ret = PMSM_FOC_Set_CruiseSpeed(target_rpm);
+		ret = mot_contrl_set_cruise_speed(&motor.controller, target_rpm);
 	}else {
-		ret = PMSM_FOC_Set_CruiseSpeed(PMSM_FOC_GetSpeed() + target_rpm);
+		ret = mot_contrl_set_cruise_speed(&motor.controller, mot_contrl_get_speed(&motor.controller) + target_rpm);
 	}
 	if (ret) {
 		motor.cruise_time = shark_get_seconds();
@@ -519,8 +549,8 @@ void mc_set_rpm_limit(s16 limit) {
 bool mc_set_ebrk_level(u8 level) {
 	if (level < CONFIG_EBRK_LVL_NUM) {
 		motor.u_set.ebrk_lvl = level;
-		eCtrl_Set_eBrk_RampTime(mc_get_ebrk_time());
-		PMSM_FOC_SetEbrkTorque(mc_get_ebrk_torque());
+		mot_contrl_set_ebrk_time(&motor.controller, mc_get_ebrk_time());
+		mot_contrl_set_ebrk_torquer(&motor.controller, mc_get_ebrk_torque());
 		return true;
 	}
 	return false;
@@ -532,7 +562,7 @@ void mc_enable_brkshutpower(u8 shut) {
 
 void mc_enable_tcs(bool enable) {
 	motor.u_set.b_tcs = enable;
-	etcs_enable(enable);
+	etcs_enable(&motor.controller.ects, enable);
 }
 
 s16 mc_get_ebrk_torque(void) {
@@ -559,17 +589,17 @@ bool mc_set_foc_mode(u8 mode) {
 	if (mc_critical_can_not_run()) {
 		return false;
 	}
-	if ((mode == CTRL_MODE_OPEN) && (ABS(PMSM_FOC_GetSpeed()) > CONFIG_ZERO_SPEED_RPM)) {
-		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+	if ((mode == CTRL_MODE_OPEN) && (ABS(mot_contrl_get_speed(&motor.controller)) > CONFIG_ZERO_SPEED_RPM)) {
+		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
 	}
 
 	u32 mask = cpu_enter_critical();
 	bool ret = false;
-	if (PMSM_FOC_SetCtrlMode(mode)) {
+	if (mot_contrl_request_mode(&motor.controller, mode)) {
 		motor.mode = mode;
 		if (mode == CTRL_MODE_OPEN || mode == CTRL_MODE_CURRENT) {
-			PMSM_FOC_Start(motor.mode);
+			mot_contrl_start(&motor.controller, motor.mode);
 			pwm_enable_channel();
 		}
 		ret = true;
@@ -583,15 +613,15 @@ bool mc_start_epm(bool epm) {
 		return true;
 	}
 	if (!motor.b_start) {
-		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
 	}
 	if (motor_encoder_get_speed() > CONFIG_ZERO_SPEED_RPM) {
-		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
 		return false;
 	}
 	if (!mc_throttle_released()) {
-		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
 		return false;
 	}
 	u32 mask = cpu_enter_critical();
@@ -600,16 +630,16 @@ bool mc_start_epm(bool epm) {
 	motor.f_epm_trq = 0.0f;
 	motor_encoder_band_epm(epm);
 	if (epm) {
-		eCtrl_set_TgtSpeed(0);
+		mot_contrl_set_target_vel(&motor.controller, 0);
 		motor.mode = CTRL_MODE_SPD;
 		motor.epm_dir = EPM_Dir_None;
-		PMSM_FOC_TorqueLimit(mc_conf()->c.max_epm_torque);
-		PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
+		mot_contrl_set_torque_limit(&motor.controller, mc_conf()->c.max_epm_torque);
+		mot_contrl_request_mode(&motor.controller, CTRL_MODE_SPD);
 	}else {
 		motor.epm_dir = EPM_Dir_None;
 		motor.mode = CTRL_MODE_TRQ;
 		motor.b_epm_cmd_move = false;
-		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
+		mot_contrl_request_mode(&motor.controller, CTRL_MODE_TRQ);
 		mc_gear_vmode_changed();
 	}
 	cpu_exit_critical(mask);
@@ -622,12 +652,12 @@ bool mc_is_epm(void) {
 }
 
 bool mc_is_start(void) {
-	return (motor.b_start || PMSM_FOC_Is_Start());
+	return (motor.b_start || mot_contrl_is_start(&motor.controller));
 }
 
-bool mc_start_epm_move(EPM_Dir_t dir, bool is_command) {
+bool mc_start_epm_move(epm_dir_t dir, bool is_command) {
 	if (!motor.b_epm || !motor.b_start) {
-		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
 	}
 	if ((dir == motor.epm_dir) && (is_command == motor.b_epm_cmd_move)) {
@@ -642,35 +672,23 @@ bool mc_start_epm_move(EPM_Dir_t dir, bool is_command) {
 	if (dir != EPM_Dir_None) {
 		motor.b_epm_cmd_move = is_command;
 		
-		if (!PMSM_FOC_Is_Start()) {
-			PMSM_FOC_Start(motor.mode);
+		if (!mot_contrl_is_start(&motor.controller)) {
+			mot_contrl_start(&motor.controller, motor.mode);
 			pwm_enable_channel();
-		}else if (PMSM_FOC_AutoHoldding()) {
+		}else if (mot_contrl_is_auto_holdding(&motor.controller)) {
 			mc_auto_hold(false);
 		}
 		if (dir == EPM_Dir_Back) {
-#ifdef CONFIG_SPEED_LADRC
-			PMSM_FOC_Change_VelLoop_Params(CONFIG_LADRC_EPM_Wcv, CONFIG_LADRC_EPMBK_B0);
-#else
-			PMSM_FOC_Change_VelLoop_Params(0.2f, 7.5f);
-#endif
+			mot_contrl_velloop_params(&motor.controller, 0.2f, 7.5f);
 		}else {
-#ifdef CONFIG_SPEED_LADRC
-			PMSM_FOC_Change_VelLoop_Params(CONFIG_LADRC_EPM_Wcv, CONFIG_LADRC_EPM_B0);
-#else
-			PMSM_FOC_Change_VelLoop_Params(0.2f, 7.5f);
-#endif
+			mot_contrl_velloop_params(&motor.controller, 0.2f, 7.5f);
 		}
-		PMSM_FOC_TorqueLimit(motor.f_epm_trq);
-		PMSM_FOC_Set_TgtSpeed(motor.f_epm_vel);
+		mot_contrl_set_torque_limit(&motor.controller, motor.f_epm_trq);
+		mot_contrl_set_target_vel(&motor.controller, motor.f_epm_vel);
 	}else {
 		motor.b_epm_cmd_move = false;
-#ifdef CONFIG_SPEED_LADRC
-		PMSM_FOC_Change_VelLoop_Params(nv_get_foc_params()->f_adrc_vel_Wcv, nv_get_foc_params()->f_adrc_vel_B0);
-#else
-		PMSM_FOC_Change_VelLoop_Params(mc_conf()->c.pid[PID_Vel_ID].kp, mc_conf()->c.pid[PID_Vel_ID].ki);
-#endif
-		PMSM_FOC_Set_TgtSpeed(0);
+		mot_contrl_velloop_params(&motor.controller, mc_conf()->c.pid[PID_Vel_ID].kp, mc_conf()->c.pid[PID_Vel_ID].ki);
+		mot_contrl_set_target_vel(&motor.controller, 0);
 	}
 	cpu_exit_critical(mask);
 	return true;
@@ -690,11 +708,11 @@ void mc_set_fan_duty(u8 duty) {
 	fan_set_duty(duty);
 }
 
-bool mc_command_epm_move(EPM_Dir_t dir) {
+bool mc_command_epm_move(epm_dir_t dir) {
 	return mc_start_epm_move(dir, true);
 }
 
-bool mc_throttle_epm_move(EPM_Dir_t dir) {
+bool mc_throttle_epm_move(epm_dir_t dir) {
 	return mc_start_epm_move(dir, false);
 }
 
@@ -713,7 +731,7 @@ void mc_use_throttle(void) {
 
 void mc_get_running_status(u8 *data) {
 	data[0] = motor.mode;
-	data[0] |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
+	data[0] |= (mot_contrl_is_auto_holdding(&motor.controller)?1:0) << 2;
 	data[0] |= (motor.b_break?1:0) << 3;
 	data[0] |= (motor.b_cruise?1:0) << 4;
 	data[0] |= (motor.b_start?1:0) << 5;
@@ -725,18 +743,18 @@ u16 mc_get_running_status2(void) {
 	u16 data = 0;
 	data = motor.b_start?1:0;
 	data |= (motor.n_gear & 0x7) << 1;
-	data |= (PMSM_FOC_AutoHoldding()?1:0) << 3;
+	data |= (mot_contrl_is_auto_holdding(&motor.controller)?1:0) << 3;
 	data |= (motor.b_break?1:0) << 4;
 	data |= (motor.b_cruise?1:0) << 5;
 	data |= (mc_is_epm()?1:0) << 6;
 	data |= (motor.b_lock_motor) << 7; //motor locked
-	data |= (eCtrl_is_eBrk_Running()?1:0) << 8; //能量回收运行标志
+	data |= (mot_contrl_ebrk_is_running(&motor.controller)?1:0) << 8; //能量回收运行标志
 	data |= ((motor.n_CritiCalErrMask != 0)?1:0) << 9;
 	data |= (fan_pwm_is_running()?1:0) << 10; //风扇是否运行
 	data |= (motor.b_runStall?1:0) << 11; //是否堵转
-	data |= (PMSM_FOC_iDC_is_Limited()?1:0) << 12; //是否欠压限制母线电流
-	data |= (PMSM_FOC_Torque_is_Limited()?1:0) << 13; //是否高温限扭矩
-	data |= (etcs_is_running()?1:0) << 14; //电子tcs是否正在工作
+	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 |= (throttle_not_released_err()?1:0) << 15;
 	return data;
 }
@@ -748,12 +766,12 @@ static int   _force_wait = 2000;
 void mc_force_run_open(s16 vd, s16 vq, bool align) {
 	if (motor.b_start || motor.b_force_run) {
 		if (vd == 0 && vq == 0) {
-			PMSM_FOC_SetOpenVdq(0, 0);
+			mot_contrl_set_vdq(&motor.controller, 0, 0);
 			delay_ms(500);
 			wdog_reload();
 			adc_stop_convert();
 			pwm_stop();
-			PMSM_FOC_Stop();
+			mot_contrl_stop(&motor.controller);
 			pwm_up_enable(true);
 			motor.b_force_run = false;
 			motor.b_ignor_throttle = false;
@@ -766,20 +784,20 @@ void mc_force_run_open(s16 vd, s16 vq, bool align) {
 	motor.b_ignor_throttle = true;
 	MC_Check_MosVbusThrottle();
 	if (mc_unsafe_critical_error()) {
-		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
 	}
 	
 	pwm_up_enable(false);
 	pwm_turn_on_low_side();
 	task_udelay(500);
-	PMSM_FOC_Start(CTRL_MODE_OPEN);
+	mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
 	phase_current_offset_calibrate();
 	pwm_start();
 	adc_start_convert();
 	pwm_enable_channel();
 	phase_current_calibrate_wait();
-	PMSM_FOC_Set_MotAngle(0);
-	PMSM_FOC_SetOpenVdq((float)vd, 0);
+	mot_contrl_set_angle(&motor.controller, 0);
+	mot_contrl_set_vdq(&motor.controller, (float)vd, 0);
 	if (align) {
 		_force_wait = 2000 + 1;
 	}else {
@@ -796,30 +814,30 @@ bool mc_ind_motor_start(bool start) {
 		motor.b_ignor_throttle = true;
 		MC_Check_MosVbusThrottle();
 		if (mc_unsafe_critical_error() || mot_params_flux_pending()) {
-			PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
+			mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
 			return false;
 		}
 		pwm_up_enable(false);
 		pwm_turn_on_low_side();
 		task_udelay(500);
-		PMSM_FOC_Start(CTRL_MODE_OPEN);
+		mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
 		phase_current_offset_calibrate();
 		pwm_start();
 		adc_start_convert();
 		pwm_enable_channel();
 		phase_current_calibrate_wait();
-		PMSM_FOC_SetOpenVdq_Immediate(0, 0);
+		mot_contrl_set_vdq_immediate(&motor.controller, 0, 0);
 		motor.b_ind_start = start;
 	}else {
 		u32 mask = cpu_enter_critical();
 		motor.b_ind_start = start;
-		PMSM_FOC_SetOpenVdq(0, 0);
+		mot_contrl_set_vdq(&motor.controller, 0, 0);
 		cpu_exit_critical(mask);
 		delay_us(500);
 		wdog_reload();
 		adc_stop_convert();
 		pwm_stop();
-		PMSM_FOC_Stop();
+		mot_contrl_stop(&motor.controller);
 		motor.mode = CTRL_MODE_OPEN;
 		pwm_up_enable(true);
 		motor.b_ignor_throttle = false;
@@ -834,11 +852,11 @@ static void _encoder_zero_off_timer_handler(shark_timer_t *t){
 	}
 	float enc_off = 0.0f;
 	float phase = motor_encoder_zero_phase_detect(&enc_off);
-	PMSM_FOC_SetOpenVdq(0, 0);
+	mot_contrl_set_vdq(&motor.controller, 0, 0);
 	delay_ms(50);
 	adc_stop_convert();
 	pwm_stop();
-	PMSM_FOC_Stop();
+	mot_contrl_stop(&motor.controller);
 	_mc_internal_init(CTRL_MODE_OPEN, false);
 	motor.b_calibrate = false;
 }
@@ -848,11 +866,11 @@ bool mc_encoder_zero_calibrate(s16 vd) {
 		if (vd == 0) {
 			encoder_clear_cnt_offset();
 			shark_timer_cancel(&_encoder_zero_off_timer);
-			PMSM_FOC_SetOpenVdq(0, 0);
+			mot_contrl_set_vdq(&motor.controller, 0, 0);
 			delay_ms(500);
 			adc_stop_convert();
 			pwm_stop();
-			PMSM_FOC_Stop();
+			mot_contrl_stop(&motor.controller);
 			_mc_internal_init(CTRL_MODE_OPEN, false);
 			motor.b_calibrate = false;
 			motor.b_ignor_throttle = false;
@@ -863,21 +881,21 @@ bool mc_encoder_zero_calibrate(s16 vd) {
 	motor.b_ignor_throttle = true;
 	MC_Check_MosVbusThrottle();
 	if (mc_unsafe_critical_error()) {
-		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Have_CritiCal_Err);
 		return false;
 	}
 	_mc_internal_init(CTRL_MODE_OPEN, true);
 	motor.b_calibrate = true;
 	pwm_turn_on_low_side();
 	task_udelay(500);
-	PMSM_FOC_Start(CTRL_MODE_OPEN);
+	mot_contrl_start(&motor.controller, CTRL_MODE_OPEN);
 	phase_current_offset_calibrate();
 	pwm_start();
 	adc_start_convert();
 	pwm_enable_channel();
 	phase_current_calibrate_wait();
-	PMSM_FOC_Set_MotAngle(0);
-	PMSM_FOC_SetOpenVdq(vd, 0);
+	mot_contrl_set_angle(&motor.controller, 0);
+	mot_contrl_set_vdq(&motor.controller, vd, 0);
 
 	shark_timer_post(&_encoder_zero_off_timer, 6*1000);
 
@@ -901,12 +919,12 @@ bool mc_lock_motor(bool lock) {
 	int ret = true;
 	u32 mask = cpu_enter_critical();
 	if (motor.b_start) {
-		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		ret = false;
 		goto ml_ex_cri;
 	}
 	if (lock && (motor_encoder_get_speed() >= CONFIG_LOCK_MOTOR_MIN_RPM)) {
-		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+		mot_contrl_set_error(&motor.controller, FOC_NowAllowed_With_Speed);
 		ret = false;
 		goto ml_ex_cri;
 	}
@@ -928,25 +946,25 @@ bool mc_auto_hold(bool hold) {
 		return true;
 	}
 	if (!mc_conf()->s.auto_hold) {
-		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
 	}
 	if (!motor.b_start) {
-		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		mot_contrl_set_error(&motor.controller, FOC_NotAllowed);
 		return false;
 	}
 	if (hold && !mc_throttle_released()) {
-		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
+		mot_contrl_set_error(&motor.controller, FOC_Throttle_Err);
 		return false;
 	}
 	u32 mask = cpu_enter_critical();
 	motor.b_auto_hold = hold;
-	if (!PMSM_FOC_Is_Start()) {
-		PMSM_FOC_Start(motor.mode);
-		PMSM_FOC_AutoHold(hold);
+	if (!mot_contrl_is_start(&motor.controller)) {
+		mot_contrl_start(&motor.controller, motor.mode);
+		mot_contrl_set_autohold(&motor.controller, hold);
 		pwm_enable_channel();
 	}else {
-		PMSM_FOC_AutoHold(hold);
+		mot_contrl_set_autohold(&motor.controller, hold);
 	}
 	cpu_exit_critical(mask);
 	return true;
@@ -1067,9 +1085,9 @@ void MC_Brake_IRQHandler(void) {
 	}
 	if (motor.b_break) {
 		mc_enable_cruise(false);
-		PMSM_FOC_Brake(true);
+		mot_contrl_set_hw_brake(&motor.controller, true);
 	}else {
-		PMSM_FOC_Brake(false);
+		mot_contrl_set_hw_brake(&motor.controller, false);
 	}
 }
 
@@ -1082,14 +1100,14 @@ static void mc_save_err_runtime(void) {
 	mc_error.vbus_x10 = (s16)(sample_vbus_raw() * 10.0f); 
 	mc_error.ibus_x10 = (s16)(sample_ibus_raw() * 10.0f);
 	mc_error.vacc_x10 = (s16) (sample_acc_vol_raw() * 10.0f);
-	mc_error.id_ref_x10 = (s16)(PMSM_FOC_Get()->idq_ctl[0].s_Cp * 10.0f);
-	mc_error.iq_ref_x10 = (s16)(PMSM_FOC_Get()->idq_ctl[1].s_Cp * 10.0f);
-	mc_error.id_x10 = (s16)(PMSM_FOC_Get()->out.s_RealIdq.d * 10.0f);
-	mc_error.iq_x10 = (s16)(PMSM_FOC_Get()->out.s_RealIdq.q * 10.0f);
-	mc_error.vd_x10 = (s16)(PMSM_FOC_Get()->out.s_OutVdq.d * 10.0f);
-	mc_error.vq_x10 = (s16)(PMSM_FOC_Get()->out.s_OutVdq.q * 10.0f);
-	mc_error.torque_ref_x10 = (s16)(PMSM_FOC_Get()->in.s_targetTorque * 10.0f);
-	mc_error.run_mode = PMSM_FOC_Get()->out.n_RunMode;
+	mc_error.id_ref_x10 = (s16)(motor.controller.foc.ramp_curr_dq.d * 10.0f);
+	mc_error.iq_ref_x10 = (s16)(motor.controller.foc.ramp_curr_dq.q * 10.0f);
+	mc_error.id_x10 = (s16)(motor.controller.foc.out.curr_dq.d * 10.0f);
+	mc_error.iq_x10 = (s16)(motor.controller.foc.out.curr_dq.q * 10.0f);
+	mc_error.vd_x10 = (s16)(motor.controller.foc.out.vol_dq.d * 10.0f);
+	mc_error.vq_x10 = (s16)(motor.controller.foc.out.vol_dq.q * 10.0f);
+	mc_error.torque_ref_x10 = (s16)(motor.controller.target_torque * 10.0f);
+	mc_error.run_mode = motor.controller.mode_running;
 	mc_error.rpm = (s16)motor_encoder_get_speed();
 	mc_error.b_sensorless = !foc_observer_is_encoder();
 	mc_error.b_sensorless_stable = foc_observer_sensorless_stable();
@@ -1112,7 +1130,7 @@ void MC_Protect_IRQHandler(void){
 	_mc_internal_init(CTRL_MODE_OPEN, false);
 	adc_stop_convert();
 	pwm_stop();
-	PMSM_FOC_Stop();
+	mot_contrl_stop(&motor.controller);
 	pwm_up_enable(true);
 }
 
@@ -1131,13 +1149,13 @@ static void motor_vbus_crit_low(s16 curr_vbus) {
 	if (curr_vbus < motor.s_vbus_hw_min) {
 		_vbus_e_count ++;
 		if (_vbus_e_count >= 2) {
-			if (PMSM_FOC_Is_Start()) {
+			if (mot_contrl_is_start(&motor.controller)) {
 				pwm_disable_channel();
 				mc_save_err_runtime();
-				PMSM_FOC_Stop();
+				mot_contrl_stop(&motor.controller);
 			}
 			if (mc_set_critical_error(FOC_CRIT_Vol_HW_Err)) {
-				if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
+				if (mot_contrl_get_speed(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
 					mc_crit_err_add_s16(FOC_CRIT_Vol_HW_Err, curr_vbus);
 				}
 			}
@@ -1148,7 +1166,7 @@ static void motor_vbus_crit_low(s16 curr_vbus) {
 }
 
 void TIMER_UP_IRQHandler(void){
-	if (!motor.b_start && !PMSM_FOC_Is_Start()) {
+	if (!motor.b_start && !mot_contrl_is_start(&motor.controller)) {
 		motor_encoder_update(false);
 		motor_vbus_crit_low((s16)get_vbus_int());
 	}
@@ -1178,8 +1196,8 @@ void ADC_IRQHandler(void) {
 
 #if (CONFIG_ENABLE_IAB_REC==1)
 	if (b_iab_rec && (iab_w_count < CONFIG_IAB_REC_COUNT)) {
-		ia[iab_w_count] = (s16)PMSM_FOC_Get()->in.s_iABC[0];
-		ib[iab_w_count] = (s16)PMSM_FOC_Get()->in.s_iABC[1];
+		ia[iab_w_count] = (s16)motor.controller.foc.in.curr_abc[0];
+		ib[iab_w_count] = (s16)motor.controller.foc.in.curr_abc[1];
 		iab_w_count ++;
 	}
 #endif
@@ -1187,17 +1205,17 @@ void ADC_IRQHandler(void) {
 	float vd, vq;
 	if (motor.b_ind_start) {
 		mot_params_high_freq_inject();
-		vd = PMSM_FOC_Get()->out.s_OutVdq.d;
-		vq = PMSM_FOC_Get()->out.s_OutVdq.q;
+		vd = motor.controller.foc.out.vol_dq.d;
+		vq = motor.controller.foc.out.vol_dq.q;
 	}
-	if (!PMSM_FOC_Schedule()) {/* FOC 角度错误，立即停机 */
-		if (PMSM_FOC_Is_Start()) {
+	if (!mot_contrl_update(&motor.controller)) {/* FOC 角度错误，立即停机 */
+		if (mot_contrl_is_start(&motor.controller)) {
 			pwm_disable_channel();
 			/* 记录错误 */
 			if (!foc_observer_is_force_sensorless()) {
 				mc_save_err_runtime();
 			}
-			PMSM_FOC_Stop();
+			mot_contrl_stop(&motor.controller);
 			g_meas_foc.first = true;
 			if (!foc_observer_is_force_sensorless()) {
 				if (mc_set_critical_error(FOC_CRIT_Angle_Err)) {
@@ -1216,8 +1234,8 @@ void ADC_IRQHandler(void) {
 		}
 	}
 	if (motor.b_ind_start) {
-		float id = PMSM_FOC_Get()->out.s_RealIdq.d;
-		float iq = PMSM_FOC_Get()->out.s_RealIdq.q;
+		float id = motor.controller.foc.out.curr_dq.d;
+		float iq = motor.controller.foc.out.curr_dq.q;
 		mot_params_hj_sample_vi(vd, vq, id, iq);
 	}
 	TIME_MEATURE_END();
@@ -1251,7 +1269,7 @@ void mc_start_current_rec(bool rec) {
 }
 #endif
 static bool mc_run_stall_process(u8 run_mode) {
-	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
+	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !mot_contrl_is_auto_holdding(&motor.controller)) {
 		//堵转判断
 		if (motor.b_runStall) {
 			if (!mc_throttle_released()) {
@@ -1259,8 +1277,8 @@ static bool mc_run_stall_process(u8 run_mode) {
 			}
 			motor.runStall_time = 0;
 			motor.b_runStall = false; //转把释放，清除堵转标志
-		}else if (PMSM_FOC_Get_Real_dqVector() >= CONFIG_STALL_MAX_CURRENT){
-			if (ABS(PMSM_FOC_GetSpeed()) < 1.0f && (motor.runStall_time == 0)) {
+		}else if (mot_contrl_get_current_vector(&motor.controller) >= CONFIG_STALL_MAX_CURRENT){
+			if (ABS(mot_contrl_get_speed(&motor.controller)) < 1.0f && (motor.runStall_time == 0)) {
 				motor.runStall_time = get_tick_ms();
 				motor.runStall_pos = motor_encoder_get_position();
 			}
@@ -1268,8 +1286,8 @@ static bool mc_run_stall_process(u8 run_mode) {
 				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
 					motor.b_runStall = true;
 					motor.runStall_time = 0;
-					PMSM_FOC_Set_Torque(0);
-					thro_torque_reset();
+					mot_contrl_set_torque(&motor.controller, 0);
+					throttle_torque_reset();
 					return true;
 				}
 				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
@@ -1285,12 +1303,12 @@ static bool mc_run_stall_process(u8 run_mode) {
 
 static void mc_autohold_process(void) {
 	if (!mc_conf()->s.auto_hold) {
-		if (PMSM_FOC_AutoHoldding()) {
+		if (mot_contrl_is_auto_holdding(&motor.controller)) {
 			mc_auto_hold(false);
 		}
 		return;
 	}
-	if (PMSM_FOC_AutoHoldding()) {
+	if (mot_contrl_is_auto_holdding(&motor.controller)) {
 		if (!mc_throttle_released()) {
 			mc_auto_hold(false);
 			motor.b_wait_brk_release = false;
@@ -1300,7 +1318,7 @@ static void mc_autohold_process(void) {
 			mc_auto_hold(false);
 		}
 	}
-	if (!PMSM_FOC_AutoHoldding() && motor.b_break && (motor_encoder_get_speed() == 0)) {
+	if (!mot_contrl_is_auto_holdding(&motor.controller) && motor.b_break && (motor_encoder_get_speed() == 0)) {
 		if (motor.n_autohold_time == 0) {
 			motor.n_autohold_time = get_tick_ms();
 		}else {
@@ -1336,13 +1354,13 @@ static void mc_process_epm_move(void) {
 		target_vel = -mc_conf()->c.max_epm_back_rpm;
 		target_trq = mc_conf()->c.max_epm_back_torque;
 	}else if (!motor.b_epm_cmd_move) {
-		target_vel = get_throttle_ration(throttle_get_signal()) * 2.0f * (float)target_vel;
+		target_vel = throttle_vol_to_open_ration(throttle_get_signal()) * 2.0f * (float)target_vel;
 		step = 0.07f;
 	}
 	step_towards(&motor.f_epm_vel, target_vel, step);
 	motor.f_epm_trq = target_trq;
-	PMSM_FOC_TorqueLimit(motor.f_epm_trq);
-	PMSM_FOC_Set_TgtSpeed(motor.f_epm_vel);
+	mot_contrl_set_torque_limit(&motor.controller, motor.f_epm_trq);
+	mot_contrl_set_target_vel(&motor.controller, motor.f_epm_vel);
 }
 
 static bool mc_process_force_running(void) {
@@ -1353,7 +1371,7 @@ static bool mc_process_force_running(void) {
 			}else {
 				_force_angle += 1.5f;
 				rand_angle(_force_angle);
-				PMSM_FOC_Set_MotAngle(_force_angle);
+				mot_contrl_set_angle(&motor.controller, _force_angle);
 			}
 		}
 		return true;
@@ -1363,7 +1381,7 @@ static bool mc_process_force_running(void) {
 
 static void mc_process_brake_light(void) {
 	bool can_lighting = false;
-	if (motor.b_break || motor.b_auto_hold || eCtrl_is_eBrk_Running() || ((!mc_critical_can_not_run()) && motor_encoder_get_speed() > 2000)) {
+	if (motor.b_break || motor.b_auto_hold || mot_contrl_ebrk_is_running(&motor.controller) || ((!mc_critical_can_not_run()) && motor_encoder_get_speed() > 2000)) {
 		can_lighting = true;
 	}
 	gpio_brk_light_enable(can_lighting);
@@ -1373,8 +1391,8 @@ static void mc_process_brake_light(void) {
 static void mc_process_curise(void) {
 	static bool can_pause_resume = false;
 	if (motor.b_cruise) {
-		if (PMSM_FOC_GetSpeed() < CONFIG_CRUISE_EXIT_RPM) {
-			mc_enable_cruise(false);
+		if (mot_contrl_get_speed(&motor.controller) < CONFIG_CRUISE_EXIT_RPM) {
+			mot_contrl_set_cruise(&motor.controller, false);
 			return;
 		}
 		/* 定速巡航模式下，必须转把归位后才能开始通过拧动转把加速 */
@@ -1384,25 +1402,24 @@ static void mc_process_curise(void) {
 		if (!can_pause_resume) {
 			return;
 		}
-		if (PMSM_FOC_Is_CruiseEnabled()) {
+		if (mot_contrl_is_cruise_enabled(&motor.controller)) {
 			u32 cruise_time = shark_get_seconds() - motor.cruise_time;
 			if ((cruise_time >= 3) && motor.cruise_torque == 0.0f) {
-				motor.cruise_torque = PMSM_FOC_Get()->in.s_targetTorque;
+				motor.cruise_torque = motor.controller.target_torque;
 			}else if ((cruise_time >= 3) && (motor.cruise_torque > 0.0f)){
 				float trq_req = get_user_request_torque();
 				if (trq_req > motor.cruise_torque * 1.2f) {
-					PMSM_FOC_PauseCruise(); //需要加速,暂停定速巡航
+					mot_contrl_pause_cruise(&motor.controller); //需要加速,暂停定速巡航
 				}
 			}
 		}else {
 			float trq_req = get_user_request_torque();
 			if (trq_req <= motor.cruise_torque * 1.1f) {
-				PMSM_FOC_ResumeCruise(); //重新开始定速巡航，巡航速度还是前一次定速巡航给的速度
-				//motor.cruise_time = shark_get_seconds();
+				mot_contrl_resume_cruise(&motor.controller); //重新开始定速巡航，巡航速度还是前一次定速巡航给的速度
 			}
 		}
 	}else {
-		PMSM_FOC_EnableCruise(false);
+		mot_contrl_set_cruise(&motor.controller, false);
 		can_pause_resume = false;
 	}
 }
@@ -1423,8 +1440,8 @@ static bool mc_can_stop_foc(void) {
 			return true;
 		}
 	}
-	if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
-		if (!PMSM_FOC_AutoHoldding() && motor.epm_dir == EPM_Dir_None) {
+	if (mc_throttle_released() && mot_contrl_get_speed(&motor.controller) == 0.0f) {
+		if (!mot_contrl_is_auto_holdding(&motor.controller) && motor.epm_dir == EPM_Dir_None) {
 			return true;
 		}
 	}
@@ -1450,24 +1467,49 @@ static bool mc_can_restart_foc(void) {
 
 static void mc_motor_runstop(void) {
 	u32 mask;
-	if (PMSM_FOC_Is_Start() && mc_can_stop_foc()) {
+	if (mot_contrl_is_start(&motor.controller) && mc_can_stop_foc()) {
 		mask = cpu_enter_critical();
-		PMSM_FOC_Stop();
+		mot_contrl_stop(&motor.controller);
 		pwm_disable_channel();
 		g_meas_foc.first = true;
 		cpu_exit_critical(mask);
 	}
-	if (!PMSM_FOC_Is_Start() && mc_can_restart_foc()) {
+	if (!mot_contrl_is_start(&motor.controller) && mc_can_restart_foc()) {
 		mask = cpu_enter_critical();
-		PMSM_FOC_Start(motor.mode);
+		mot_contrl_start(&motor.controller, motor.mode);
 		mc_gear_vmode_changed();
-		thro_torque_reset();
+		throttle_torque_reset();
 		pwm_enable_channel();
 		g_meas_foc.first = true;
 		cpu_exit_critical(mask);
 	}
 }
-
+static void mc_process_throttle_torque(float vol) {
+	float torque = throttle_get_torque(&motor.controller, vol);
+	if (motor.controller.mode_running == CTRL_MODE_TRQ) {
+		throttle_set_torque(&motor.controller, torque);
+	}else if (motor.controller.mode_running == CTRL_MODE_SPD) {
+		if (!mc_is_cruise_enabled()) {
+			float vel_ref = motor.controller.userlim.mot_vel * throttle_get_open_ration_filted();
+			mot_contrl_set_target_vel(&motor.controller, vel_ref);
+		}
+	}else if (motor.controller.mode_running == CTRL_MODE_EBRAKE){
+		float vel = mot_contrl_get_speed(&motor.controller);
+		float ebrk_trq = motor_get_ebreak_toruqe(vel);
+			
+		if (ebrk_trq >= -mot_contrl_get_ebrk_torque(&motor.controller)/2){
+			mot_contrl_set_ebrk_time(&motor.controller, 1);
+		}
+		if (ebrk_trq != 0) {
+			mot_contrl_set_torque(&motor.controller, ebrk_trq);
+		}	
+		if ((mot_contrl_get_final_torque(&motor.controller) < 0.0001f && vel < CONFIG_MIN_RPM_EXIT_EBRAKE) ||
+			(!mc_throttle_released() || (mc_throttle_released() && (vel == 0.0f)))) {
+			mot_contrl_energy_recovery(&motor.controller, false);
+			throttle_torque_reset();
+		}
+	}
+}
 /*FOC 的部分处理，比如速度环，状态机，转把采集等*/
 measure_time_t g_meas_MCTask;
 #define mc_TaskStart time_measure_start(&g_meas_MCTask)
@@ -1486,14 +1528,14 @@ void Sched_MC_mTask(void) {
 #ifdef CONFIG_CRUISE_ENABLE_ACCL
 	mc_process_curise();
 #endif
-	u8 runMode = PMSM_FOC_CtrlMode();
+	u8 runMode = mot_contrl_mode(&motor.controller);
 
 	/*保护功能*/
-	u8 limted = PMSM_FOC_RunTime_Limit();
+	u8 limted = mot_contrl_protect(&motor.controller);
 	/* 母线电流，实际采集的相电流矢量大小的计算 */
-	PMSM_FOC_Calc_Current();
+	mot_contrl_calc_current(&motor.controller);
 
-	if ((PMSM_FOC_GetVbusCurrent() > (CONFIG_HW_MAX_DC_CURRENT * 1.1f)) || (PMSM_FOC_GetVbusCurrent() < CONFIG_HW_MAX_CHRG_CURRENT)) {
+	if ((mot_contrl_get_dc_current(&motor.controller) > (CONFIG_HW_MAX_DC_CURRENT * 1.1f)) || (mot_contrl_get_dc_current(&motor.controller) < CONFIG_HW_MAX_CHRG_CURRENT)) {
 		vbus_err_cnt ++;
 		if (vbus_err_cnt >= 5) {
 			if (mc_set_critical_error(FOC_CRIT_IDC_OV)) {
@@ -1534,8 +1576,7 @@ void Sched_MC_mTask(void) {
 	}
 	/* 堵转处理 */
 	if (mc_run_stall_process(runMode) || (motor.mode == CTRL_MODE_CURRENT)) {
-		eCtrl_Running();
-		PMSM_FOC_Slow_Task();
+		mot_contrl_slow_task(&motor.controller);
 		mc_motor_runstop();
 		if (motor.b_ind_start) {
 			mot_params_flux_stop();
@@ -1553,24 +1594,22 @@ void Sched_MC_mTask(void) {
 			mc_motor_runstop();
 		}
 		if (runMode != CTRL_MODE_OPEN) {
-			eCtrl_Running();
 			if (runMode == CTRL_MODE_SPD) {
 				if (mc_is_epm()) {
 					mc_process_throttle_epm();
 					mc_process_epm_move();
 				}else if (motor.s_target_speed != MAX_S16) {
-					PMSM_FOC_Set_TgtSpeed(motor.s_target_speed);
+					mot_contrl_set_target_vel(&motor.controller, motor.s_target_speed);
 				}
 			}else {
 				float thro = throttle_get_signal();
 				if (motor.b_ignor_throttle) {
 					float r = (float)motor.u_throttle_ration/100.0f;
-					thro = thro_ration_to_voltage(r);
+					thro = throttle_open_ration_to_vol(r);
 				}
-				thro_torque_process(runMode, thro);
+				mc_process_throttle_torque(thro);
 			}
-			etcs_process();
-			PMSM_FOC_Slow_Task();
+			mot_contrl_slow_task(&motor.controller);
 		}
 #endif
 	}

Applications/foc/motor/motor.h

@@ -1,10 +1,11 @@
 #ifndef _MOTOR_H__
 #define _MOTOR_H__
 #include "os/os_types.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "foc/motor/hall.h"
 #include "foc/motor/encoder.h"
 #include "foc/mc_config.h"
+#include "foc/foc_config.h"
 
 #define IDC_USER_LIMIT_NONE ((s16)0x3fff)
 #define RPM_USER_LIMIT_NONE ((s16)0x3fff)
@@ -40,7 +41,7 @@ typedef struct {
 	bool   b_cruise;
 	u32    cruise_time;
 	float  cruise_torque;
-	EPM_Dir_t epm_dir;
+	epm_dir_t epm_dir;
 	bool   b_epm_cmd_move;
 	float  f_epm_vel;
 	float  f_epm_trq;
@@ -61,10 +62,11 @@ typedef struct {
 	u8     motor_lim;
 	s16    s_vbus_hw_min;
 	s16    s_target_speed;
+	mot_contrl_t controller;
 	user_rt_set u_set; //用户运行时设置
 	fan_t  fan[2];
 }m_contrl_t;
-
+extern m_contrl_t motor;
 m_contrl_t * mc_params(void);
 void mc_init(void);
 bool mc_start(u8 mode);
@@ -80,10 +82,10 @@ bool mc_encoder_zero_calibrate(s16 vd);
 bool mc_set_foc_mode(u8 mode);
 bool mc_is_epm(void);
 bool mc_start_epm(bool epm);
-bool mc_start_epm_move(EPM_Dir_t dir, bool is_command);
+bool mc_start_epm_move(epm_dir_t dir, bool is_command);
 void mc_get_running_status(u8 *data);
-bool mc_command_epm_move(EPM_Dir_t dir);
-bool mc_throttle_epm_move(EPM_Dir_t dir);
+bool mc_command_epm_move(epm_dir_t dir);
+bool mc_throttle_epm_move(epm_dir_t dir);
 bool mc_is_start(void);
 bool mc_set_gear(u8 gear);
 u8 mc_get_gear(void);
@@ -115,6 +117,7 @@ bool mc_ind_motor_start(bool start);
 void mc_enable_brkshutpower(u8 shut);
 void mc_enable_tcs(bool enable);
 bool mc_set_target_vel(s16 vel);
+float mc_get_max_torque_with_gear_vel(s16 vel, u8 gear);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL

Applications/foc/motor/motor_param.c

@@ -1,8 +1,9 @@
 #include "bsp/bsp.h"
 #include "foc/motor/motor_param.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "math/fast_math.h"
 #include "foc/mc_config.h"
+#include "foc/motor/motor.h"
 #include "libs/logger.h"
 
 #if defined(CONFIG_MOTOR_TORQUE_CONF)
@@ -208,7 +209,7 @@ void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
 	}
 }
 #else
-void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
+void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out) {
 	float d = 0;
 	float q = 0;
 #if defined(CONFIG_MOT_ADV_ANGLE)
@@ -244,7 +245,7 @@ void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
 
 
 float motor_get_ebreak_toruqe(float rpm) {
-	float max_e_trq = PMSM_FOC_GetEbrkTorque();
+	float max_e_trq = mot_contrl_get_ebrk_torque(&motor.controller);
 	if (rpm >= 2000) {
 		return -max_e_trq;
 	}else if (rpm >= 1000) {

Applications/foc/motor/motor_param.h

@@ -2,7 +2,7 @@
 #define _MOTOR_PARAM_H__
 #include "bsp/bsp.h"
 #include "os/os_types.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 
 /* 电机外特性map, 每个转速点对应的最大扭矩 */
 typedef struct {
@@ -22,7 +22,7 @@ typedef struct {
 }iq_lq_map_t;
 typedef torque2dq_t torque_map_t;
 s16 motor_max_torque_for_rpm(s16 rpm);
-void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out);
+void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out);
 float motor_get_lq_from_iq(s16 iq);
 float motor_get_ebreak_toruqe(float rpm);
 

Applications/foc/motor/throttle.c

@@ -1,11 +1,12 @@
 #include "foc/foc_config.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "foc/samples.h"
 #include "math/fast_math.h"
 #include "bsp/bsp_driver.h"
 #include "libs/logger.h"
 #include "foc/mc_config.h"
 #include "foc/motor/throttle.h"
+#include "foc/motor/motor.h"
 #include "foc/mc_error.h"
 
 static u8 err_mask;
@@ -15,10 +16,22 @@ static int  _auto_detect_sv_cnt = 0;
 static float _auto_detect_sv_totle = 0;
 static int _detect_release_cnt = 0;
 #define CONFIG_SAFE_INV_V   0.06f
+static throttle_torque_t _throttle;
+
+void throttle_torque_reset(void) {
+	_throttle.accl = false;
+	_throttle.thro_filted = 0.0f;
+	_throttle.thro_ration = _throttle.thro_ration_last = 0.0f;
+	_throttle.torque_req = _throttle.torque_real = _throttle.torque_acc_ = 0.0f;
+	_throttle.gear = mc_get_internal_gear();
+}
+
 
 void throttle_init(void) {
 	_start_v = mc_conf()->c.thro_start_vol;
 	_end_v   = mc_conf()->c.thro_end_vol;
+	throttle_torque_reset();
+	_throttle.vel_filted = 0;
 }
 
 bool throttle1_is_error(void) {
@@ -113,6 +126,32 @@ void throttle_force_detect(void) {
 	cpu_exit_critical(mask);
 }
 
+/* 获取转把电压对应的油门开度 */
+float throttle_vol_to_open_ration(int thro_val) {
+	if (thro_val <= throttle_start_vol()) {
+		return 0;
+	}
+	float delta = thro_val - throttle_start_vol();
+	int ration = (delta * 100.0f) / throttle_vol_range();
+	return ((float)ration)/100.0f;
+}
+
+/* 获取油门开度 */
+float throttle_get_open_ration(void) {
+	float thro_val = throttle_get_signal();
+	return throttle_vol_to_open_ration(thro_val);
+}
+
+/* 获取油门开度对应的转把电压 */
+float throttle_open_ration_to_vol(float r) {
+	if (r == 0) {
+		return 0;
+	}
+	float vol = throttle_start_vol() + r * throttle_vol_range();
+	return fclamp(vol, 0, throttle_end_vol());
+}
+
+
 void throttle_detect(bool ready) {
 	float thr_5v, thr_sig;
 
@@ -164,3 +203,145 @@ void throttle_detect(bool ready) {
 		}
 	}
 }
+
+static float _throttle_torque_for_accelerate(float ration) {
+	float max_torque = mc_get_max_torque_with_gear_vel((s16)_throttle.vel_filted, _throttle.gear);
+	float thro_torque = max_torque * ration;
+
+	float acc_r = 1.0f;
+	if (_throttle.thro_ration_last < 1.0f) {
+		acc_r = (ration - _throttle.thro_ration_last)/ (1.0f - _throttle.thro_ration_last);
+	}
+	acc_r = fclamp(acc_r, 0, 1.0f);
+	float acc_torque = _throttle.torque_real + acc_r * (max_torque - _throttle.torque_real);
+	if (acc_torque < 0) {
+		acc_torque = 0;
+	}
+	/*
+	   直接获取油门开度对应的加速扭矩thro_torque 不小于间接计算得到的 acc_torque
+	*/
+	float torque_acc_ = thro_torque - acc_torque;
+	float step = 0.0f;
+	if (torque_acc_ > 0) {
+		float acc_t = mc_get_gear_config()->accl_time;
+		step = torque_acc_ / (acc_t + 0.00001f);
+	}else {
+		torque_acc_ = 0;
+	}
+	step_towards(&_throttle.torque_acc_, torque_acc_, step);
+	return (acc_torque + _throttle.torque_acc_);
+}
+
+
+static float throttle_torque_for_accelerate(void) {
+	return _throttle_torque_for_accelerate(_throttle.thro_ration);
+}
+
+static float throttle_torque_for_decelerate(void) {
+	if (_throttle.thro_ration_last == 0.0f) {
+		return 0;
+	}
+	float dec_r = _throttle.thro_ration / _throttle.thro_ration_last;
+	dec_r = fclamp(dec_r, 0.0f, 1.0f);
+	return dec_r * _throttle.torque_real;
+}
+
+float throttle_get_open_ration_filted(void) {
+	return _throttle.thro_ration;
+}
+
+#define THRO_RPM_LP_CEOF 0.01f
+float throttle_get_torque(mot_contrl_t * ctrl, float vol) {
+	float thro_r = throttle_vol_to_open_ration(vol);
+	_throttle.gear = mc_get_internal_gear();
+	LowPass_Filter(_throttle.vel_filted, mot_contrl_get_speed(ctrl), THRO_RPM_LP_CEOF);
+
+	if (thro_r > _throttle.thro_ration) {
+		if (!_throttle.accl) {
+			_throttle.thro_ration_last = _throttle.thro_ration;
+			_throttle.torque_real = _throttle.torque_req;
+			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
+				_throttle.torque_real = 0;
+			}
+			_throttle.torque_acc_ = 0;
+		}
+		_throttle.accl = true;
+	}else if (thro_r < _throttle.thro_ration) {
+		if (_throttle.accl) {
+			_throttle.thro_ration_last = _throttle.thro_ration;
+			_throttle.torque_real = ctrl->target_torque_raw;
+			/* 如果扭矩给定的ramp没有结束，使用原始扭矩请求作为减扭矩的起始点 */
+			if (_throttle.torque_req - ctrl->input_torque.interpolation >= 10.0f ) {
+				_throttle.torque_real = _throttle.torque_req;
+			}
+			if (_throttle.torque_real < 0) { //电子刹车的时候，扭矩可能为负
+				_throttle.torque_real = 0;
+			}
+		}
+		_throttle.accl = false;
+	}
+	_throttle.thro_ration = thro_r;
+	if (_throttle.accl) {
+		return throttle_torque_for_accelerate();
+	}else {
+		return throttle_torque_for_decelerate();
+	}
+}
+
+void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
+	float curr_vel = mot_contrl_get_speed(ctrl);
+	float ref_torque = torque;
+	if (_throttle.accl) { //加速需求		
+		float hold_torque = ctrl->autohold_torque;
+		if (hold_torque < 0) { //下坡驻车,最小给0扭矩
+			hold_torque = 0;
+		}
+		ref_torque	= MAX(hold_torque, ref_torque);
+		if (curr_vel <= CONFIG_ZERO_SPEED_RPM) {//从静止开始加速
+			if (_throttle.torque_req < hold_torque) {
+				_throttle.torque_req = hold_torque;
+				line_ramp_reset(&ctrl->input_torque, hold_torque);
+			}
+		}else {
+			ctrl->autohold_torque = 0;
+		}
+		/* 处理加速ramp时间的变化，需要缓慢变小，变大可以立即处理，
+		*  加速时间缓慢变小可以防止突然大扭矩加速
+		*/
+		u16 now_ramp_time = ctrl->input_torque.time;
+		u16 next_ramp_time = mc_get_gear_config()->accl_time;
+		if (curr_vel < CONFIG_ZERO_SPEED_RAMP_RMP) {
+			next_ramp_time = mc_get_gear_config()->zero_accl;
+		}
+
+		if (now_ramp_time != next_ramp_time) {
+			if (next_ramp_time > now_ramp_time) {
+				mot_contrl_set_torque_acc_time(ctrl, next_ramp_time);
+			}else {
+				float f_now = (float)now_ramp_time;
+				float f_next = (float)next_ramp_time;
+				step_towards(&f_now, f_next, 0.5f);
+				mot_contrl_set_torque_acc_time(ctrl ,(u16)f_now);
+			}
+		}
+		_throttle.torque_req = ref_torque;
+	}else {
+		float ref_torque = throttle_torque_for_decelerate();
+		/* autohold 启动的情况下，转把在0位置附近小幅抖动 */
+		if (curr_vel <= CONFIG_ZERO_SPEED_RPM) {
+			float hold_torque = ctrl->autohold_torque;
+			ref_torque = MAX(hold_torque, ref_torque);
+		}
+		_throttle.torque_req = ref_torque;
+	}
+	mot_contrl_set_torque(ctrl, _throttle.torque_req);
+}
+
+/* 定速巡航需要判断是否需要加速 */
+float get_user_request_torque(void) {
+	if (_throttle.accl) {
+		return throttle_torque_for_accelerate();
+	}
+	return throttle_torque_for_decelerate();
+}
+

Applications/foc/motor/throttle.h

@@ -6,6 +6,18 @@
 #define	THRO2_SIG_ERR_BIT 0x08
 #define THRO_NOT_RELEASED 0x10
 
+typedef struct {
+	bool  accl;
+	float torque_req;
+	float torque_real;
+	float torque_acc_;
+	float thro_filted;
+	float vel_filted;
+	float thro_ration;
+	float thro_ration_last;
+	u8    gear;
+}throttle_torque_t;
+
 void throttle_init(void);
 bool throttle_is_released(void);
 bool throttle_is_all_error(void);
@@ -18,5 +30,12 @@ bool throttle2_is_error(void);
 float throttle_start_vol(void);
 float throttle_end_vol(void);
 float throttle_vol_range(void);
+void throttle_set_torque(mot_contrl_t * ctrl, float torque);
+float throttle_get_torque(mot_contrl_t * ctrl, float vol);
+float throttle_get_open_ration_filted(void);
+float get_user_request_torque(void);
+float throttle_open_ration_to_vol(float r);
+float throttle_vol_to_open_ration(int thro_val);
+void throttle_torque_reset(void);
 #endif /* _THROTTLE_H__ */
 

Applications/prot/can_foc_msg.c

@@ -4,7 +4,7 @@
 #include "foc/commands.h"
 #include "foc/samples.h"
 #include "foc/motor/motor.h"
-#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/core/controller.h"
 #include "foc/core/foc_observer.h"
 
 
@@ -17,9 +17,9 @@ void can_report_speed(u8 can, s16 rpm) {
 
 void can_report_power(u8 can) {
 	u8 data[8];
-	s16 rpm = (s16)PMSM_FOC_GetSpeed();
+	s16 rpm = (s16)mot_contrl_get_speed(&motor.controller);
 	float vDC = get_vbus_float();
-	float iDC = PMSM_FOC_GetVbusCurrent();	
+	float iDC = mot_contrl_get_dc_current(&motor.controller);	
 	s16 v = S16Q5(vDC);
 	s16 i = S16Q5(iDC);
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Power));
@@ -33,9 +33,9 @@ void can_report_power(u8 can) {
 void can_report_phase_current(u8 can) {
 	u8 data[8];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Current));
-	encode_s16(data + 2, S16Q5(PMSM_FOC_Get()->in.s_iABC[0]));
-	encode_s16(data + 4, S16Q5(PMSM_FOC_Get()->in.s_iABC[1]));
-	encode_s16(data + 6, S16Q5(PMSM_FOC_Get()->in.s_iABC[2]));
+	encode_s16(data + 2, S16Q5(motor.controller.foc.in.curr_abc[0]));
+	encode_s16(data + 4, S16Q5(motor.controller.foc.in.curr_abc[1]));
+	encode_s16(data + 6, S16Q5(motor.controller.foc.in.curr_abc[2]));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
@@ -53,8 +53,8 @@ void can_report_phase_voltage(u8 can) {
 void can_report_dq_current(u8 can) {
 	u8 data[6];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Current));
-	float id = PMSM_FOC_Get()->out.s_FilterIdq.d;
-	float iq = PMSM_FOC_Get()->out.s_FilterIdq.q;
+	float id = motor.controller.out_idq_filterd.d;
+	float iq = motor.controller.out_idq_filterd.q;
 	encode_s16(data + 2, S16Q5(id));
 	encode_s16(data + 4, S16Q5(iq));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
@@ -70,7 +70,7 @@ void can_response_hall_offset(u8 can, int offset) {
 
 void can_report_pid_value(u8 can, u8 id) {
 	float kp, ki, kd;
-	PMSM_FOC_GetPid(id, &kp, &ki, &kd);
+	mot_contrl_get_pid(&motor.controller, id, &kp, &ki, &kd);
 	u8 data[15];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Pid));
 	data[2] = id;
@@ -90,7 +90,7 @@ void can_report_foc_status(u8 can) {
 	encode_s16(data + 9, get_mos_temp_raw());
 	encode_u24(data + 11, shark_get_seconds());
 	encode_u8(data + 14, mc_get_gear());
-	encode_u8(data + 15, mc_params()->mos_lim | (mc_params()->motor_lim<<4));
+	encode_u8(data + 15, motor.mos_lim | (motor.motor_lim<<4));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
@@ -98,11 +98,11 @@ void can_report_foc_status(u8 can) {
 void can_mcast_foc_status2(void) {
 	u8 data[8];
 	encode_u16(data, mc_get_running_status2());
-	encode_u16(data + 2, ABS((s16)(PMSM_FOC_GetSpeed())));
+	encode_u16(data + 2, ABS((s16)(mot_contrl_get_speed(&motor.controller))));
 	float vDC = get_vbus_float();
 	encode_s16(data + 4, (s16)(vDC*10));
-	float iDC = PMSM_FOC_GetVbusCurrent();
-	if (!PMSM_FOC_Is_Start()) {
+	float iDC = mot_contrl_get_dc_current(&motor.controller);
+	if (!mot_contrl_is_start(&motor.controller)) {
 		iDC = 0;
 	}
 	encode_s16(data + 6, (s16)(iDC*10));
@@ -111,13 +111,13 @@ void can_mcast_foc_status2(void) {
 
 void can_report_mpta_values(u8 can) {
 	u8 data[8];
-	if (!PMSM_FOC_Get()->in.b_MTPA_calibrate) {
+	if (!motor.controller.b_mtpa_calibrate) {
 		return;
 	}
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_MTPA_DQ_Angle));
-	encode_s16(data + 2, S16Q5(PMSM_FOC_Get()->out.s_RealCurrentFiltered));
-	encode_s16(data + 4, S16Q5(PMSM_FOC_Get()->out.s_FilterIdq.d));
-	encode_s16(data + 6, S16Q5(PMSM_FOC_Get()->out.s_FilterIdq.q));
+	encode_s16(data + 2, S16Q5(mot_contrl_get_current_vector(&motor.controller)));
+	encode_s16(data + 4, S16Q5(motor.controller.out_idq_filterd.d));
+	encode_s16(data + 6, S16Q5(motor.controller.out_idq_filterd.q));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
@@ -129,11 +129,11 @@ void can_report_ext_status(u8 can) {
 	data[1] = mc_is_start()?0:1;
 	data[1] |= (mc_is_cruise_enabled()?1:0) << 3;
 	data[1] |= mc_get_gear() << 6;
-	encode_s16(data + 2, ABS((s16)(PMSM_FOC_GetSpeed()/4.0f)));
+	encode_s16(data + 2, ABS((s16)(mot_contrl_get_speed(&motor.controller)/4.0f)));
 	float vDC = get_vbus_float();
 	encode_s16(data + 4, (s16)(vDC*10));
-	float iDC = PMSM_FOC_GetVbusCurrent();
-	if (!PMSM_FOC_Is_Start()) {
+	float iDC = mot_contrl_get_dc_current(&motor.controller);
+	if (!mot_contrl_is_start(&motor.controller)) {
 		iDC = 0;
 	}
 	encode_s16(data + 6, (s16)(iDC*10));
@@ -174,11 +174,6 @@ void can_response_vols(u8 can, u8 key) {
 	can_send_response(can, data, len);
 }
 
-void can_report_plot_values(u8 can) {
-    s16 trq_ref = eCtrl_get_RefTorque() * 10;
-	s16 real_ref = PMSM_FOC_Get_Real_dqVector() * 10;
-	can_plot2(trq_ref, real_ref);
-}
 
 void can_plot1(s16 v1) {
 	u8 data[4];

Applications/prot/can_pc_message.c

@@ -12,7 +12,7 @@ bool can_process_iap_message(can_message_t *can_message) {
 	rsplen = 3;
 	switch(can_message->key) {
 		case BUILD_CMD_KEY(0xF0):
-			if (PMSM_FOC_GetSpeed() > 10.0f) {
+			if (mot_contrl_get_speed(&motor.controller) > 10.0f) {
 				response[2] = 1;
 				iap_send_data(can_message->src, response, rsplen);
 				return true;

Project/MC105_V3.uvoptx

@@ -334,8 +334,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\e_ctrl.c</PathWithFileName>
-      <FilenameWithoutPath>e_ctrl.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\svpwm.c</PathWithFileName>
+      <FilenameWithoutPath>svpwm.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -346,8 +346,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\svpwm.c</PathWithFileName>
-      <FilenameWithoutPath>svpwm.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\mc_error.c</PathWithFileName>
+      <FilenameWithoutPath>mc_error.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -358,8 +358,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\PMSM_FOC_Core.c</PathWithFileName>
-      <FilenameWithoutPath>PMSM_FOC_Core.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\limit.c</PathWithFileName>
+      <FilenameWithoutPath>limit.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -370,8 +370,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\mc_error.c</PathWithFileName>
-      <FilenameWithoutPath>mc_error.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\smo_observer.c</PathWithFileName>
+      <FilenameWithoutPath>smo_observer.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -382,8 +382,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\limit.c</PathWithFileName>
-      <FilenameWithoutPath>limit.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\foc_observer.c</PathWithFileName>
+      <FilenameWithoutPath>foc_observer.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -394,8 +394,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\smo_observer.c</PathWithFileName>
-      <FilenameWithoutPath>smo_observer.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\adrc.c</PathWithFileName>
+      <FilenameWithoutPath>adrc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -406,8 +406,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\foc_observer.c</PathWithFileName>
-      <FilenameWithoutPath>foc_observer.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\ladrc_observer.c</PathWithFileName>
+      <FilenameWithoutPath>ladrc_observer.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -418,8 +418,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\adrc.c</PathWithFileName>
-      <FilenameWithoutPath>adrc.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\F_Calc.c</PathWithFileName>
+      <FilenameWithoutPath>F_Calc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -430,8 +430,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\thro_torque.c</PathWithFileName>
-      <FilenameWithoutPath>thro_torque.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\etcs.c</PathWithFileName>
+      <FilenameWithoutPath>etcs.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -442,8 +442,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\ladrc_observer.c</PathWithFileName>
-      <FilenameWithoutPath>ladrc_observer.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\controller.c</PathWithFileName>
+      <FilenameWithoutPath>controller.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -454,20 +454,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\F_Calc.c</PathWithFileName>
-      <FilenameWithoutPath>F_Calc.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
-    <File>
-      <GroupNumber>2</GroupNumber>
-      <FileNumber>19</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\etcs.c</PathWithFileName>
-      <FilenameWithoutPath>etcs.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\foc.c</PathWithFileName>
+      <FilenameWithoutPath>foc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -481,7 +469,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -493,7 +481,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -505,7 +493,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -517,7 +505,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -529,7 +517,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -541,7 +529,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -553,7 +541,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -565,7 +553,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -585,7 +573,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -597,7 +585,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -609,7 +597,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>29</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -629,7 +617,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -641,7 +629,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -653,7 +641,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>32</FileNumber>
       <FileType>4</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -665,7 +653,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -685,7 +673,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -697,7 +685,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -709,7 +697,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -721,7 +709,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -733,7 +721,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <FileNumber>38</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -745,7 +733,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -757,7 +745,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <FileNumber>40</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -769,7 +757,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>42</FileNumber>
+      <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -781,7 +769,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>43</FileNumber>
+      <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -793,7 +781,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>44</FileNumber>
+      <FileNumber>43</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -805,7 +793,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>45</FileNumber>
+      <FileNumber>44</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -817,7 +805,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>46</FileNumber>
+      <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -829,7 +817,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>47</FileNumber>
+      <FileNumber>46</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -849,7 +837,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>48</FileNumber>
+      <FileNumber>47</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -861,7 +849,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <FileNumber>48</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -873,7 +861,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>49</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -885,7 +873,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -897,7 +885,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -917,7 +905,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -929,7 +917,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -941,7 +929,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -961,7 +949,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -973,7 +961,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -985,7 +973,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -997,7 +985,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1009,7 +997,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1021,7 +1009,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>60</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1033,7 +1021,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>61</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1045,7 +1033,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>63</FileNumber>
+      <FileNumber>62</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1057,7 +1045,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>64</FileNumber>
+      <FileNumber>63</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1069,7 +1057,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>65</FileNumber>
+      <FileNumber>64</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1081,7 +1069,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>66</FileNumber>
+      <FileNumber>65</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1093,7 +1081,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>67</FileNumber>
+      <FileNumber>66</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1105,7 +1093,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>68</FileNumber>
+      <FileNumber>67</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1117,7 +1105,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>69</FileNumber>
+      <FileNumber>68</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1129,7 +1117,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>70</FileNumber>
+      <FileNumber>69</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1141,7 +1129,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>71</FileNumber>
+      <FileNumber>70</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1161,7 +1149,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>72</FileNumber>
+      <FileNumber>71</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1173,7 +1161,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>73</FileNumber>
+      <FileNumber>72</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1185,7 +1173,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>74</FileNumber>
+      <FileNumber>73</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/MC105_V3.uvprojx

@@ -423,21 +423,11 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\samples.c</FilePath>
             </File>
-            <File>
-              <FileName>e_ctrl.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\e_ctrl.c</FilePath>
-            </File>
             <File>
               <FileName>svpwm.c</FileName>
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\svpwm.c</FilePath>
             </File>
-            <File>
-              <FileName>PMSM_FOC_Core.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\PMSM_FOC_Core.c</FilePath>
-            </File>
             <File>
               <FileName>mc_error.c</FileName>
               <FileType>1</FileType>
@@ -463,11 +453,6 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\adrc.c</FilePath>
             </File>
-            <File>
-              <FileName>thro_torque.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\thro_torque.c</FilePath>
-            </File>
             <File>
               <FileName>ladrc_observer.c</FileName>
               <FileType>1</FileType>
@@ -483,6 +468,16 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\etcs.c</FilePath>
             </File>
+            <File>
+              <FileName>controller.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\core\controller.c</FilePath>
+            </File>
+            <File>
+              <FileName>foc.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\core\foc.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>