加入堵转判断和保护

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

Applications/bsp/board_mc_v1.h

@@ -212,5 +212,7 @@
 
 #define CONFIG_BEEP 
 
+#define CONFIG_STALL_MAX_CURRENT 100.0f //最大堵转相电流电流
+#define CONFIG_STALL_MAX_TIME    3000   //ms, 超过最大堵转电流持续时间，判断堵转
 #endif /*_BOARD_MC_V1_H__ */
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -14,9 +14,8 @@
 #include "libs/logger.h"
 #include "math/fir.h"
 
-PMSM_FOC_Ctrl _gFOC_Ctrl;
+PMSM_FOC_Ctrl gFoc_Ctrl;
 static Fir_t phase1, phase2;
-TD_t speed_td;
 
 static bool g_focinit = false;
 
@@ -27,8 +26,8 @@ static __INLINE void RevPark(DQ_t *dq, float angle, AB_t *alpha_beta) {
 #if 0
 	SinCos_Lut(angle, &s, &c);
 #else
-	s = _gFOC_Ctrl.out.sin;
-	c = _gFOC_Ctrl.out.cos;
+	s = gFoc_Ctrl.out.sin;
+	c = gFoc_Ctrl.out.cos;
 #endif
 
 	alpha_beta->a = dq->d * c - dq->q * s;
@@ -45,8 +44,8 @@ static __INLINE void Park(AB_t *alpha_beta, float angle, DQ_t *dq) {
 #if 0
 	SinCos_Lut(angle, &s, &c);
 #else
-	s = _gFOC_Ctrl.out.sin;
-	c = _gFOC_Ctrl.out.cos;
+	s = gFoc_Ctrl.out.sin;
+	c = gFoc_Ctrl.out.cos;
 #endif
 	dq->d = alpha_beta->a * c + alpha_beta->b * s;
 	dq->q = -alpha_beta->a * s + alpha_beta->b * c;
@@ -122,105 +121,104 @@ static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, float target) {
 
 
 static void PMSM_FOC_Reset_PID(void) {
-	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_id, 0);
-	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_iq, 0);
-	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_spd, 0);
-	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_fw, 0);
-	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, 0);
-	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_lock, 0);
-	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_power, 0);
+	PI_Controller_Reset(gFoc_Ctrl.pi_id, 0);
+	PI_Controller_Reset(gFoc_Ctrl.pi_iq, 0);
+	PI_Controller_Reset(gFoc_Ctrl.pi_speed, 0);
+	PI_Controller_Reset(gFoc_Ctrl.pi_fw, 0);
+	PI_Controller_Reset(gFoc_Ctrl.pi_torque, 0);
+	PI_Controller_Reset(gFoc_Ctrl.pi_lock, 0);
+	PI_Controller_Reset(gFoc_Ctrl.pi_power, 0);
 }
 
 static void PMSM_FOC_Conf_PID(void) {
-	_gFOC_Ctrl.pi_ctl_id->kp = nv_get_foc_params()->pid_conf[PID_D_id].kp;
-	_gFOC_Ctrl.pi_ctl_id->ki = nv_get_foc_params()->pid_conf[PID_D_id].ki;
-	_gFOC_Ctrl.pi_ctl_id->kb = nv_get_foc_params()->pid_conf[PID_D_id].kb;
-	_gFOC_Ctrl.pi_ctl_id->DT = (1.0f/(float)IDQ_CTRL_TS);
+	gFoc_Ctrl.pi_id->kp = nv_get_foc_params()->pid_conf[PID_D_id].kp;
+	gFoc_Ctrl.pi_id->ki = nv_get_foc_params()->pid_conf[PID_D_id].ki;
+	gFoc_Ctrl.pi_id->kb = nv_get_foc_params()->pid_conf[PID_D_id].kb;
+	gFoc_Ctrl.pi_id->DT = (1.0f/(float)IDQ_CTRL_TS);
 
-	_gFOC_Ctrl.pi_ctl_iq->kp = nv_get_foc_params()->pid_conf[PID_Q_id].kp;
-	_gFOC_Ctrl.pi_ctl_iq->ki = nv_get_foc_params()->pid_conf[PID_Q_id].ki;
-	_gFOC_Ctrl.pi_ctl_iq->kb = nv_get_foc_params()->pid_conf[PID_Q_id].kb;
-	_gFOC_Ctrl.pi_ctl_iq->DT = (1.0f/(float)IDQ_CTRL_TS);
+	gFoc_Ctrl.pi_iq->kp = nv_get_foc_params()->pid_conf[PID_Q_id].kp;
+	gFoc_Ctrl.pi_iq->ki = nv_get_foc_params()->pid_conf[PID_Q_id].ki;
+	gFoc_Ctrl.pi_iq->kb = nv_get_foc_params()->pid_conf[PID_Q_id].kb;
+	gFoc_Ctrl.pi_iq->DT = (1.0f/(float)IDQ_CTRL_TS);
 
-	_gFOC_Ctrl.pi_ctl_trq->kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
-	_gFOC_Ctrl.pi_ctl_trq->ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
-	_gFOC_Ctrl.pi_ctl_trq->kb = nv_get_foc_params()->pid_conf[PID_TRQ_id].kb;
-	_gFOC_Ctrl.pi_ctl_trq->DT = (1.0f/(float)SPD_CTRL_TS);
+	gFoc_Ctrl.pi_torque->kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
+	gFoc_Ctrl.pi_torque->ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
+	gFoc_Ctrl.pi_torque->kb = nv_get_foc_params()->pid_conf[PID_TRQ_id].kb;
+	gFoc_Ctrl.pi_torque->DT = (1.0f/(float)SPD_CTRL_TS);
 
-	_gFOC_Ctrl.pi_ctl_spd->kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
-	_gFOC_Ctrl.pi_ctl_spd->ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
-	_gFOC_Ctrl.pi_ctl_spd->kb = nv_get_foc_params()->pid_conf[PID_Spd_id].kb;
-	_gFOC_Ctrl.pi_ctl_spd->DT = (1.0f/(float)SPD_CTRL_TS);
+	gFoc_Ctrl.pi_speed->kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
+	gFoc_Ctrl.pi_speed->ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
+	gFoc_Ctrl.pi_speed->kb = nv_get_foc_params()->pid_conf[PID_Spd_id].kb;
+	gFoc_Ctrl.pi_speed->DT = (1.0f/(float)SPD_CTRL_TS);
 
-	_gFOC_Ctrl.pi_ctl_power->kp = nv_get_foc_params()->pid_conf[PID_Pow_id].kp;
-	_gFOC_Ctrl.pi_ctl_power->ki = nv_get_foc_params()->pid_conf[PID_Pow_id].ki;
-	_gFOC_Ctrl.pi_ctl_power->kb = nv_get_foc_params()->pid_conf[PID_Pow_id].kb;
-	_gFOC_Ctrl.pi_ctl_power->DT = (1.0f/(float)SPD_CTRL_TS);
+	gFoc_Ctrl.pi_power->kp = nv_get_foc_params()->pid_conf[PID_Pow_id].kp;
+	gFoc_Ctrl.pi_power->ki = nv_get_foc_params()->pid_conf[PID_Pow_id].ki;
+	gFoc_Ctrl.pi_power->kb = nv_get_foc_params()->pid_conf[PID_Pow_id].kb;
+	gFoc_Ctrl.pi_power->DT = (1.0f/(float)SPD_CTRL_TS);
 
-	_gFOC_Ctrl.pi_ctl_lock->kp = nv_get_foc_params()->pid_conf[PID_Lock_id].kp;
-	_gFOC_Ctrl.pi_ctl_lock->ki = nv_get_foc_params()->pid_conf[PID_Lock_id].ki;
-	_gFOC_Ctrl.pi_ctl_lock->kb = nv_get_foc_params()->pid_conf[PID_Lock_id].kb;
-	_gFOC_Ctrl.pi_ctl_lock->DT = (1.0f/(float)SPD_CTRL_TS);
+	gFoc_Ctrl.pi_lock->kp = nv_get_foc_params()->pid_conf[PID_Lock_id].kp;
+	gFoc_Ctrl.pi_lock->ki = nv_get_foc_params()->pid_conf[PID_Lock_id].ki;
+	gFoc_Ctrl.pi_lock->kb = nv_get_foc_params()->pid_conf[PID_Lock_id].kb;
+	gFoc_Ctrl.pi_lock->DT = (1.0f/(float)SPD_CTRL_TS);
 
 }
 
 static void PMSM_FOC_UserInit(void) {
-	memset(&_gFOC_Ctrl.userLim, 0, sizeof(_gFOC_Ctrl.userLim));
-	_gFOC_Ctrl.userLim.s_iDCLim = nv_get_foc_params()->s_maxiDC;
-	_gFOC_Ctrl.userLim.s_motRPMLim = nv_get_foc_params()->s_maxRPM;//(MAX_SPEED);
-	_gFOC_Ctrl.userLim.s_torqueLim = nv_get_foc_params()->s_maxTorque;//MAX_TORQUE;
-	_gFOC_Ctrl.userLim.s_PhaseCurrLim = nv_get_foc_params()->s_PhaseCurrLim;
-	_gFOC_Ctrl.userLim.s_vDCMaxLim = nv_get_foc_params()->s_maxvDC;
-	_gFOC_Ctrl.userLim.s_vDCMinLim = _gFOC_Ctrl.userLim.s_vDCMaxLim / 3;
-	_gFOC_Ctrl.userLim.s_iDCeBrkLim = nv_get_foc_params()->s_iDCeBrkLim;
-	_gFOC_Ctrl.userLim.s_PhaseCurreBrkLim = nv_get_foc_params()->s_PhaseCurreBrkLim;
-	_gFOC_Ctrl.userLim.s_PhaseVoleBrkLim = _gFOC_Ctrl.hwLim.s_PhaseVolMax - 20;
+	memset(&gFoc_Ctrl.userLim, 0, sizeof(gFoc_Ctrl.userLim));
+	gFoc_Ctrl.userLim.s_iDCLim = nv_get_foc_params()->s_maxiDC;
+	gFoc_Ctrl.userLim.s_motRPMLim = nv_get_foc_params()->s_maxRPM;//(MAX_SPEED);
+	gFoc_Ctrl.userLim.s_torqueLim = nv_get_foc_params()->s_maxTorque;//MAX_TORQUE;
+	gFoc_Ctrl.userLim.s_PhaseCurrLim = nv_get_foc_params()->s_PhaseCurrLim;
+	gFoc_Ctrl.userLim.s_vDCMaxLim = nv_get_foc_params()->s_maxvDC;
+	gFoc_Ctrl.userLim.s_vDCMinLim = gFoc_Ctrl.userLim.s_vDCMaxLim / 3;
+	gFoc_Ctrl.userLim.s_iDCeBrkLim = nv_get_foc_params()->s_iDCeBrkLim;
+	gFoc_Ctrl.userLim.s_PhaseCurreBrkLim = nv_get_foc_params()->s_PhaseCurreBrkLim;
+	gFoc_Ctrl.userLim.s_PhaseVoleBrkLim = gFoc_Ctrl.hwLim.s_PhaseVolMax - 20;
 }
 
 void PMSM_FOC_CoreInit(void) {
 	Fir_init(&phase1);
 	Fir_init(&phase2);
-	_gFOC_Ctrl.pi_ctl_id = &PI_Ctrl_ID;
-	_gFOC_Ctrl.pi_ctl_iq = &PI_Ctrl_IQ;
-	_gFOC_Ctrl.pi_ctl_spd = &PI_Ctrl_Spd;
-	_gFOC_Ctrl.pi_ctl_fw = &PI_Ctrl_fw;
-	_gFOC_Ctrl.pi_ctl_trq = &PI_Ctrl_trq;
-	_gFOC_Ctrl.pi_ctl_lock = &PI_Ctrl_lock;
-	_gFOC_Ctrl.pi_ctl_power = &PI_Ctrl_Power;
-
-	TD_Init(&speed_td, 2.0f, (1.0f/(float)SPD_CTRL_TS));
+	gFoc_Ctrl.pi_id = &PI_Ctrl_ID;
+	gFoc_Ctrl.pi_iq = &PI_Ctrl_IQ;
+	gFoc_Ctrl.pi_speed = &PI_Ctrl_Spd;
+	gFoc_Ctrl.pi_fw = &PI_Ctrl_fw;
+	gFoc_Ctrl.pi_torque = &PI_Ctrl_trq;
+	gFoc_Ctrl.pi_lock = &PI_Ctrl_lock;
+	gFoc_Ctrl.pi_power = &PI_Ctrl_Power;
+
 	PMSM_FOC_Conf_PID();
 	
-	memset(&_gFOC_Ctrl.in, 0, sizeof(_gFOC_Ctrl.in));
-	memset(&_gFOC_Ctrl.out, 0, sizeof(_gFOC_Ctrl.out));
+	memset(&gFoc_Ctrl.in, 0, sizeof(gFoc_Ctrl.in));
+	memset(&gFoc_Ctrl.out, 0, sizeof(gFoc_Ctrl.out));
 	
-	_gFOC_Ctrl.hwLim.s_iDCMax = CONFIG_MAX_VBUS_CURRENT;
-	_gFOC_Ctrl.hwLim.s_motRPMMax = CONFIG_MAX_MOT_RPM;
-	_gFOC_Ctrl.hwLim.s_PhaseCurrMax = CONFIG_MAX_PHASE_CURR;
-	_gFOC_Ctrl.hwLim.s_PhaseVolMax = CONFIG_MAX_PHASE_VOL;
-	_gFOC_Ctrl.hwLim.s_vDCMax      = CONFIG_MAX_VBUS_VOLTAGE;
-	_gFOC_Ctrl.hwLim.s_torqueMax  = CONFIG_MAX_TORQUE;
+	gFoc_Ctrl.hwLim.s_iDCMax = CONFIG_MAX_VBUS_CURRENT;
+	gFoc_Ctrl.hwLim.s_motRPMMax = CONFIG_MAX_MOT_RPM;
+	gFoc_Ctrl.hwLim.s_PhaseCurrMax = CONFIG_MAX_PHASE_CURR;
+	gFoc_Ctrl.hwLim.s_PhaseVolMax = CONFIG_MAX_PHASE_VOL;
+	gFoc_Ctrl.hwLim.s_vDCMax      = CONFIG_MAX_VBUS_VOLTAGE;
+	gFoc_Ctrl.hwLim.s_torqueMax  = CONFIG_MAX_TORQUE;
 
 	if (!g_focinit) {
 		PMSM_FOC_UserInit();
 		shark_task_create(PMSM_FOC_Debug_Task, NULL);
 		g_focinit = true;
 	}
-	_gFOC_Ctrl.params.n_modulation = nv_get_foc_params()->n_modulation;//SVM_Modulation;
-	_gFOC_Ctrl.params.n_PhaseFilterCeof = nv_get_foc_params()->n_PhaseFilterCeof;//(0.2f);
-	//_gFOC_Ctrl.params.n_TrqVelLimGain = nv_get_foc_params()->n_TrqVelLimGain;
-	_gFOC_Ctrl.params.n_poles = nv_get_motor_params()->poles;//MOTOR_POLES;
-	_gFOC_Ctrl.in.s_manualAngle = INVALID_ANGLE;
-	_gFOC_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxvDC;//(MAX_vDC);
+	gFoc_Ctrl.params.n_modulation = nv_get_foc_params()->n_modulation;//SVM_Modulation;
+	gFoc_Ctrl.params.n_PhaseFilterCeof = nv_get_foc_params()->n_PhaseFilterCeof;//(0.2f);
+	//gFoc_Ctrl.params.n_TrqVelLimGain = nv_get_foc_params()->n_TrqVelLimGain;
+	gFoc_Ctrl.params.n_poles = nv_get_motor_params()->poles;//MOTOR_POLES;
+	gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
+	gFoc_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxvDC;//(MAX_vDC);
 
-	_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
-	_gFOC_Ctrl.out.f_vdqRation = 0;
+	gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
+	gFoc_Ctrl.out.f_vdqRation = 0;
 
-	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[0], 1);
-	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[1], 1);
+	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[0], 1);
+	FOC_DqRamp_init(&gFoc_Ctrl.idq_ctl[1], 1);
 
-	FOC_DqRamp_init(&_gFOC_Ctrl.vdq_ctl[0], (IDQ_CTRL_TS/VDQ_RAMP_TS));
-	FOC_DqRamp_init(&_gFOC_Ctrl.vdq_ctl[1], (IDQ_CTRL_TS/VDQ_RAMP_TS));	
+	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[0], (IDQ_CTRL_TS/VDQ_RAMP_TS));
+	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[1], (IDQ_CTRL_TS/VDQ_RAMP_TS));	
 	PMSM_FOC_Reset_PID();
 }
 
@@ -229,79 +227,79 @@ void PMSM_FOC_CoreInit(void) {
 static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	AB_t vAB;
 #ifdef PHASE_LFP	
-	float *iabc = _gFOC_Ctrl.in.s_iABCFilter;
+	float *iabc = gFoc_Ctrl.in.s_iABCFilter;
 #elif defined PHASE_LFP_FIR
-	float *iabc = _gFOC_Ctrl.in.s_iABCFilter;
+	float *iabc = gFoc_Ctrl.in.s_iABCFilter;
 #else
-	float *iabc = _gFOC_Ctrl.in.s_iABC;
+	float *iabc = gFoc_Ctrl.in.s_iABC;
 #endif
 
-	if (!_gFOC_Ctrl.in.b_MTPA_calibrate && (_gFOC_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
-		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_manualAngle;
-		_gFOC_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
+	if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
+		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_manualAngle;
+		gFoc_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
 	}else {
-		_gFOC_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
-		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_hallAngle;
+		gFoc_Ctrl.in.s_hallAngle = motor_encoder_get_angle();
+		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_hallAngle;
 	}
-	SinCos_Lut(_gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.sin, &_gFOC_Ctrl.out.cos);
+	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
 	
-	_gFOC_Ctrl.in.s_motRPM = motor_encoder_get_speed();
-	_gFOC_Ctrl.in.s_vDC = get_vbus_int();
+	gFoc_Ctrl.in.s_motRPM = motor_encoder_get_speed();
+	gFoc_Ctrl.in.s_vDC = get_vbus_int();
 	//sample current
-	phase_current_get(_gFOC_Ctrl.in.s_iABC);
-	get_phase_vols(_gFOC_Ctrl.in.s_vABC);
-	_gFOC_Ctrl.in.s_vABC[0] -= _gFOC_Ctrl.in.s_vDC/2.0f;
-	_gFOC_Ctrl.in.s_vABC[1] -= _gFOC_Ctrl.in.s_vDC/2.0f;
-	_gFOC_Ctrl.in.s_vABC[2] -= _gFOC_Ctrl.in.s_vDC/2.0f;
+	phase_current_get(gFoc_Ctrl.in.s_iABC);
+	get_phase_vols(gFoc_Ctrl.in.s_vABC);
+	gFoc_Ctrl.in.s_vABC[0] -= gFoc_Ctrl.in.s_vDC/2.0f;
+	gFoc_Ctrl.in.s_vABC[1] -= gFoc_Ctrl.in.s_vDC/2.0f;
+	gFoc_Ctrl.in.s_vABC[2] -= gFoc_Ctrl.in.s_vDC/2.0f;
 
-	Clark(_gFOC_Ctrl.in.s_vABC[0], _gFOC_Ctrl.in.s_vABC[1], _gFOC_Ctrl.in.s_vABC[2], &vAB);
+	Clark(gFoc_Ctrl.in.s_vABC[0], gFoc_Ctrl.in.s_vABC[1], gFoc_Ctrl.in.s_vABC[2], &vAB);
 	
-	Park(&vAB, _gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.s_RealVdq);
+	Park(&vAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealVdq);
 
 #ifdef PHASE_LFP
-	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[0], _gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.params.n_PhaseFilterCeof);
-	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[1], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.params.n_PhaseFilterCeof);
-	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[2], _gFOC_Ctrl.in.s_iABC[2], _gFOC_Ctrl.params.n_PhaseFilterCeof);
+	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], gFoc_Ctrl.params.n_PhaseFilterCeof);
+	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], gFoc_Ctrl.params.n_PhaseFilterCeof);
+	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[2], gFoc_Ctrl.in.s_iABC[2], gFoc_Ctrl.params.n_PhaseFilterCeof);
 #elif defined PHASE_LFP_FIR
-	_gFOC_Ctrl.in.s_iABCFilter[1] = Fir_Filter(&phase1, _gFOC_Ctrl.in.s_iABC[1]);
-	_gFOC_Ctrl.in.s_iABCFilter[2] = Fir_Filter(&phase2, _gFOC_Ctrl.in.s_iABC[2]);
-	_gFOC_Ctrl.in.s_iABCFilter[0] = -(_gFOC_Ctrl.in.s_iABCFilter[1] + _gFOC_Ctrl.in.s_iABCFilter[2]);
+	gFoc_Ctrl.in.s_iABCFilter[1] = Fir_Filter(&phase1, gFoc_Ctrl.in.s_iABC[1]);
+	gFoc_Ctrl.in.s_iABCFilter[2] = Fir_Filter(&phase2, gFoc_Ctrl.in.s_iABC[2]);
+	gFoc_Ctrl.in.s_iABCFilter[0] = -(gFoc_Ctrl.in.s_iABCFilter[1] + gFoc_Ctrl.in.s_iABCFilter[2]);
 #endif
 	Clark(iabc[0], iabc[1], iabc[2], &vAB);
 
-	Park(&vAB, _gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.s_RealIdq);
+	Park(&vAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
 
 }
 
 static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 	/* update id pi ctrl */
-	_gFOC_Ctrl.params.maxvDQ.d = _gFOC_Ctrl.in.s_vDC;//MAX_vDC;
-	_gFOC_Ctrl.params.minvDQ.d = -_gFOC_Ctrl.in.s_vDC;//MAX_vDC;
-	_gFOC_Ctrl.params.maxvDQ.q = _gFOC_Ctrl.in.s_vDC;//MAX_vDC;
-	_gFOC_Ctrl.params.minvDQ.q = -_gFOC_Ctrl.in.s_vDC;//MAX_vDC;
+	gFoc_Ctrl.params.maxvDQ.d = gFoc_Ctrl.in.s_vDC;//MAX_vDC;
+	gFoc_Ctrl.params.minvDQ.d = -gFoc_Ctrl.in.s_vDC;//MAX_vDC;
+	gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.in.s_vDC;//MAX_vDC;
+	gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.in.s_vDC;//MAX_vDC;
 
-	if (_gFOC_Ctrl.params.maxvDQ.d != _gFOC_Ctrl.pi_ctl_id->max) {
-		_gFOC_Ctrl.pi_ctl_id->max = _gFOC_Ctrl.params.maxvDQ.d;
+	if (gFoc_Ctrl.params.maxvDQ.d != gFoc_Ctrl.pi_id->max) {
+		gFoc_Ctrl.pi_id->max = gFoc_Ctrl.params.maxvDQ.d;
 	}
-	if (_gFOC_Ctrl.params.minvDQ.d != _gFOC_Ctrl.pi_ctl_id->min) {
-		_gFOC_Ctrl.pi_ctl_id->min = _gFOC_Ctrl.params.minvDQ.d;
+	if (gFoc_Ctrl.params.minvDQ.d != gFoc_Ctrl.pi_id->min) {
+		gFoc_Ctrl.pi_id->min = gFoc_Ctrl.params.minvDQ.d;
 	}
 	/* update iq pi ctrl */
-	if (_gFOC_Ctrl.params.maxvDQ.q != _gFOC_Ctrl.pi_ctl_iq->max) {
-		_gFOC_Ctrl.pi_ctl_iq->max = _gFOC_Ctrl.params.maxvDQ.q;
+	if (gFoc_Ctrl.params.maxvDQ.q != gFoc_Ctrl.pi_iq->max) {
+		gFoc_Ctrl.pi_iq->max = gFoc_Ctrl.params.maxvDQ.q;
 	}
-	if (_gFOC_Ctrl.params.minvDQ.q != _gFOC_Ctrl.pi_ctl_iq->min) {
-		_gFOC_Ctrl.pi_ctl_iq->min = _gFOC_Ctrl.params.minvDQ.q;
+	if (gFoc_Ctrl.params.minvDQ.q != gFoc_Ctrl.pi_iq->min) {
+		gFoc_Ctrl.pi_iq->min = gFoc_Ctrl.params.minvDQ.q;
 	}	
 }
 
 
 static u32 PMSM_FOC_Debug_Task(void *p) {
-	if (_gFOC_Ctrl.in.b_motEnable) {
-		//plot_3data16(FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[0]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[1]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[2]));
-		plot_3data16(_gFOC_Ctrl.in.s_targetTorque, _gFOC_Ctrl.in.s_targetRPM, _gFOC_Ctrl.in.s_motRPM);
-		//plot_3data16(FtoS16x10(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x10(_gFOC_Ctrl.out.s_RealIdq.q), FtoS16x10(_gFOC_Ctrl.idq_ctl[1].s_FinalTgt));
-		//plot_3data16( _gFOC_Ctrl.in.s_motRPM, speed_td.target, speed_td.diff);
+	if (gFoc_Ctrl.in.b_motEnable) {
+		//plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
+		plot_3data16(gFoc_Ctrl.in.s_targetTorque, gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motRPM);
+		//plot_3data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q), FtoS16x10(gFoc_Ctrl.idq_ctl[1].s_FinalTgt));
+		//plot_3data16( gFoc_Ctrl.in.s_motRPM, speed_td.target, speed_td.diff);
 	}
 	return 1;
 }
@@ -309,42 +307,42 @@ static u32 PMSM_FOC_Debug_Task(void *p) {
 void PMSM_FOC_Schedule(void) {
 	AB_t vAB;
 
-	_gFOC_Ctrl.ctrl_count++;
+	gFoc_Ctrl.ctrl_count++;
 
 	PMSM_FOC_Update_Hardware();
 
-	if (_gFOC_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
+	if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
 
 		PMSM_FOC_Update_PI_Idq();
 	
-		float target_d = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[0]);
-		float err = target_d - _gFOC_Ctrl.out.s_RealIdq.d;
-		_gFOC_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(_gFOC_Ctrl.pi_ctl_id, err);
+		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
+		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
+		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(gFoc_Ctrl.pi_id, err);
 
-		float target_q = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[1]);
-		err = target_q - _gFOC_Ctrl.out.s_RealIdq.q;
-		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(_gFOC_Ctrl.pi_ctl_iq, err);
-		_gFOC_Ctrl.out.test_targetIQ = target_q;
+		float target_q = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[1]);
+		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
+		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(gFoc_Ctrl.pi_iq, err);
+		gFoc_Ctrl.out.test_targetIQ = target_q;
 
 	}else {
-		_gFOC_Ctrl.in.s_targetVdq.d = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[0]);
-		_gFOC_Ctrl.in.s_targetVdq.q = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[1]);	
+		gFoc_Ctrl.in.s_targetVdq.d = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[0]);
+		gFoc_Ctrl.in.s_targetVdq.q = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[1]);	
 	}
 
-	_gFOC_Ctrl.out.f_vdqRation = Circle_Limitation(&_gFOC_Ctrl.in.s_targetVdq, _gFOC_Ctrl.in.s_vDC, _gFOC_Ctrl.params.n_modulation, &_gFOC_Ctrl.out.s_OutVdq);
+	gFoc_Ctrl.out.f_vdqRation = Circle_Limitation(&gFoc_Ctrl.in.s_targetVdq, gFoc_Ctrl.in.s_vDC, gFoc_Ctrl.params.n_modulation, &gFoc_Ctrl.out.s_OutVdq);
 	
-	RevPark(&_gFOC_Ctrl.out.s_OutVdq, _gFOC_Ctrl.in.s_motAngle, &vAB);
+	RevPark(&gFoc_Ctrl.out.s_OutVdq, gFoc_Ctrl.in.s_motAngle, &vAB);
 	
-	SVM_Duty_Fix(&vAB, _gFOC_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &_gFOC_Ctrl.out);
+	SVM_Duty_Fix(&vAB, gFoc_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &gFoc_Ctrl.out);
 
-	phase_current_point(&_gFOC_Ctrl.out);
+	phase_current_point(&gFoc_Ctrl.out);
 	
-	pwm_update_duty(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
-	pwm_update_sample(_gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2, _gFOC_Ctrl.out.n_CPhases);
+	pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);
+	pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);
 
-	if (_gFOC_Ctrl.ctrl_count % 5 == 0) {
-		//plot_3data16(FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[0]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[1]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[2]));
-		//plot_3data16((s16)_gFOC_Ctrl.out.s_RealIdq.d, (s16)_gFOC_Ctrl.out.s_RealIdq.q, (s16)_gFOC_Ctrl.idq_ctl[1].s_Cp);
+	if (gFoc_Ctrl.ctrl_count % 5 == 0) {
+		//plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
+		//plot_3data16((s16)gFoc_Ctrl.out.s_RealIdq.d, (s16)gFoc_Ctrl.out.s_RealIdq.q, (s16)gFoc_Ctrl.idq_ctl[1].s_Cp);
 	}
 }
 
@@ -354,186 +352,186 @@ void PMSM_FOC_LogDebug(void) {
 
 /*called in media task */
 u8 PMSM_FOC_CtrlMode(void) {
-	u8 preMode = _gFOC_Ctrl.out.n_RunMode;
-	if (!_gFOC_Ctrl.in.b_motEnable) {
-		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
-	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_OPEN) {
-		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
-	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_SPD || _gFOC_Ctrl.in.b_cruiseEna){
-		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_SPD;
-	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT) {
-		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT;
-	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT_BRK) {
-		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT_BRK;
+	u8 preMode = gFoc_Ctrl.out.n_RunMode;
+	if (!gFoc_Ctrl.in.b_motEnable) {
+		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
+	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_OPEN) {
+		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
+	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_SPD || gFoc_Ctrl.in.b_cruiseEna){
+		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_SPD;
+	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT) {
+		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT;
+	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT_BRK) {
+		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT_BRK;
 	}else {
-		if (!_gFOC_Ctrl.in.b_cruiseEna) {
-			_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
+		if (!gFoc_Ctrl.in.b_cruiseEna) {
+			gFoc_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
 		}
 	}
-	if (preMode != _gFOC_Ctrl.out.n_RunMode) {
-		if ((preMode == CTRL_MODE_SPD) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
-			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, _gFOC_Ctrl.in.s_targetTorque);
-		}else if ((preMode == CTRL_MODE_TRQ) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
-			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_spd, _gFOC_Ctrl.in.s_targetTorque);
-		}else if ((preMode == CTRL_MODE_CURRENT) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
-			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, _gFOC_Ctrl.in.s_targetTorque);
-		}else if ((preMode == CTRL_MODE_TRQ) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT)) {
+	if (preMode != gFoc_Ctrl.out.n_RunMode) {
+		if ((preMode == CTRL_MODE_SPD) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
+			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
+			PI_Controller_Reset(gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque);
+		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
+			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT)) {
 			
 		}	
 	}
-	return _gFOC_Ctrl.out.n_RunMode;
+	return gFoc_Ctrl.out.n_RunMode;
 }
 
 /* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
 static __INLINE float PMSM_FOC_Limit_Power(float maxTrq) {
 #if 0
 	PI_Ctrl_Power.max = maxTrq;
-	float errRef = _gFOC_Ctrl.userLim.s_iDCLim - _gFOC_Ctrl.out.s_FilteriDC;
-	return PI_Controller_run(_gFOC_Ctrl.pi_ctl_power, errRef);
+	float errRef = gFoc_Ctrl.userLim.s_iDCLim - gFoc_Ctrl.out.s_FilteriDC;
+	return PI_Controller_run(gFoc_Ctrl.pi_power, errRef);
 #else
 	return maxTrq;
 #endif
 }
 static __INLINE void PMSM_FOC_idq_Assign(void) {
-	if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT || _gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
-		if (_gFOC_Ctrl.in.b_MTPA_calibrate && (_gFOC_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
+	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT || gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
+		if (gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
 			float s, c;
-			normal_sincosf(degree_2_pi(_gFOC_Ctrl.in.s_manualAngle + 90.0f), &s, &c);
-			_gFOC_Ctrl.in.s_targetIdq.d = _gFOC_Ctrl.in.s_targetCurrent * c;
-			_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetCurrent * s;
+			normal_sincosf(degree_2_pi(gFoc_Ctrl.in.s_manualAngle + 90.0f), &s, &c);
+			gFoc_Ctrl.in.s_targetIdq.d = gFoc_Ctrl.in.s_targetCurrent * c;
+			gFoc_Ctrl.in.s_targetIdq.q = gFoc_Ctrl.in.s_targetCurrent * s;
 		}else {
-			_gFOC_Ctrl.in.s_targetIdq.d = 0;
-			_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetCurrent;
+			gFoc_Ctrl.in.s_targetIdq.d = 0;
+			gFoc_Ctrl.in.s_targetIdq.q = gFoc_Ctrl.in.s_targetCurrent;
 		}
-	}else if ((_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) || (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
-		torque_get_idq(_gFOC_Ctrl.in.s_targetTorque, _gFOC_Ctrl.in.s_motRPM, &_gFOC_Ctrl.in.s_targetIdq);
+	}else if ((gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) || (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
+		torque_get_idq(gFoc_Ctrl.in.s_targetTorque, gFoc_Ctrl.in.s_motRPM, &gFoc_Ctrl.in.s_targetIdq);
 	}
 
-	FOC_Set_iDqRamp(&_gFOC_Ctrl.idq_ctl[0], _gFOC_Ctrl.in.s_targetIdq.d);
-	FOC_Set_iDqRamp(&_gFOC_Ctrl.idq_ctl[1], _gFOC_Ctrl.in.s_targetIdq.q);
+	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[0], gFoc_Ctrl.in.s_targetIdq.d);
+	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[1], gFoc_Ctrl.in.s_targetIdq.q);
 }
 
 /*called in media task */
 void PMSM_FOC_idqCalc(void) {
-	if (_gFOC_Ctrl.in.b_motLock) {
+	if (gFoc_Ctrl.in.b_motLock) {
 		float vel_count = motor_encoder_get_vel_count();
 		float errRef = 0 - vel_count;
-		_gFOC_Ctrl.in.s_targetTorque = PI_Controller_run(_gFOC_Ctrl.pi_ctl_lock ,errRef);
+		gFoc_Ctrl.in.s_targetTorque = PI_Controller_run(gFoc_Ctrl.pi_lock ,errRef);
 		PMSM_FOC_idq_Assign();
 		return;
 	}
-	if ((_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) || (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK)) {
-		_gFOC_Ctrl.in.s_targetCurrent = eCtrl_get_RefCurrent();
-		if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
-			if (eCtrl_get_FinalCurrent() < 0.0001f && _gFOC_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE) {
-				_gFOC_Ctrl.in.s_targetCurrent = 0;
+	if ((gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) || (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK)) {
+		gFoc_Ctrl.in.s_targetCurrent = eCtrl_get_RefCurrent();
+		if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
+			if (eCtrl_get_FinalCurrent() < 0.0001f && gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE) {
+				gFoc_Ctrl.in.s_targetCurrent = 0;
 			}
 		}
-	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
+	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 		float refTorque = eCtrl_get_FinalTorque();
-		_gFOC_Ctrl.pi_ctl_trq->max = refTorque;
-		_gFOC_Ctrl.pi_ctl_trq->min = -_gFOC_Ctrl.userLim.s_PhaseCurreBrkLim;
-		if ((eCtrl_get_FinalTorque() <= 0.0001f) && (_gFOC_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
-			_gFOC_Ctrl.pi_ctl_trq->max = 0;
-			_gFOC_Ctrl.pi_ctl_trq->min = 0; //防止倒转
+		gFoc_Ctrl.pi_torque->max = refTorque;
+		gFoc_Ctrl.pi_torque->min = -gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+		if ((eCtrl_get_FinalTorque() <= 0.0001f) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
+			gFoc_Ctrl.pi_torque->max = 0;
+			gFoc_Ctrl.pi_torque->min = 0; //防止倒转
 		}
-		float errRef = _gFOC_Ctrl.userLim.s_motRPMLim - _gFOC_Ctrl.in.s_motRPM;
-		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_trq, errRef);
-		_gFOC_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
-	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
-		_gFOC_Ctrl.pi_ctl_spd->max = _gFOC_Ctrl.userLim.s_PhaseCurrLim;
-		_gFOC_Ctrl.pi_ctl_spd->min = 0;//-_gFOC_Ctrl.userLim.s_PhaseCurrLim;
+		float errRef = gFoc_Ctrl.userLim.s_motRPMLim - gFoc_Ctrl.in.s_motRPM;
+		float maxTrq = PI_Controller_RunSat(gFoc_Ctrl.pi_torque, errRef);
+		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
+	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
+		gFoc_Ctrl.pi_speed->max = gFoc_Ctrl.userLim.s_PhaseCurrLim;
+		gFoc_Ctrl.pi_speed->min = 0;//-gFoc_Ctrl.userLim.s_PhaseCurrLim;
 		float refSpeed = eCtrl_get_RefSpeed();
-		if (_gFOC_Ctrl.in.b_cruiseEna) {
-			refSpeed = _gFOC_Ctrl.in.s_cruiseRPM;
+		if (gFoc_Ctrl.in.b_cruiseEna) {
+			refSpeed = gFoc_Ctrl.in.s_cruiseRPM;
 		}else {
-			if ((eCtrl_get_FinalSpeed() == 0) && (_gFOC_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
-				_gFOC_Ctrl.pi_ctl_spd->max = 0;
-				_gFOC_Ctrl.pi_ctl_spd->min = 0; //防止倒转
+			if ((eCtrl_get_FinalSpeed() == 0) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
+				gFoc_Ctrl.pi_speed->max = 0;
+				gFoc_Ctrl.pi_speed->min = 0; //防止倒转
 			}
 		}
-		_gFOC_Ctrl.in.s_targetRPM = refSpeed;
-		float errRef = refSpeed - _gFOC_Ctrl.in.s_motRPM;
-		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_spd, errRef);
-		_gFOC_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
+		gFoc_Ctrl.in.s_targetRPM = refSpeed;
+		float errRef = refSpeed - gFoc_Ctrl.in.s_motRPM;
+		float maxTrq = PI_Controller_RunSat(gFoc_Ctrl.pi_speed, errRef);
+		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
 	}
-	TD_run(&speed_td, _gFOC_Ctrl.in.s_motRPM);
+
 	PMSM_FOC_idq_Assign();
 }
 
 PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
-	return &_gFOC_Ctrl;
+	return &gFoc_Ctrl;
 }
 
 void PMSM_FOC_Start(u8 nCtrlMode) {
-	if (_gFOC_Ctrl.in.b_motEnable) {
+	if (gFoc_Ctrl.in.b_motEnable) {
 		return;
 	}
 	PMSM_FOC_CoreInit();
-	_gFOC_Ctrl.in.n_ctlMode = nCtrlMode;
-	_gFOC_Ctrl.in.b_motEnable = true;
+	gFoc_Ctrl.in.n_ctlMode = nCtrlMode;
+	gFoc_Ctrl.in.b_motEnable = true;
 }
 
 void PMSM_FOC_Stop(void) {
-	if (!_gFOC_Ctrl.in.b_motEnable) {
+	if (!gFoc_Ctrl.in.b_motEnable) {
 		return;
 	}
 	PMSM_FOC_CoreInit();
-	_gFOC_Ctrl.in.b_motEnable = false;
+	gFoc_Ctrl.in.b_motEnable = false;
 }
 
 bool PMSM_FOC_Is_Start(void) {
-	return _gFOC_Ctrl.in.b_motEnable;
+	return gFoc_Ctrl.in.b_motEnable;
 }
 
 void PMSM_FOC_iBusLimit(float ibusLimit) {
-	_gFOC_Ctrl.userLim.s_iDCLim = (ibusLimit);
+	gFoc_Ctrl.userLim.s_iDCLim = (ibusLimit);
 }
 
 float PMSM_FOC_GetiBusLimit(void) {
-	return _gFOC_Ctrl.userLim.s_iDCLim;
+	return gFoc_Ctrl.userLim.s_iDCLim;
 }
 
 
 void PMSM_FOC_SpeedLimit(float speedLimit) {
-	if (speedLimit > _gFOC_Ctrl.hwLim.s_motRPMMax) {
-		speedLimit = _gFOC_Ctrl.hwLim.s_motRPMMax;
+	if (speedLimit > gFoc_Ctrl.hwLim.s_motRPMMax) {
+		speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
 	}
-	_gFOC_Ctrl.userLim.s_motRPMLim = (speedLimit);
+	gFoc_Ctrl.userLim.s_motRPMLim = (speedLimit);
 }
 
 float PMSM_FOC_GetSpeedLimit(void) {
-	return _gFOC_Ctrl.userLim.s_motRPMLim;
+	return gFoc_Ctrl.userLim.s_motRPMLim;
 }
 
 void PMSM_FOC_TorqueLimit(float torqueLimit) {
-	if (torqueLimit > _gFOC_Ctrl.hwLim.s_torqueMax) {
-		torqueLimit = _gFOC_Ctrl.hwLim.s_torqueMax;
+	if (torqueLimit > gFoc_Ctrl.hwLim.s_torqueMax) {
+		torqueLimit = gFoc_Ctrl.hwLim.s_torqueMax;
 	}
-	_gFOC_Ctrl.userLim.s_torqueLim = torqueLimit;
+	gFoc_Ctrl.userLim.s_torqueLim = torqueLimit;
 }
 float PMSM_FOC_GetTorqueLimit(void) {
-	return _gFOC_Ctrl.userLim.s_torqueLim;
+	return gFoc_Ctrl.userLim.s_torqueLim;
 }
 
 void PMSM_FOC_SeteBrkPhaseCurrent(float curr) {
-	_gFOC_Ctrl.userLim.s_PhaseCurreBrkLim = curr;
+	gFoc_Ctrl.userLim.s_PhaseCurreBrkLim = curr;
 }
 
 float PMSM_FOC_GeteBrkPhaseCurrent(void) {
-	return _gFOC_Ctrl.userLim.s_PhaseCurreBrkLim ;
+	return gFoc_Ctrl.userLim.s_PhaseCurreBrkLim ;
 }
 
 float PMSM_FOC_GetVbusVoltage(void) {
-	return _gFOC_Ctrl.in.s_vDC;
+	return gFoc_Ctrl.in.s_vDC;
 }
 
 float PMSM_FOC_GetVbusCurrent(void) {
-	return _gFOC_Ctrl.out.s_FilteriDC;
+	return gFoc_Ctrl.out.s_FilteriDC;
 }
 
 DQ_t* PMSM_FOC_GetDQCurrent(void) {
-	return &_gFOC_Ctrl.out.s_RealIdq;
+	return &gFoc_Ctrl.out.s_RealIdq;
 }
 
 bool PMSM_FOC_SetCtrlMode(u8 mode) {
@@ -541,74 +539,74 @@ bool PMSM_FOC_SetCtrlMode(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Param_Err);
 		return false;
 	}
-	_gFOC_Ctrl.in.n_ctlMode = mode;
+	gFoc_Ctrl.in.n_ctlMode = mode;
 	return true;
 }
 
 void PMSM_FOC_GetRunningStatus(u8 *data) {
-	data[0] = _gFOC_Ctrl.in.n_ctlMode;
-	data[0] |= _gFOC_Ctrl.out.n_RunMode << 2;
-	data[0] |= (_gFOC_Ctrl.in.b_cruiseEna?1:0) << 4;
+	data[0] = gFoc_Ctrl.in.n_ctlMode;
+	data[0] |= gFoc_Ctrl.out.n_RunMode << 2;
+	data[0] |= (gFoc_Ctrl.in.b_cruiseEna?1:0) << 4;
 	data[0] |= (PMSM_FOC_Is_CruiseEnabled()?1:0) << 5;
 	data[0] |= (PMSM_FOC_is_epmMode()?1:0) << 6;
 	data[0] |= (0) << 7; //motor locked
 }
 
 u8 PMSM_FOC_GetCtrlMode(void) {
-	return _gFOC_Ctrl.in.n_ctlMode;
+	return gFoc_Ctrl.in.n_ctlMode;
 }
 
 void PMSM_FOC_PhaseCurrLim(float lim) {
-	if (lim > _gFOC_Ctrl.hwLim.s_PhaseCurrMax) {
-		lim = _gFOC_Ctrl.hwLim.s_PhaseCurrMax;
+	if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
+		lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
 	}
-	_gFOC_Ctrl.userLim.s_PhaseCurrLim = lim;
+	gFoc_Ctrl.userLim.s_PhaseCurrLim = lim;
 }
 
 float PMSM_FOC_GetPhaseCurrLim(void) {
-	return _gFOC_Ctrl.userLim.s_PhaseCurrLim;
+	return gFoc_Ctrl.userLim.s_PhaseCurrLim;
 }
 
 void PMSM_FOC_SetOpenVdq(float vd, float vq) {
-	FOC_Set_vDqRamp(&_gFOC_Ctrl.vdq_ctl[0], vd);
-	FOC_Set_vDqRamp(&_gFOC_Ctrl.vdq_ctl[1], vq);
+	FOC_Set_vDqRamp(&gFoc_Ctrl.vdq_ctl[0], vd);
+	FOC_Set_vDqRamp(&gFoc_Ctrl.vdq_ctl[1], vq);
 }
 
 
 bool PMSM_FOC_EnableCruise(bool enable) {
-	if (enable != _gFOC_Ctrl.in.b_cruiseEna) {
+	if (enable != gFoc_Ctrl.in.b_cruiseEna) {
 		float motSpd = PMSM_FOC_GetSpeed();
 		if (enable && (motSpd < CONFIG_MIN_CRUISE_RPM)) { //
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		_gFOC_Ctrl.in.s_cruiseRPM = motSpd;
-		_gFOC_Ctrl.in.b_cruiseEna = enable;
+		gFoc_Ctrl.in.s_cruiseRPM = motSpd;
+		gFoc_Ctrl.in.b_cruiseEna = enable;
 	}
 	
 	return true;
 }
 
 bool PMSM_FOC_Is_CruiseEnabled(void) {
-	return (_gFOC_Ctrl.in.b_cruiseEna && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD));
+	return (gFoc_Ctrl.in.b_cruiseEna && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD));
 }
 
 bool PMSM_FOC_Set_Speed(float rpm) {
-	if (_gFOC_Ctrl.in.b_cruiseEna) {
+	if (gFoc_Ctrl.in.b_cruiseEna) {
 		return false;
 	}
-	eCtrl_set_TgtSpeed(min(ABS(rpm), _gFOC_Ctrl.userLim.s_motRPMLim)*SIGN(rpm));
+	eCtrl_set_TgtSpeed(min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm));
 	return true;
 }
 
 bool PMSM_FOC_Set_epmMode(bool epm) {
-	if (_gFOC_Ctrl.in.b_epmMode != epm) {
+	if (gFoc_Ctrl.in.b_epmMode != epm) {
 		if (PMSM_FOC_GetSpeed() != 0.0f) {
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		_gFOC_Ctrl.in.epmDirection = EPM_Dir_None;
-		_gFOC_Ctrl.in.b_epmMode = epm;
+		gFoc_Ctrl.in.epmDirection = EPM_Dir_None;
+		gFoc_Ctrl.in.b_epmMode = epm;
 		if (epm) {
 			PMSM_FOC_SpeedLimit(nv_get_foc_params()->s_maxEpmRPM);
 			eCtrl_set_TgtSpeed(0);
@@ -622,43 +620,43 @@ bool PMSM_FOC_Set_epmMode(bool epm) {
 }
 
 bool PMSM_FOC_is_epmMode(void) {
-	return _gFOC_Ctrl.in.b_epmMode;
+	return gFoc_Ctrl.in.b_epmMode;
 }
 
 bool PMSM_FOC_Start_epmMove(bool move, EPM_Dir_t dir) {
-	if (!_gFOC_Ctrl.in.b_epmMode) {
+	if (!gFoc_Ctrl.in.b_epmMode) {
 		return false;
 	}
 	if (move) {
-		if (_gFOC_Ctrl.in.epmDirection != EPM_Dir_None) {
+		if (gFoc_Ctrl.in.epmDirection != EPM_Dir_None) {
 			return false;
 		}
-		_gFOC_Ctrl.in.epmDirection = dir;
+		gFoc_Ctrl.in.epmDirection = dir;
 	}else {
-		_gFOC_Ctrl.in.epmDirection = EPM_Dir_None;
+		gFoc_Ctrl.in.epmDirection = EPM_Dir_None;
 	}
 	return true;
 }
 
 EPM_Dir_t PMSM_FOC_Get_epmDir(void) {
-	return _gFOC_Ctrl.in.epmDirection;
+	return gFoc_Ctrl.in.epmDirection;
 }
 
 bool PMSM_FOC_Set_Current(float is) {
-	if (is > _gFOC_Ctrl.userLim.s_PhaseCurrLim) {
-		is = _gFOC_Ctrl.userLim.s_PhaseCurrLim;
-	}else if (is < -_gFOC_Ctrl.userLim.s_PhaseCurrLim) {
-		is = -_gFOC_Ctrl.userLim.s_PhaseCurrLim;
+	if (is > gFoc_Ctrl.userLim.s_PhaseCurrLim) {
+		is = gFoc_Ctrl.userLim.s_PhaseCurrLim;
+	}else if (is < -gFoc_Ctrl.userLim.s_PhaseCurrLim) {
+		is = -gFoc_Ctrl.userLim.s_PhaseCurrLim;
 	}
 	eCtrl_set_TgtCurrent(is);
 	return true;
 }
 
 bool PMSM_FOC_Set_Torque(float trq) {
-	if (trq > _gFOC_Ctrl.userLim.s_torqueLim) {
-		trq = _gFOC_Ctrl.userLim.s_torqueLim;
-	}else if (trq < -_gFOC_Ctrl.userLim.s_torqueLim) {
-		trq = -_gFOC_Ctrl.userLim.s_torqueLim;
+	if (trq > gFoc_Ctrl.userLim.s_torqueLim) {
+		trq = gFoc_Ctrl.userLim.s_torqueLim;
+	}else if (trq < -gFoc_Ctrl.userLim.s_torqueLim) {
+		trq = -gFoc_Ctrl.userLim.s_torqueLim;
 	}
 	eCtrl_set_TgtTorque(trq);
 	return true;
@@ -670,7 +668,7 @@ bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		_gFOC_Ctrl.in.s_cruiseRPM = rpm;
+		gFoc_Ctrl.in.s_cruiseRPM = rpm;
 		return true;
 	}
 	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
@@ -679,46 +677,51 @@ bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 
 void PMSM_FOC_MTPA_Calibrate(bool enable) {
 	if (enable) {
-		_gFOC_Ctrl.in.b_MTPA_calibrate = true;
-		_gFOC_Ctrl.in.s_manualAngle = 0;
+		gFoc_Ctrl.in.b_MTPA_calibrate = true;
+		gFoc_Ctrl.in.s_manualAngle = 0;
 	}else {
-		_gFOC_Ctrl.in.s_manualAngle = INVALID_ANGLE;
-		_gFOC_Ctrl.in.b_MTPA_calibrate = false;
+		gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
+		gFoc_Ctrl.in.b_MTPA_calibrate = false;
 	}
 }
 
 void PMSM_FOC_Set_Angle(float angle) {
-	_gFOC_Ctrl.in.s_manualAngle = (angle);
+	gFoc_Ctrl.in.s_manualAngle = (angle);
 }
 
 void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
-	dq->d = _gFOC_Ctrl.in.s_targetIdq.d;
-	dq->q = _gFOC_Ctrl.in.s_targetIdq.q;
+	dq->d = gFoc_Ctrl.in.s_targetIdq.d;
+	dq->q = gFoc_Ctrl.in.s_targetIdq.q;
 }
 
 float PMSM_FOC_GetSpeed(void) {
-	return _gFOC_Ctrl.in.s_motRPM;
+	return gFoc_Ctrl.in.s_motRPM;
 }
 
 
 void PMSM_FOC_LockMotor(bool lock) {
-	if (_gFOC_Ctrl.in.b_motLock != lock) {		
+	if (gFoc_Ctrl.in.b_motLock != lock) {		
 		motor_encoder_lock_pos(lock);
-		PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_lock, 0);
-		_gFOC_Ctrl.in.b_motLock = lock;
+		PI_Controller_Reset(gFoc_Ctrl.pi_lock, 0);
+		gFoc_Ctrl.in.b_motLock = lock;
 	}
 }
 
+
+bool PMSM_FOC_MotorLocking(void) {
+	return gFoc_Ctrl.in.b_motLock;
+}
+
 static PI_Controller *_pid(u8 id) {
 	PI_Controller *pi = NULL;
 	if (id == PID_D_id) {
-		pi = _gFOC_Ctrl.pi_ctl_id;
+		pi = gFoc_Ctrl.pi_id;
 	}else if (id == PID_Q_id) {
-		pi = _gFOC_Ctrl.pi_ctl_iq;
+		pi = gFoc_Ctrl.pi_iq;
 	}else if (id == PID_TRQ_id) {
-		pi = _gFOC_Ctrl.pi_ctl_trq;
+		pi = gFoc_Ctrl.pi_torque;
 	}else if (id == PID_Spd_id) {
-		pi = _gFOC_Ctrl.pi_ctl_spd;
+		pi = gFoc_Ctrl.pi_speed;
 	}
 	return pi;
 }
@@ -748,50 +751,50 @@ void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb) {
 }
 
 void PMSM_FOC_SetErrCode(u8 error) {
-	if (_gFOC_Ctrl.out.n_Error != error) {
-		_gFOC_Ctrl.out.n_Error = error;
+	if (gFoc_Ctrl.out.n_Error != error) {
+		gFoc_Ctrl.out.n_Error = error;
 	}
 }
 
 u8 PMSM_FOC_GetErrCode(void) {
-	return _gFOC_Ctrl.out.n_Error;
+	return gFoc_Ctrl.out.n_Error;
 }
 
 void PMSM_FOC_SetCriticalError(u8 err) {
-	_gFOC_Ctrl.out.n_CritiCalErrMask |= (1u << err);
+	gFoc_Ctrl.out.n_CritiCalErrMask |= (1u << err);
 }
 
 void PMSM_FOC_ClrCriticalError(u8 err) {
-	_gFOC_Ctrl.out.n_CritiCalErrMask &= ~(1u << err);
+	gFoc_Ctrl.out.n_CritiCalErrMask &= ~(1u << err);
 }
 
 u32 PMSM_FOC_GetCriticalError(void) {
-	return _gFOC_Ctrl.out.n_CritiCalErrMask;
+	return gFoc_Ctrl.out.n_CritiCalErrMask;
 }
 //获取母线电流
 float PMSM_FOC_Calc_iDC(void) {
-	float vd = _gFOC_Ctrl.out.s_OutVdq.d;
-	float vq = _gFOC_Ctrl.out.s_OutVdq.q;
+	float vd = gFoc_Ctrl.out.s_OutVdq.d;
+	float vq = gFoc_Ctrl.out.s_OutVdq.q;
 #ifdef NO_SAMPLE_IDC
-	LowPass_Filter(_gFOC_Ctrl.out.s_FilterIdq.d, _gFOC_Ctrl.out.s_RealIdq.d, 0.01f);
-	LowPass_Filter(_gFOC_Ctrl.out.s_FilterIdq.q, _gFOC_Ctrl.out.s_RealIdq.q, 0.01f);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.01f);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.01f);
 #endif
-	float id = _gFOC_Ctrl.out.s_FilterIdq.d;
-	float iq = _gFOC_Ctrl.out.s_FilterIdq.q;
+	float id = gFoc_Ctrl.out.s_FilterIdq.d;
+	float iq = gFoc_Ctrl.out.s_FilterIdq.q;
     /*
 		根据公式（等幅值变换，功率不等）:
 		iDC x vDC = 2/3(iq x vq + id x vd);
 	*/
 	float m_pow = (vd * id + vq * iq); //s32q10
-	float raw_idc = m_pow / _gFOC_Ctrl.in.s_vDC;// * 1.5f * 0.66f; //s16q5
-	LowPass_Filter(_gFOC_Ctrl.out.s_FilteriDC, raw_idc, 0.01f);
-	return _gFOC_Ctrl.out.s_FilteriDC;
+	float raw_idc = m_pow / gFoc_Ctrl.in.s_vDC;// * 1.5f * 0.66f; //s16q5
+	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.01f);
+	return gFoc_Ctrl.out.s_FilteriDC;
 }
 
 void PMSM_FOC_Brake(bool brake) {
-	_gFOC_Ctrl.in.b_eBrake = brake;
-	if (_gFOC_Ctrl.in.b_eBrake & _gFOC_Ctrl.in.b_cruiseEna) {
-		_gFOC_Ctrl.in.b_cruiseEna = false;
+	gFoc_Ctrl.in.b_eBrake = brake;
+	if (gFoc_Ctrl.in.b_eBrake & gFoc_Ctrl.in.b_cruiseEna) {
+		gFoc_Ctrl.in.b_cruiseEna = false;
 	}
 	eCtrl_brake_signal(brake);
 }

Applications/foc/core/PMSM_FOC_Core.h

@@ -108,13 +108,13 @@ typedef struct {
 }dq_Rctrl; //dq ramp ctrl
 
 typedef struct {
-	PI_Controller *pi_ctl_id;
-	PI_Controller *pi_ctl_iq;
-	PI_Controller *pi_ctl_spd;
-	PI_Controller *pi_ctl_fw;
-	PI_Controller *pi_ctl_trq;
-	PI_Controller *pi_ctl_lock;
-	PI_Controller *pi_ctl_power;
+	PI_Controller *pi_id;
+	PI_Controller *pi_iq;
+	PI_Controller *pi_speed;
+	PI_Controller *pi_fw;
+	PI_Controller *pi_torque;
+	PI_Controller *pi_lock;
+	PI_Controller *pi_power;
 	dq_Rctrl      idq_ctl[2];
 	dq_Rctrl      vdq_ctl[2];	
 	FOC_InP       in;
@@ -240,6 +240,7 @@ void PMSM_FOC_GetRunningStatus(u8 *data);
 bool PMSM_FOC_Is_CruiseEnabled(void);
 void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
 void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb);
+bool PMSM_FOC_MotorLocking(void);
 
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/foc_config.h

@@ -7,7 +7,8 @@
 #define CONFIG_THROTTLE_MAX_VALUE 3.8f /* 转把最大值 */
 #define CONFIG_THROTTLE_MIN_RPM   10   /* 转把对应最小的速度 */
 #define CONFIG_THROTTLE_MIN_IDQ   20   /* 转把对应最小的扭矩电流 Q轴 */
-#define CONFIG_MIN_CRUISE_RPM 	   1000     /* 能启动定速巡航的最小速度 */
+#define CONFIG_MIN_CRUISE_RPM 	  1000   /* 能启动定速巡航的最小速度 */
+  
 #ifdef GD32_FOC_DEMO
 #define MAX_vDC (16)   /* 母线最大电压 V*/
 #else

Applications/foc/motor/motor.c

@@ -121,6 +121,8 @@ bool mc_start(u8 mode) {
 	phase_current_calibrate_wait();
 	motor.throttle = 0;
 	motor.b_start = true;
+	motor.b_runStall = false;
+	motor.runStall_time = 0;
 	if (phase_curr_offset_check()) {
 		PMSM_FOC_SetCriticalError(FOC_CRIT_CURR_OFF_Err);
 		mc_stop();
@@ -409,6 +411,28 @@ void Sched_MC_mTask(void) {
 	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
 		return;
 	}
+	if ((runMode == CTRL_MODE_TRQ || runMode == CTRL_MODE_SPD) && !PMSM_FOC_MotorLocking()) {
+		//堵转判断
+		if (motor.b_runStall) {
+			if (!mc_throttle_released()) {
+				return;
+			}
+			motor.b_runStall = false; //转把释放，清除堵转标志
+		}else if ((ABS(PMSM_FOC_GetSpeed()) < 1.0f) && (PMSM_FOC_Get()->out.s_RealIdq.q >= CONFIG_STALL_MAX_CURRENT)){
+			if (motor.runStall_time == 0) {
+				motor.runStall_time = get_tick_ms();
+			}else {
+				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
+					motor.b_runStall = true;
+					motor.runStall_time = 0;
+					torque_speed_target(runMode, 0.0f);
+					return;
+				}
+			}
+		}else {
+			motor.runStall_time = 0;
+		}
+	}
 	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
 		if (motor.mode != CTRL_MODE_OPEN) {
 			u32 mask;

Applications/foc/motor/motor.h

@@ -10,6 +10,8 @@ typedef struct {
 	float  throttle;
 	bool   b_ignor_throttle;
 	bool   b_calibrate;
+	bool   b_runStall; //是否堵转
+	u32    runStall_time;
 	s16    s_testAngle;
 	s32    s_targetFix;
 	s8     s_direction;