PI controller's I use DT

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

Applications/foc/core/PI_Controller.h

@@ -1,23 +1,27 @@
 #ifndef _PI_Contrller_H__
 #define _PI_Contrller_H__
 #include "math/fix_math.h"
+#include "bsp/bsp.h"
 typedef struct {
-	s32q14_t  kp;
-	s32q14_t  ki;
-	s32q14_t  max;
-	s32q14_t  min;
-	s32q14_t  Ui;
+	s16q5_t  kp;
+	s16q5_t  ki;
+	s16q5_t  max;
+	s16q5_t  min;
+	s16q5_t  Ui;
+	s16q14_t DT;
 }PI_Controller;
 
-static __INLINE void PI_Controller_Reset(PI_Controller *pi, s32q14_t init) {
+static __INLINE void PI_Controller_Reset(PI_Controller *pi, s16q5_t init) {
 	pi->Ui = init;
 }
 
-static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s32q14_t err) {
-	s32q14_t kp_err = S32Q14_MUL(err,pi->kp);
-	pi->Ui = MATH_sat(pi->Ui + S32Q14_MUL(kp_err,pi->ki), pi->min, pi->max);
+static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s16q5_t err) {
+	s16q5_t kp_err = S16_mul(err,pi->kp, 5);
+	s16q5_t ki_err = S16_mul(err,pi->ki, 5);
+	s16q5_t integral = S16_mul(ki_err, pi->DT, 14);
+	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
 	s32q14_t out = pi->Ui + kp_err;
-	return MATH_sat(out, pi->min, pi->max) >> 9;
+	return MATH_sat(out, pi->min, pi->max);
 }
 
 #endif	/*_PI_Contrller_H__*/

Applications/foc/core/PMSM_FOC_Core.c

@@ -120,7 +120,8 @@ void PMSM_FOC_CoreInit(void) {
 	_gFOC_Ctrl.out.n_RunMode = OPEN_MODE;
 	_gFOC_Ctrl.out.f_vdqRation = S16Q14(0.9f);
 	_gFOC_Ctrl.in.s_manualAngle = 0x3D00;
-
+	_gFOC_Ctrl.in.n_PhaseFilterCeof = S16Q10(0.2f);
+	
 	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[0], 1);
 	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[1], 1);
 
@@ -129,35 +130,33 @@ void PMSM_FOC_CoreInit(void) {
 	PMSM_FOC_Reset_PID();
 }
 
-s16 test_motangle = 0;
+
 static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	if (_gFOC_Ctrl.in.s_manualAngle != 0x3D00) {
 		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_manualAngle;
-		test_motangle = hall_sensor_get_theta();
+		_gFOC_Ctrl.in.s_hallAngle = hall_sensor_get_theta();
 	}else {
-		_gFOC_Ctrl.in.s_motAngle = hall_sensor_get_theta();
+		_gFOC_Ctrl.in.s_hallAngle = hall_sensor_get_theta();
+		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_hallAngle;
 	}
 
 	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed();
 	//sample current
 	phase_current_get(_gFOC_Ctrl.in.s_iABC);
 
+	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[0], _gFOC_Ctrl.in.s_iABC[0], 0.2f);
+	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[1], _gFOC_Ctrl.in.s_iABC[1], 0.2f);
+	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[2], _gFOC_Ctrl.in.s_iABC[2], 0.2f);
+	
 	_gFOC_Ctrl.in.s_vDC = get_vbus_sfix5();
 }
 
-/* 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_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 int _g_ctl_count = 0;
 extern int get_hall_stat(int samples);
 void PMSM_FOC_Schedule(void) {
 	AB_t vAB;
-	s16q5_t *iabc = _gFOC_Ctrl.in.s_iABC;
+	s16q5_t *iabc = _gFOC_Ctrl.in.s_iABCFilter;
 
 	PMSM_FOC_Update_Hardware();
 
@@ -187,9 +186,10 @@ void PMSM_FOC_Schedule(void) {
 	pwm_update_sample(_gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2, _gFOC_Ctrl.out.n_CPhases);
 
 	if (_g_ctl_count++ % 5 == 0) {
+		plot_1data16(_gFOC_Ctrl.in.s_motRPM>>5);
 		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
-		//plot_3data16(test_motangle>>5, _gFOC_Ctrl.in.s_motAngle>>5, get_hall_stat(9)*60);
-		plot_3data16(_gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.in.s_iABC[2]);
+		//plot_3data16(_gFOC_Ctrl.in.s_hallAngle>>5, _gFOC_Ctrl.in.s_motAngle>>5, get_hall_stat(9)*60);
+		//plot_3data16(iabc[0], iabc[1], iabc[2]);
 		//plot_3data16(_gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2, _gFOC_Ctrl.out.n_CPhases * 10 + 3000);
 	}	
 }
@@ -217,14 +217,16 @@ u8 PMSM_FOC_CtrlMode(void) {
 	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;
+	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_motEnable) {
-		return;
-	}
-	if (_gFOC_Ctrl.out.n_RunMode == OPEN_MODE) {
-		return;
-	}
 	if (_gFOC_Ctrl.out.n_RunMode == TRQ_MODE) {
 		_gFOC_Ctrl.in.s_targetTrq = eCtrl_get_RefTorque();
 	}else {

Applications/foc/core/PMSM_FOC_Core.h

@@ -16,16 +16,19 @@ typedef struct {
 	u8 		n_poles;
 	s16q5_t s_maxIdq;
 	s16q5_t s_maxiDC;
-	s32q14_t  s_maxRPM;
+	s32q5_t  s_maxRPM;
 	s16q5_t s_iABC[3];
-	s16     s_motRPM;   //from hall or encoder
+	s16q5_t s_iABCFilter[3];
+	s32q5_t     s_motRPM;   //from hall or encoder
 	s16q5_t s_motAngle; //from hall or encoder
+	s16q5_t s_hallAngle;
 	s16     s_targetRPM;
 	s16q5_t s_targetTrq;
 	DQ_t    s_targetIdq;
 	DQ_t    s_targetVdq;
 	s16q5_t s_vDC;
 	s16q14_t n_modulation;
+	s16q14_t n_PhaseFilterCeof;
 	u8      n_ctlMode;
 	bool    b_motEnable;
 	bool    b_cruiseEna;

Applications/foc/core/PMSM_FOC_Params.h

@@ -1,35 +1,41 @@
 #include "foc/foc_config.h"
 #include "foc/core/PI_Controller.h"
-
+#define CURR_BANDWITH 1000
+#define STAR_R   0.5F
+#define STAR_L   0.0002F
 static PI_Controller PI_Ctrl_ID = {
-	.kp = S32Q14(0.001f),
-	.ki = S32Q14(0.003f),
-	.max = S32Q14(MAX_iDQ),
-	.min = S32Q14(-MAX_iDQ),
+	.kp = S16Q5(CURR_BANDWITH * STAR_R),
+	.ki = S16Q5(CURR_BANDWITH * STAR_L),
+	.max = S16Q5(MAX_vDC),
+	.min = S16Q5(-MAX_vDC),
+	.DT  = S16Q14(1.0f/(float)IDQ_CTRL_TS),
 	.Ui = 0,
 };
 
 static PI_Controller PI_Ctrl_IQ = {
-	.kp = S32Q14(0.001f),
-	.ki = S32Q14(0.003f),
-	.max = S32Q14(MAX_iDQ),
-	.min = S32Q14(-MAX_iDQ),
+	.kp = S16Q5(CURR_BANDWITH * STAR_R),
+	.ki = S16Q5(CURR_BANDWITH * STAR_L),
+	.max = S16Q5(MAX_vDC),
+	.min = S16Q5(-MAX_vDC),
+	.DT  = S16Q14(1.0f/(float)IDQ_CTRL_TS),
 	.Ui = 0,
 };
 
 static PI_Controller PI_Ctrl_Spd = {
-	.kp = S32Q14(0.001f),
-	.ki = S32Q14(0.003f),
-	.max = S32Q14(MAX_SPEED),
-	.min = S32Q14(0),
+	.kp = S16Q5(0.001f),
+	.ki = S16Q5(0.003f),
+	.max = S16Q5(MAX_iDQ),
+	.min = S16Q5(-MAX_iDQ),
+	.DT  = S16Q14(1.0f/(float)SPD_CTRL_TS),
 	.Ui = 0,
 };
 
 static PI_Controller PI_Ctrl_fw = {
-	.kp = S32Q14(0.001f),
-	.ki = S32Q14(0.003f),
-	.max = S32Q14(20),
-	.min = S32Q14(0),
+	.kp = S16Q5(0.001f),
+	.ki = S16Q5(0.003f),
+	.max = S16Q5(20),
+	.min = S16Q5(0),
+	.DT  = S16Q14(1.0f/(float)SPD_CTRL_TS),
 	.Ui = 0,
 };
 

Applications/foc/motor/current.c

@@ -93,7 +93,7 @@ bool phase_current_offset(void) {
 	return true;
 }
 
-#define LowPass_filter S16Q14(1)
+
 void phase_current_get(s16 *iABC){
 	current_samp_t *cs = &g_cs;
 	s32 phase_current1, phase_current2;
@@ -119,9 +119,9 @@ void phase_current_get(s16 *iABC){
 		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
 		cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
 	}
-	iABC[0] = S16_mul(cs->adc_ia, ADC_TO_CURR_ceof, 9);
-	iABC[1] = S16_mul(cs->adc_ib, ADC_TO_CURR_ceof, 9);
-	iABC[2] = S16_mul(cs->adc_ic, ADC_TO_CURR_ceof, 9);
+	iABC[0] = cs->adc_ia;//S16_mul(cs->adc_ia, ADC_TO_CURR_ceof, 9);
+	iABC[1] = cs->adc_ib;//S16_mul(cs->adc_ib, ADC_TO_CURR_ceof, 9);
+	iABC[2] = cs->adc_ic;//S16_mul(cs->adc_ic, ADC_TO_CURR_ceof, 9);
 }
 
 #if 0

Applications/foc/motor/hall.c

@@ -442,8 +442,8 @@ void HALL_IRQHandler(void) {
 	_sensor_hander.hall_stat = hall_stat_now;
 	_sensor_hander.hall_ticks = hall_ticks_now;
 	_sensor_hander.el_speed = _hall_angle_speed();	//s32q5
-	_sensor_hander.rpm = _sensor_hander.el_speed / 360 * 60; //s32q5
-	//plot_3data16(_sensor_hander.rpm >> 5, (_sensor_hander.immediately_el_speed/6) >> 5, (_sensor_hander.estimate_el_speed/6)>>5);
+	_sensor_hander.rpm = (_sensor_hander.el_speed / 360 * 60); //s32q5
+	//plot_3data16(_sensor_hander.rpm >> 5, _sensor_hander.el_speed>>5, delta_us);
 	time_measure_end(&g_meas_hall);
 }
 

Applications/foc/motor/hall.h

@@ -32,7 +32,7 @@ typedef struct {
 	u32        	ticks[SAMPLE_MAX_COUNT];
 	s32q19_t   	angles[SAMPLE_MAX_COUNT];
 	u32   		ticks_sum;
-	s32q19_t   	angles_sum;
+	s32   		angles_sum;
 	u32   		index;
 	bool  		full;
 }hall_sample_t;

Applications/math/fast_math.h

@@ -46,7 +46,8 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
  * @param filter_constant
  * Filter constant. Range 0.0 to 1.0, where 1.0 gives the unfiltered value.
  */
-#define LowPass_Filter(value, sample, filter_constant)	(value = value * (1.0f - filter_constant) + sample * filter_constant)
+//#define LowPass_Filter(value, sample, filter_constant)	(value = value * (1.0f - filter_constant) + sample * filter_constant)
+#define LowPass_Filter(value, sample, filter_constant)	(value = (sample - value) * filter_constant + value)
 
 #endif /* _Fast_Math_H__ */
 

Project/GD32_DEMO.uvoptx

@@ -313,8 +313,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\ramp_ctrl.c</PathWithFileName>
-      <FilenameWithoutPath>ramp_ctrl.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\PMSM_FOC_Core.c</PathWithFileName>
+      <FilenameWithoutPath>PMSM_FOC_Core.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -325,8 +325,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\commands.c</PathWithFileName>
-      <FilenameWithoutPath>commands.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\ramp_ctrl.c</PathWithFileName>
+      <FilenameWithoutPath>ramp_ctrl.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -337,8 +337,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\samples.c</PathWithFileName>
-      <FilenameWithoutPath>samples.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\commands.c</PathWithFileName>
+      <FilenameWithoutPath>commands.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -349,8 +349,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\samples.c</PathWithFileName>
+      <FilenameWithoutPath>samples.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -361,8 +361,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\svpwm.c</PathWithFileName>
-      <FilenameWithoutPath>svpwm.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\e_ctrl.c</PathWithFileName>
+      <FilenameWithoutPath>e_ctrl.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -373,8 +373,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\core\svpwm.c</PathWithFileName>
+      <FilenameWithoutPath>svpwm.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>

Project/GD32_DEMO.uvprojx

@@ -403,6 +403,11 @@
         <Group>
           <GroupName>Foc</GroupName>
           <Files>
+            <File>
+              <FileName>PMSM_FOC_Core.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\core\PMSM_FOC_Core.c</FilePath>
+            </File>
             <File>
               <FileName>ramp_ctrl.c</FileName>
               <FileType>1</FileType>
@@ -428,11 +433,6 @@
               <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>
           </Files>
         </Group>
         <Group>