update，uart更新，加入单独的电流控制模式，加入MTPA标定模式

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

Applications/app/app.c

@@ -40,7 +40,7 @@ void fetch_jtag_cmd(void) {
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 5) {
-		PMSM_FOC_Set_Trque(1.0f);
+		PMSM_FOC_Set_Current(2.0f);
 		jtag_cmd = 0;
 	}
 }

Applications/bsp/uart.c

@@ -135,9 +135,7 @@ static void shark_uart_dma_tx(shark_uart_t *uart)
 
 	if (value & DMA_CHXCTL_CHEN) {
 		if (SET != dma_flag_get(SHARK_UART0_tx_dma, uart->tx_dma_ch, DMA_FLAG_FTF)) {
-			if (!((uart->tx_length <= 2) && (DMA_CHCNT(SHARK_UART0_tx_dma, uart->tx_dma_ch) == 0))) {
-				return; //workaround: when tx len=1, there will no any FTF or HTF, so need check dma count
-			}
+			return;
 		}
 		dma_flag_clear(SHARK_UART0_tx_dma, uart->tx_dma_ch, DMA_FLAG_FTF);
 		byte_queue_skip(&uart->tx_queue, uart->tx_length);

Applications/foc/commands.c

@@ -49,7 +49,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			bool success;
 			foc_start_cmd_t *scmd = (foc_start_cmd_t *)command->data;
 			if (scmd->start_stop == Foc_Start) {
-				success = mc_start(TRQ_MODE);
+				success = mc_start(CTRL_MODE_CURRENT);
 			}else if (scmd->start_stop == Foc_Stop) {
 				success = mc_stop();
 			}

Applications/foc/core/PI_Controller.h

@@ -34,13 +34,9 @@ typedef struct {
 	float  DT;
 }PI_Controller;
 
-static __INLINE void PI_Controller_Init(PI_Controller *pi, float kp, float ki, float max, float min, float DT) {
-	pi->kp = kp;
-	pi->ki = ki;
+static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min) {
 	pi->max = max;
 	pi->min = min;
-	pi->Ui = 0;
-	pi->DT = DT;
 }
 static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 	pi->Ui = (init);

Applications/foc/core/PMSM_FOC_Core.c

@@ -101,6 +101,7 @@ static void PMSM_FOC_Reset_PID(void) {
 	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);
 }
 
 
@@ -109,13 +110,15 @@ void PMSM_FOC_CoreInit(void) {
 	_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;
+	
 	memset(&_gFOC_Ctrl.in, 0, sizeof(_gFOC_Ctrl.in));
 	memset(&_gFOC_Ctrl.out, 0, sizeof(_gFOC_Ctrl.out));
 	_gFOC_Ctrl.params.s_maxiDC = (MAX_iDQ);
 	_gFOC_Ctrl.params.s_maxiDC = (MAX_iDC);
 	_gFOC_Ctrl.params.s_maxRPM = (MAX_SPEED);
 	_gFOC_Ctrl.params.n_modulation = SVM_Modulation;
-	_gFOC_Ctrl.params.n_PhaseFilterCeof = (0.2f);
+	_gFOC_Ctrl.params.n_PhaseFilterCeof = (0.1f);
 	_gFOC_Ctrl.params.maxvDQ.d = MAX_vDC;
 	_gFOC_Ctrl.params.minvDQ.d = -MAX_vDC;
 	_gFOC_Ctrl.params.maxvDQ.q = MAX_vDC;
@@ -123,7 +126,7 @@ void PMSM_FOC_CoreInit(void) {
 	_gFOC_Ctrl.params.s_maxIdq = MAX_iDQ;
 	_gFOC_Ctrl.params.s_minIdq = -MAX_iDQ;
 	_gFOC_Ctrl.params.n_poles = MOTOR_POLES;
-	_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
+	_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
 	_gFOC_Ctrl.out.f_vdqRation = 0;
 	_gFOC_Ctrl.in.s_manualAngle = 0x3D00;
 	_gFOC_Ctrl.in.s_vDC = (MAX_vDC);
@@ -136,9 +139,9 @@ void PMSM_FOC_CoreInit(void) {
 	PMSM_FOC_Reset_PID();
 }
 
-
+//#define PHASE_LFP
 static __INLINE void PMSM_FOC_Update_Hardware(void) {
-	if (_gFOC_Ctrl.in.s_manualAngle != 0x3D00) {
+	if ((_gFOC_Ctrl.in.s_manualAngle != 0x3D00) && !_gFOC_Ctrl.in.b_MTPA_calibrate) {
 		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_manualAngle;
 		_gFOC_Ctrl.in.s_hallAngle = hall_sensor_get_theta();
 	}else {
@@ -158,14 +161,7 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 #endif	
 }
 
-static __INLINE void PMSM_FOC_Update_PI_Ctrls(void) {
-	/* update speed pi ctrl */
-	if (_gFOC_Ctrl.params.s_maxIdq != _gFOC_Ctrl.pi_ctl_spd->max) {
-		_gFOC_Ctrl.pi_ctl_spd->max = _gFOC_Ctrl.params.s_maxIdq;
-	}
-	if (_gFOC_Ctrl.params.s_minIdq != _gFOC_Ctrl.pi_ctl_spd->min) {
-		_gFOC_Ctrl.pi_ctl_spd->min = _gFOC_Ctrl.params.s_minIdq;
-	}
+static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 	/* update id pi ctrl */
 	if (_gFOC_Ctrl.params.maxvDQ.d != _gFOC_Ctrl.pi_ctl_id->max) {
 		_gFOC_Ctrl.pi_ctl_id->max = _gFOC_Ctrl.params.maxvDQ.d;
@@ -193,9 +189,9 @@ void PMSM_FOC_Schedule(void) {
 
 	PMSM_FOC_Update_Hardware();
 
-	if (_gFOC_Ctrl.out.n_RunMode != OPEN_MODE) {
+	if (_gFOC_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
 
-		PMSM_FOC_Update_PI_Ctrls();
+		PMSM_FOC_Update_PI_Idq();
 	
 		Clark(iabc[0], iabc[1], iabc[2], &vAB);
 
@@ -225,11 +221,12 @@ void PMSM_FOC_Schedule(void) {
 	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_1data16(FtoS16x1000(PMSM_FOC_Get_iDC()));
 		//plot_2data16(FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
-		plot_1data16(_gFOC_Ctrl.in.s_motRPM);
+		//plot_1data16(_gFOC_Ctrl.in.s_motRPM);
 		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(_gFOC_Ctrl.in.s_motAngle));
 		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(_gFOC_Ctrl.in.s_targetVdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d));
-		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), FtoS16(_gFOC_Ctrl.in.s_hallAngle)*10);
+		plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), FtoS16(_gFOC_Ctrl.in.s_hallAngle)*10);
 		//plot_1data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle));
 	}	
 }
@@ -246,32 +243,65 @@ void PMSM_FOC_LogDebug(void) {
 /*called in media task */
 u8 PMSM_FOC_CtrlMode(void) {
 	if (!_gFOC_Ctrl.in.b_motEnable) {
-		_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
-	}else if (!_gFOC_Ctrl.in.b_motEnable || _gFOC_Ctrl.in.n_ctlMode == OPEN_MODE) {
-		_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
-	}else if (_gFOC_Ctrl.in.n_ctlMode == SPD_MODE || _gFOC_Ctrl.in.b_cruiseEna){
-		_gFOC_Ctrl.out.n_RunMode = SPD_MODE;
+		_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 {
-		_gFOC_Ctrl.out.n_RunMode = TRQ_MODE;
+		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
 	}
 	return _gFOC_Ctrl.out.n_RunMode;
 }
 
 /* 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;
+	if (_gFOC_Ctrl.in.b_MTPA_calibrate) {
+		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;
+	}else {
+		_gFOC_Ctrl.in.s_targetIdq.d = 0;
+		_gFOC_Ctrl.in.s_targetIdq.q = _gFOC_Ctrl.in.s_targetCurrent;
+	}
 	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);
 }
 
+static __INLINE void PMSM_FOC_Update_PI_Spd(void) {
+	/* update speed pi ctrl */
+	if (_gFOC_Ctrl.params.s_maxIdq != _gFOC_Ctrl.pi_ctl_spd->max) {
+		_gFOC_Ctrl.pi_ctl_spd->max = _gFOC_Ctrl.params.s_maxIdq;
+	}
+	if (_gFOC_Ctrl.params.s_minIdq != _gFOC_Ctrl.pi_ctl_spd->min) {
+		_gFOC_Ctrl.pi_ctl_spd->min = _gFOC_Ctrl.params.s_minIdq;
+	}
+}
+
+static __INLINE void PMSM_FOC_Update_PI_Trq(void) {
+	/* update speed pi ctrl */
+	float trqIs = _gFOC_Ctrl.in.s_targetTrque;
+	if (trqIs != _gFOC_Ctrl.pi_ctl_trq->max) {
+		_gFOC_Ctrl.pi_ctl_trq->max = trqIs;
+	}
+}
+
+
 /*called in media task */
 void PMSM_FOC_idqCalc(void) {
-	if (_gFOC_Ctrl.out.n_RunMode == TRQ_MODE) {
-		_gFOC_Ctrl.in.s_targetTrq = (eCtrl_get_RefTorque());
+	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) {
+		PMSM_FOC_Update_PI_Trq();
+		float errRef = _gFOC_Ctrl.params.s_maxRPM - _gFOC_Ctrl.in.s_motRPM;
+		_gFOC_Ctrl.in.s_targetCurrent = PI_Controller_run(_gFOC_Ctrl.pi_ctl_trq, errRef);
 	}else {
-		float errRef = min(eCtrl_get_RefSpd(), _gFOC_Ctrl.params.s_maxRPM) - _gFOC_Ctrl.in.s_motRPM;
-		_gFOC_Ctrl.in.s_targetTrq = PI_Controller_run(_gFOC_Ctrl.pi_ctl_spd, errRef);
+		float errRef = min(eCtrl_get_RefSpeed(), _gFOC_Ctrl.params.s_maxRPM) - _gFOC_Ctrl.in.s_motRPM;
+		PMSM_FOC_Update_PI_Spd();
+		_gFOC_Ctrl.in.s_targetCurrent = PI_Controller_run(_gFOC_Ctrl.pi_ctl_spd, errRef);
 		
 	}
 	PMSM_FOC_idq_Assign();
@@ -325,12 +355,12 @@ void PMSM_FOC_SetOpenVdq(float vd, float vq) {
 
 bool PMSM_FOC_EnableCruise(bool enable) {
 	if (enable != _gFOC_Ctrl.in.b_cruiseEna) {
-		s32q4_t motSpd = PMSM_FOC_GetSpeed();
+		float motSpd = PMSM_FOC_GetSpeed();
 		if (motSpd < MIN_CRUISE_RPM) { //
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		eCtrl_set_TargetSpeed(motSpd);
+		eCtrl_set_TgtSpeed(motSpd);
 		_gFOC_Ctrl.in.b_cruiseEna = enable;
 	}
 	
@@ -338,36 +368,50 @@ bool PMSM_FOC_EnableCruise(bool enable) {
 }
 
 bool PMSM_FOC_Is_CruiseEnabled(void) {
-	return (_gFOC_Ctrl.in.b_cruiseEna && (_gFOC_Ctrl.out.n_RunMode == SPD_MODE));
+	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) {
 		return false;
 	}
-	eCtrl_set_TargetSpeed(rpm);
+	eCtrl_set_TgtSpeed(rpm);
+	return true;
+}
+
+bool PMSM_FOC_Set_Current(float is) {
+	eCtrl_set_TgtCurrent(is);
 	return true;
 }
 
-bool PMSM_FOC_Set_Trque(float Trq) {
-	eCtrl_set_TrqCurrent(Trq);
+bool PMSM_FOC_Set_Trque(float trq) {
+	_gFOC_Ctrl.in.s_targetTrque = trq;
 	return true;
 }
 
 bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 	if (PMSM_FOC_Is_CruiseEnabled()) {
-		eCtrl_set_TargetSpeed(rpm);
+		eCtrl_set_TgtSpeed(rpm);
 		return true;
 	}
 	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
 	return false;
 }
 
+void PMSM_FOC_MTPA_Calibrate(bool enable) {
+	_gFOC_Ctrl.in.b_MTPA_calibrate = enable;
+}
+
 void PMSM_FOC_Set_Angle(float angle) {
 	_gFOC_Ctrl.in.s_manualAngle = (angle);
 }
 
-s32q4_t PMSM_FOC_GetSpeed(void) {
+void PMSM_FOC_Get_TgtIDQ(DQ_t * dq) {
+	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;
 }
 
@@ -377,24 +421,15 @@ void PMSM_FOC_LockMotor(bool lock) {
 }
 
 void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min) {
-	_gFOC_Ctrl.pi_ctl_spd->kp = S32Q14(kp);
-	_gFOC_Ctrl.pi_ctl_spd->ki = S32Q14(ki);
-	_gFOC_Ctrl.pi_ctl_spd->max = S32Q14(max);
-	_gFOC_Ctrl.pi_ctl_spd->min = S32Q14(min);
+
 }
 
 void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min) {
-	_gFOC_Ctrl.pi_ctl_id->kp = S32Q14(kp);
-	_gFOC_Ctrl.pi_ctl_id->ki = S32Q14(ki);
-	_gFOC_Ctrl.pi_ctl_id->max = S32Q14(max);
-	_gFOC_Ctrl.pi_ctl_id->min = S32Q14(min);
+
 }
 
 void PMSM_FOC_SetIQPid(float kp, float ki, float max, float min) {
-	_gFOC_Ctrl.pi_ctl_iq->kp = S32Q14(kp);
-	_gFOC_Ctrl.pi_ctl_iq->ki = S32Q14(ki);
-	_gFOC_Ctrl.pi_ctl_iq->max = S32Q14(max);
-	_gFOC_Ctrl.pi_ctl_iq->min = S32Q14(min);
+
 }
 
 void PMSM_FOC_SetTrqPid(float kp, float ki, float max, float min) {
@@ -402,10 +437,7 @@ void PMSM_FOC_SetTrqPid(float kp, float ki, float max, float min) {
 }
 
 void PMSM_FOC_SetFW_I(float kp, float ki, float max, float min) {
-	_gFOC_Ctrl.pi_ctl_fw->kp = S32Q14(kp);
-	_gFOC_Ctrl.pi_ctl_fw->ki = S32Q14(ki);
-	_gFOC_Ctrl.pi_ctl_fw->max = S32Q14(max);
-	_gFOC_Ctrl.pi_ctl_fw->min = S32Q14(min);
+
 }
 
 void PMSM_FOC_SetErrCode(u8 error) {
@@ -419,18 +451,18 @@ u8 PMSM_FOC_GetErrCode(void) {
 }
 
 //获取母线电流
-s16q5_t PMSM_FOC_Get_iDC(void) {
-	s32 vd = _gFOC_Ctrl.out.s_OutVdq.d;
-	s32 vq = _gFOC_Ctrl.out.s_OutVdq.q;
-	s32 id = _gFOC_Ctrl.out.s_RealIdq.d;
-	s32 iq = _gFOC_Ctrl.out.s_RealIdq.q;
+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;
     /*
 		根据公式（等幅值变换，功率不等）:
 		iDC x vDC = 2/3(iq x vq + id x vd);
 	*/
-	s32 m_pow = (vd * id + vq * iq); //s32q10
-	s16 iDC = m_pow / _gFOC_Ctrl.in.s_vDC; //s16q5
-	return S16Q5toF(iDC) * 0.667f;
+	float m_pow = (vd * id + vq * iq); //s32q10
+	float iDC = m_pow / _gFOC_Ctrl.in.s_vDC; //s16q5
+	return (iDC) * 0.667f;
 }
 
 void PMSM_FOC_Brake(bool brake) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -30,18 +30,20 @@ typedef struct {
 	float 	s_iABCFilter[3];
 	float   s_motRPM;   //from hall or encoder
 	float 	s_motAngle; //from hall or encoder
-	float 	s_hallAngle;
+	float 	s_hallAngle;//from hall or encoder
 	float   s_targetRPM;
-	float 	s_targetTrq;
+	float 	s_targetCurrent;
 	DQ_t    s_targetIdq;
 	DQ_t    s_targetVdq;
+	float   s_targetTrque;
 	float 	s_vDC;
 	u8      n_ctlMode;
 	bool    b_motEnable;
 	bool    b_cruiseEna;
 	bool    b_motLock;
 	bool    b_eBrake;
-	float   s_manualAngle; //mainly used when calibrate hall
+	bool    b_MTPA_calibrate;
+	float   s_manualAngle; //mainly used when calibrate hall/mtpa. 
 }FOC_InP;
 
 typedef struct {
@@ -72,6 +74,7 @@ typedef struct {
 	PI_Controller *pi_ctl_iq;
 	PI_Controller *pi_ctl_spd;
 	PI_Controller *pi_ctl_fw;
+	PI_Controller *pi_ctl_trq;
 	dq_Rctrl      idq_ctl[2];
 	dq_Rctrl      vdq_ctl[2];	
 	FOC_InP       in;
@@ -95,9 +98,10 @@ typedef enum {
 }foc_fault_t;
 
 
-#define OPEN_MODE                      ((u8)0U)
-#define SPD_MODE                       ((u8)1U)
-#define TRQ_MODE                       ((u8)2U)
+#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 FOC_CALIMOD_HALL               ((u8) 1U)
 #define FOC_CALIMOD_MTPA               ((u8) 2U)
@@ -134,12 +138,13 @@ void PMSM_FOC_SetCtrlMode(u8 mode);
 void PMSM_FOC_SetOpenVdq(float vd, float vq);
 bool PMSM_FOC_EnableCruise(bool enable);
 bool PMSM_FOC_Set_Speed(float rpm);
-bool PMSM_FOC_Set_Trque(float current);
+bool PMSM_FOC_Set_Trque(float trque);
+bool PMSM_FOC_Set_Current(float current);
 bool PMSM_FOC_Set_CruiseSpeed(float rpm);
-s32q4_t PMSM_FOC_GetSpeed(void);
+float PMSM_FOC_GetSpeed(void);
 bool PMSM_FOC_Lock_Motor(bool lock);
 void PMSM_FOC_Brake(bool brake);
-s16q5_t PMSM_FOC_Get_iDC(void);
+float PMSM_FOC_Get_iDC(void);
 void PMSM_FOC_LockMotor(bool lock);
 void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min);
 void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min);
@@ -150,5 +155,7 @@ void PMSM_FOC_Set_Angle(float angle);
 bool PMSM_FOC_Is_Start(void);
 void PMSM_FOC_SetErrCode(u8 error);
 u8 PMSM_FOC_GetErrCode(void);
+void PMSM_FOC_MTPA_Calibrate(bool enable);
+void PMSM_FOC_Get_TgtIDQ(DQ_t * dq);
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/core/PMSM_FOC_Params.h

@@ -38,3 +38,11 @@ static PI_Controller PI_Ctrl_fw = {
 	.Ui = 0,
 };
 
+static PI_Controller PI_Ctrl_trq = {
+	.kp = (0.001f),
+	.ki = (0.003f),
+	.max = (MAX_iDQ),
+	.min = (0),
+	.DT  = (1.0f/(float)SPD_CTRL_TS),
+	.Ui = 0,
+};

Applications/foc/core/e_ctrl.c

@@ -4,8 +4,8 @@
 #include "libs/logger.h"
 
 static e_Ctrl g_eCtrl;
-static void _eCtrl_set_TrqCurrent(float c);
-static void _eCtrl_set_TargetSpeed(float s);
+static void _eCtrl_set_TgtCurrent(float c);
+static void _eCtrl_set_TgtSpeed(float s);
 
 void eCtrl_init(u16 ebrk_time, u16 accl_time){
 	g_eCtrl.ebrk_time = ebrk_time;
@@ -29,10 +29,10 @@ void eCtrl_set_accl_brk(u16 accl_time, u16 ebrk_time) {
 }
 
 
-void eCtrl_set_TrqCurrent(float c) {
+void eCtrl_set_TgtCurrent(float c) {
 	g_eCtrl.torque_shadow = c;
 }
-void eCtrl_set_TargetSpeed(float s) {
+void eCtrl_set_TgtSpeed(float s) {
 	g_eCtrl.speed_shadow = s;
 }
 
@@ -44,10 +44,10 @@ void eCtrl_Running(void) {
 		etime_changed = true;
 	}
 	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target || etime_changed) {
-		_eCtrl_set_TrqCurrent(g_eCtrl.torque_shadow);
+		_eCtrl_set_TgtCurrent(g_eCtrl.torque_shadow);
 	}
 	if (g_eCtrl.speed_shadow != g_eCtrl.speed.target || etime_changed) {
-		_eCtrl_set_TargetSpeed(g_eCtrl.speed_shadow);
+		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
 	}
 	eRamp_running(&g_eCtrl.torque);
 	eRamp_running(&g_eCtrl.speed);
@@ -77,30 +77,30 @@ static void _eCtrl_set_target(e_Ramp *ramp, float c) {
 	eRamp_set_step(ramp, step_val);
 
 }
-static void _eCtrl_set_TrqCurrent(float c) {
+static void _eCtrl_set_TgtCurrent(float c) {
 	_eCtrl_set_target(&g_eCtrl.torque, c);
 }
 
-static void _eCtrl_set_TargetSpeed(float s) {
+static void _eCtrl_set_TgtSpeed(float s) {
 	_eCtrl_set_target(&g_eCtrl.speed, s);
 }
 
 
 
-float eCtrl_get_RefSpd(void) {
+float eCtrl_get_RefSpeed(void) {
 	return eRamp_get_intepolation(&g_eCtrl.speed);
 }
 
-float eCtrl_get_RefTorque(void) {
+float eCtrl_get_RefCurrent(void) {
 	return eRamp_get_intepolation(&g_eCtrl.torque);
 }
 
 
-float eCtrl_get_FinalSpd(void) {
+float eCtrl_get_FinalSpeed(void) {
 	return eRamp_get_target(&g_eCtrl.speed);
 }
 
-float eCtrl_get_FinalTorque(void) {
+float eCtrl_get_FinalCurrent(void) {
 	return eRamp_get_target(&g_eCtrl.torque);
 }
 
@@ -120,8 +120,8 @@ void eCtrl_brake_signal(bool hw_brake) {
 				ebrk_torque = eCTRL_NEG_TORQUE;
 			}
 		}
-		eCtrl_set_TrqCurrent(ebrk_torque);
-		eCtrl_set_TargetSpeed(ebrk_speed);
+		eCtrl_set_TgtCurrent(ebrk_torque);
+		eCtrl_set_TgtSpeed(ebrk_speed);
 	}
 }
 

Applications/foc/core/e_ctrl.h

@@ -60,12 +60,12 @@ static float eRamp_get_target(e_Ramp *r) {
 void eCtrl_init(u16 ebrk_time, u16 accl_time);
 void eCtrl_set_accl_brk(u16 accl_time, u16 ebrk_time);
 void eCtrl_brake_signal(bool hw_brake);
-void eCtrl_set_TrqCurrent(float c);
-void eCtrl_set_TargetSpeed(float s);
-float eCtrl_get_RefSpd(void);
-float eCtrl_get_RefTorque(void);
-float eCtrl_get_FinalSpd(void);
-float eCtrl_get_FinalTorque(void);
+void eCtrl_set_TgtCurrent(float c);
+void eCtrl_set_TgtSpeed(float s);
+float eCtrl_get_RefSpeed(void);
+float eCtrl_get_RefCurrent(void);
+float eCtrl_get_FinalSpeed(void);
+float eCtrl_get_FinalCurrent(void);
 void eCtrl_Running(void);
 
 #endif /* EBRAKE_CTRL_H__ */

Applications/foc/foc_config.h

@@ -12,7 +12,7 @@
 #define MAX_iDQ            150  /* 最大DQ轴电流， A*/
 #define MAX_SPEED          8200 /* 最大转速 RPM*/
 #define MAX_iDC            45   /* 最大母线电流 A*/
-#define MIN_CRUISE_RPM 1000     /* 能启动定速巡航的最小速度 */
+#define MIN_CRUISE_RPM 	   1000     /* 能启动定速巡航的最小速度 */
 #ifdef GD32_FOC_DEMO
 #define MAX_vDC (24)   /* 母线最大电压 V*/
 #else

Applications/foc/motor/hall.c

@@ -451,7 +451,7 @@ void HALL_IRQHandler(void) {
 		//plot_2data16(estimate_delta_angle>>19, (estimate_delta_angle/((s32)(delta_us/FOC_CTRL_US)))>>10);//, _sensor_hander.estimate_delta_angle>>19);
 		/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
 		delta_us = _hall_get_angle_ticks();
-		//_sensor_hander.comp_count = 20;
+		//_sensor_hander.comp_count = 50;
 		//_sensor_hander.angle_comp_ts = estimate_delta_angle/(float)_sensor_hander.comp_count;
 		_sensor_hander.estimate_el_angle = theta_now;
 	}else {

Applications/foc/motor/hall.h

@@ -34,7 +34,7 @@
 #define STATE_7 (uint8_t)7
 
 #define THETA_NONE        (float)0xFFFF
-#define SAMPLE_MAX_COUNT 4
+#define SAMPLE_MAX_COUNT 16
 
 typedef struct {
 	u32       	ticks[SAMPLE_MAX_COUNT];

Applications/foc/motor/motor.c

@@ -36,7 +36,7 @@ bool mc_start(u8 mode) {
 	if (motor.b_start) {
 		return true;
 	}
-	if (mode > TRQ_MODE) {
+	if (mode > CTRL_MODE_CURRENT) {
 		PMSM_FOC_SetErrCode(FOC_Param_Err);
 		return false;
 	}
@@ -48,7 +48,7 @@ bool mc_start(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
 		return false;
 	}
-	eCtrl_init(1000, 3000);
+	eCtrl_init(1000, 1000);
 	hall_sensor_clear();
 	PMSM_FOC_Start(mode);
 	pwm_turn_on_low_side();
@@ -99,7 +99,7 @@ void mc_hall_calibrate(s16 vd) {
 	}
 	pwm_turn_on_low_side();
 	task_udelay(500);
-	PMSM_FOC_Start(OPEN_MODE);
+	PMSM_FOC_Start(CTRL_MODE_OPEN);
 	phase_current_start_cali();
 	pwm_start();
 	adc_start_convert();
@@ -239,14 +239,14 @@ void Sched_MC_mTask(void) {
 #if ANGLE_TEST
 	_debug_angle();
 #endif
-	if (runMode != OPEN_MODE) {
+	if (runMode != CTRL_MODE_OPEN) {
 		eCtrl_Running();
 		if (get_throttle_float() != motor.throttle) {
 			motor.throttle = get_throttle_float();
-			if (runMode == SPD_MODE) {
+			if (runMode == CTRL_MODE_SPD) {
 				float speed_Ref = _get_speed_from_throttle();
 				PMSM_FOC_Set_Speed(speed_Ref);
-			}else if (runMode == TRQ_MODE) {
+			}else if (runMode == CTRL_MODE_TRQ) {
 				float torque_idq = _get_idq_from_throttle();
 				PMSM_FOC_Set_Trque(torque_idq);
 			}

Applications/math/fast_math.h

@@ -30,8 +30,8 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 	*sin = arm_sin_f32(angle);
 	*cos = arm_cos_f32(angle);
 }
-#define degree_2_pi(d) ((float)d * M_PI / 180.0f)
-#define pi_2_degree(d) ((float)d * 180.0f / M_PI)
+#define degree_2_pi(d) ((float)(d) * M_PI / 180.0f)
+#define pi_2_degree(d) ((float)(d) * 180.0f / M_PI)
 
 
 /**

Project/GD32_DEMO.uvoptx

@@ -120,7 +120,6 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name>*</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>