电流环的给定使用ramp，在一个speed的控制周期内完成给定

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -10,7 +10,6 @@
 
 static PMSM_FOC_Ctrl _gFOC_Ctrl;
 
-static void PMSM_FOC_idq_Assign(void);
 
 static __INLINE void RevPark(DQ_t *dq, s16q5_t angle, AB_t *alpha_beta) {
 	s16q14_t c,s;
@@ -48,6 +47,33 @@ static __INLINE s16q5_t Circle_Limitation(DQ_t *vdq, s16q5_t vDC, s16q14_t modul
 	return 32; // s16q5 32 means int 1
 }
 
+static __INLINE void FOC_Set_DqRamp(idq_Ctrl *c, s16q5_t target) {
+	s32q14_t cp = c->s_Cp;
+	c->s_FinalTgt = (s32)target << 9; // to s32q14
+	c->s_Step = (c->s_FinalTgt - cp) / (IDQ_CTRL_TS/SPD_CTRL_TS);
+	if (c->s_Step == 0) {
+		if (c->s_FinalTgt - cp > 0) {
+			c->s_Step = 1<<14;	
+		}else {
+			c->s_Step = -(1 << 14);
+		}
+	}
+}
+
+static __INLINE s32q14_t FOC_Get_DqRamp(idq_Ctrl *c) {
+	c->s_Cp += c->s_Step;
+	if (c->s_Step < 0) {
+		if (c->s_Cp < c->s_Step) {
+			c->s_Cp = c->s_Step;
+		}
+	}else {
+		if (c->s_Cp > c->s_Step) {
+			c->s_Cp = c->s_Step;
+		}
+	}
+	return c->s_Cp;
+}
+
 static void PMSM_FOC_Reset_PID(void) {
 	PI_Controller_Reset(_gFOC_Ctrl.id_ctl, 0);
 	PI_Controller_Reset(_gFOC_Ctrl.iq_ctl, 0);
@@ -83,6 +109,14 @@ static __INLINE void PMSM_FOC_Update_Encoder(void) {
 	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed();
 }
 
+/* MPTA, 弱磁, 功率限制 */
+static __INLINE void PMSM_FOC_idq_Assign(void) {
+	_gFOC_Ctrl.in.s_targetIdq.d = 0;
+	_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetTrq;
+	FOC_Set_DqRamp(_gFOC_Ctrl.dq_ctl, _gFOC_Ctrl.in.s_targetIdq.d);
+	FOC_Set_DqRamp(_gFOC_Ctrl.dq_ctl+1, _gFOC_Ctrl.in.s_targetIdq.q);
+}
+
 
 void PMSM_FOC_Schedule(void) {
 	AB_t vAB;
@@ -95,10 +129,12 @@ void PMSM_FOC_Schedule(void) {
 	if (_gFOC_Ctrl.out.n_RunMode != OPEN_MODE) {
 		Clark(iabc[0], iabc[1], iabc[2], &vAB);
 		Park(&vAB, _gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.s_RealIdq);
-		s32q14_t err = _gFOC_Ctrl.in.s_targetIdq.d - _gFOC_Ctrl.out.s_RealIdq.d;
+
+		s32q14_t err = FOC_Get_DqRamp(_gFOC_Ctrl.dq_ctl) - _gFOC_Ctrl.out.s_RealIdq.d;
 		err = err << 9; //q5 to q14
 		_gFOC_Ctrl.in.s_targetVdq.d = PI_Controller_run(_gFOC_Ctrl.id_ctl, err);
-		err = _gFOC_Ctrl.in.s_targetIdq.q - _gFOC_Ctrl.out.s_RealIdq.q;
+		
+		err = FOC_Get_DqRamp(_gFOC_Ctrl.dq_ctl+1) - _gFOC_Ctrl.out.s_RealIdq.q;
 		err = err << 9; //q5 to q14
 		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_run(_gFOC_Ctrl.iq_ctl, err);
 	}
@@ -187,6 +223,7 @@ void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq) {
 	_gFOC_Ctrl.in.s_targetVdq.q = vq;
 }
 
+
 bool PMSM_FOC_EnableCruise(bool enable) {
 	if (enable != _gFOC_Ctrl.in.b_cruiseEna) {
 		s32q4_t motSpd = PMSM_FOC_GetSpeed();
@@ -213,8 +250,8 @@ bool PMSM_FOC_Set_Speed(s32q4_t rpm) {
 	return true;
 }
 
-bool PMSM_FOC_Set_Current(float current) {
-	eCtrl_set_TrqCurrent(current);
+bool PMSM_FOC_Set_Trque(float Trq) {
+	eCtrl_set_TrqCurrent(Trq);
 	return true;
 }
 
@@ -308,9 +345,3 @@ void PMSM_FOC_Brake(bool brake) {
 	eCtrl_brake_signal(brake);
 }
 
-
-/* MPTA, 弱磁, 功率限制 */
-static void PMSM_FOC_idq_Assign(void) {
-	_gFOC_Ctrl.in.s_targetIdq.d = 0;
-	_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetTrq;
-}

Applications/foc/core/PMSM_FOC_Core.h

@@ -45,6 +45,12 @@ typedef struct {
 	u8    n_Error;
 }FOC_OutP;
 
+typedef struct {
+	s32q14_t s_FinalTgt;
+	s32q14_t s_Cp;
+	s32q14_t s_Step;
+}idq_Ctrl;
+
 typedef struct {
 	PI_Controller *id_ctl;
 	PI_Controller *iq_ctl;
@@ -52,6 +58,7 @@ typedef struct {
 	PI_Controller *fw_ctl;
 	FOC_InP       in;
 	FOC_OutP      out;
+	idq_Ctrl      dq_ctl[2];
 }PMSM_FOC_Ctrl;
 
 typedef enum {
@@ -106,7 +113,7 @@ void PMSM_FOC_SetCtrlMode(u8 mode);
 void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq);
 bool PMSM_FOC_EnableCruise(bool enable);
 bool PMSM_FOC_Set_Speed(s32q4_t rpm);
-bool PMSM_FOC_Set_Current(float current);
+bool PMSM_FOC_Set_Trque(float current);
 bool PMSM_FOC_Set_CruiseSpeed(float rpm);
 s32q4_t PMSM_FOC_GetSpeed(void);
 bool PMSM_FOC_Lock_Motor(bool lock);

Applications/foc/foc_config.h

@@ -22,5 +22,9 @@
 #define eCTRL_STEP_TS 10          /* 斜率给定的step的时间值，单位 ms */
 #define eCTRL_Brake_TIME 1500     /* 捏住刹车的时间，超过这个时间启动ebrake，单位 ms */
 #define eCTRL_NEG_TORQUE (-100)   /* ebrake 的最大方向DQ电流，单位 ACKED_KEY*/
+
+#define IDQ_CTRL_TS FOC_PWM_FS
+#define SPD_CTRL_TS 100
+
 #endif /* _FOC_CONFIG_H__ */
 

Applications/foc/motor/motor.c

@@ -196,7 +196,7 @@ static u32 mc_main_task_handler(void *param) {
 			float speed_Ref = _get_speed_from_throttle(_motor_throttle);
 			PMSM_FOC_Set_Speed(speed_Ref);
 			float torque_idq = _get_idq_from_throttle(_motor_throttle);
-			PMSM_FOC_Set_Current(torque_idq);
+			PMSM_FOC_Set_Trque(torque_idq);
 		}
 	}
 	return 0;