扭矩模式和定速巡航相互切换

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

Applications/app/app.c

@@ -42,6 +42,10 @@ void fetch_jtag_cmd(void) {
 	}else if (jtag_cmd == 5) {
 		PMSM_FOC_Set_Current(0.5f);
 		jtag_cmd = 0;
+	}else if (jtag_cmd == 6) {
+		PMSM_FOC_EnableCruise(true);
+	}else if (jtag_cmd == 7) {
+		PMSM_FOC_EnableCruise(false);
 	}
 }
 #else

Applications/app/key_process.c

@@ -14,7 +14,7 @@
 static u8 key_value[3];
 static float foc_current = 0.0f;
 static u8 ctrl_mode = CTRL_MODE_OPEN;
-static float max_speed = 2000;
+static float max_speed = 1500;
 static u32 key_task(void *p) {
 	foc_cmd_body_t foc_cmd;
 	u8 cmd_data[16];
@@ -42,6 +42,7 @@ static u32 key_task(void *p) {
 				foc_current += 0.1f;
 			}
 			PMSM_FOC_Set_Torque(foc_current);
+			PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
 		}
 		key_value[KEY_STOP] = value;
 	}
@@ -50,13 +51,13 @@ static u32 key_task(void *p) {
 	if (value != key_value[KEY_FUNC]) {
 		if (value) {
 			if (ctrl_mode == CTRL_MODE_OPEN) {
-				ctrl_mode = CTRL_MODE_TRQ;
+				//ctrl_mode = CTRL_MODE_TRQ;
 			}else {
-				ctrl_mode = CTRL_MODE_OPEN;
+				//ctrl_mode = CTRL_MODE_OPEN;
 			}
-			PMSM_FOC_SetCtrlMode(ctrl_mode);
+			//PMSM_FOC_SetCtrlMode(ctrl_mode);
 			PMSM_FOC_SpeedLimit(max_speed);
-			//max_speed += 10;
+			max_speed += 10;
 		}
 		key_value[KEY_FUNC] = value;
 	}

Applications/foc/core/PMSM_FOC_Core.c

@@ -228,11 +228,14 @@ void PMSM_FOC_Schedule(void) {
 	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);
 
+	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);
+
 	if (_gFOC_Ctrl.ctrl_count % 5 == 0) {
 		//plot_1data16(FtoS16x1000(PMSM_FOC_Get_iDC()));
 		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
 		//plot_2data16(FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
-		plot_1data16(_gFOC_Ctrl.in.s_motRPM);
+		plot_2data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.in.s_targetTorque));
 		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(_gFOC_Ctrl.in.s_motAngle));
 		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.q));
 		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), FtoS16(_gFOC_Ctrl.in.s_hallAngle)*10);
@@ -251,6 +254,7 @@ 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) {
@@ -262,6 +266,13 @@ u8 PMSM_FOC_CtrlMode(void) {
 	}else {
 		_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);
+		}
+	}
 	return _gFOC_Ctrl.out.n_RunMode;
 }
 
@@ -289,14 +300,19 @@ void PMSM_FOC_idqCalc(void) {
 	if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
 		_gFOC_Ctrl.in.s_targetCurrent = eCtrl_get_RefCurrent();
 	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
-		_gFOC_Ctrl.pi_ctl_trq->max = eCtrl_get_RefTorque();
-		_gFOC_Ctrl.pi_ctl_trq->min = -eCtrl_get_RefTorque();
+		float refTorque = eCtrl_get_RefTorque();
+		_gFOC_Ctrl.pi_ctl_trq->max = refTorque;
+		_gFOC_Ctrl.pi_ctl_trq->min = -refTorque;
 		float errRef = _gFOC_Ctrl.params.s_maxRPM - _gFOC_Ctrl.in.s_motRPM;
 		_gFOC_Ctrl.in.s_targetTorque = PI_Controller_run(_gFOC_Ctrl.pi_ctl_trq, errRef);
 	}else {
 		_gFOC_Ctrl.pi_ctl_spd->max = _gFOC_Ctrl.params.s_maxIdq;
 		_gFOC_Ctrl.pi_ctl_spd->min = _gFOC_Ctrl.params.s_minIdq;
-		float errRef = eCtrl_get_RefSpeed() - _gFOC_Ctrl.in.s_motRPM;
+		float refSpeed = eCtrl_get_RefSpeed();
+		if (_gFOC_Ctrl.in.b_cruiseEna) {
+			refSpeed = _gFOC_Ctrl.in.s_cruiseRPM;
+		}
+		float errRef = refSpeed - _gFOC_Ctrl.in.s_motRPM;
 		_gFOC_Ctrl.in.s_targetTorque = PI_Controller_run(_gFOC_Ctrl.pi_ctl_spd, errRef);		
 	}
 	PMSM_FOC_idq_Assign();
@@ -355,7 +371,7 @@ bool PMSM_FOC_EnableCruise(bool enable) {
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		eCtrl_set_TgtSpeed(motSpd);
+		_gFOC_Ctrl.in.s_cruiseRPM = motSpd;
 		_gFOC_Ctrl.in.b_cruiseEna = enable;
 	}
 	
@@ -386,7 +402,7 @@ bool PMSM_FOC_Set_Torque(float trq) {
 
 bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 	if (PMSM_FOC_Is_CruiseEnabled()) {
-		eCtrl_set_TgtSpeed(rpm);
+		_gFOC_Ctrl.in.s_cruiseRPM = rpm;
 		return true;
 	}
 	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
@@ -455,15 +471,16 @@ u8 PMSM_FOC_GetErrCode(void) {
 float PMSM_FOC_Get_iDC(void) {
 	float vd = _gFOC_Ctrl.out.s_OutVdq.d;
 	float vq = _gFOC_Ctrl.out.s_OutVdq.q;
-	float id = _gFOC_Ctrl.out.s_RealIdq.d;
-	float iq = _gFOC_Ctrl.out.s_RealIdq.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 iDC = m_pow / _gFOC_Ctrl.in.s_vDC; //s16q5
-	return (iDC) * 0.667f;
+	_gFOC_Ctrl.out.s_FilteriDC = m_pow / _gFOC_Ctrl.in.s_vDC / 0.667f; //s16q5
+	return _gFOC_Ctrl.out.s_FilteriDC;
 }
 
 void PMSM_FOC_Brake(bool brake) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -32,6 +32,7 @@ typedef struct {
 	float 	s_motAngle; //from hall or encoder
 	float 	s_hallAngle;//from hall or encoder
 	float   s_targetRPM;
+	float   s_cruiseRPM;
 	float 	s_targetCurrent;
 	DQ_t    s_targetIdq;
 	DQ_t    s_targetVdq;
@@ -57,6 +58,8 @@ typedef struct {
 	u8    n_RunMode;
 	DQ_t  s_OutVdq;
 	DQ_t  s_RealIdq;
+	DQ_t  s_FilterIdq;
+	float s_FilteriDC;
 	float f_vdqRation;
 	u8    n_Error;
 }FOC_OutP;

Applications/foc/core/PMSM_FOC_Params.h

@@ -34,8 +34,8 @@ static PI_Controller PI_Ctrl_trq = {
 };
 
 static PI_Controller PI_Ctrl_Spd = {
-	.kp = (0.01f),
-	.ki = (0.003f),
+	.kp = 0.001f,
+	.ki = 0.001f,
 	.max = (MAX_TORQUE),
 	.min = (-MAX_TORQUE),
 	.DT  = (1.0f/(float)SPD_CTRL_TS),