SVM fix OK, fix TWO_BY_SQRT3_Q14 error

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

Applications/app/app.c

@@ -29,6 +29,12 @@ void fetch_jtag_cmd(void) {
 		cmd_data[0] = jtag_cmd;
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
+	}else if (jtag_cmd == 3 || jtag_cmd == 4) {
+		if (jtag_cmd ==3) {
+			PMSM_FOC_SetOpenVdq(0, S16Q5(8));
+		}else {
+			PMSM_FOC_SetOpenVdq(0, 0);
+		}
 	}
 }
 #else

Applications/bsp/adc.h

@@ -32,7 +32,7 @@ inserted ADC 由timer0 ch3触发，
 #define ISO3_OFFSET 15
 #define IL_OFFSET   20
 
-#define ADC_SAMPLE_TIME ADC_SAMPLETIME_13POINT5
+#define ADC_SAMPLE_TIME ADC_SAMPLETIME_7POINT5
 #define ADC_TRIGGER_PHASE ADC0_1_EXTTRIG_INSERTED_T0_CH3
 #define ADC_TRIGGER_PHASE2 ADC0_1_EXTTRIG_INSERTED_T1_CH0
 #define ADC_TRIGGER_NONE  ADC0_1_2_EXTTRIG_INSERTED_NONE

Applications/bsp/bsp.c

@@ -89,29 +89,4 @@ int wdog_set_timeout(int wdog_time)
 	return 0;
 }
 
-//10 ms
-#if 0
-static void normal_task_timer_init(void) {
-    timer_parameter_struct timer_initpara;
-    u32 timer = TIMER5;
-    rcu_periph_clock_enable(RCU_TIMER5);
-
-    timer_deinit(timer);
-	
-	memset(&timer_initpara, 0, sizeof(timer_initpara));
-    timer_initpara.prescaler        = 12000 - 1; //clk 10000
-    timer_initpara.alignedmode      = TIMER_COUNTER_EDGE;
-    timer_initpara.period          = 100;
-    timer_initpara.clockdivision    = TIMER_CKDIV_DIV1;
-    timer_initpara.repetitioncounter = 0;
-    timer_init(timer,&timer_initpara);
-	timer_counter_value_config(timer, 0);
-	timer_autoreload_value_config(timer, 100);
-	timer_counter_up_direction(timer);
-	timer_auto_reload_shadow_enable(timer);
-	timer_interrupt_enable(timer, TIMER_INT_UP);
-	timer_interrupt_flag_clear(timer, TIMER_INT_FLAG_UP);
-	nvic_irq_enable(TIMER5_IRQn, 5, 0);
-	timer_enable(timer);
-}
-#endif
+

Applications/foc/commands.c

@@ -85,9 +85,9 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		}
 		case Foc_Set_Open_Dq_Vol:
 		{
-			s32 vq = decode_s32(((u8 *)command->data) + 4);
+			s16 vq = decode_s16(((u8 *)command->data) + 2);
 			sys_debug("set v_q %d\n", vq);
-			PMSM_FOC_SetOpenVdq(0, vq);
+			PMSM_FOC_SetOpenVdq(S16Q5(0), S16Q5(vq));
 			break;
 		}
 		case Foc_Conf_Pid:

Applications/foc/core/PMSM_FOC_Core.c

@@ -18,7 +18,7 @@ static __INLINE void RevPark(DQ_t *dq, s16q5_t angle, AB_t *alpha_beta) {
 	s16q14_t c,s;
 	SinCos_Lut(angle, &s, &c);
 
-	alpha_beta->a = S16_mul(dq->d, c, 14)  - S16_mul(dq->q, s, 14);
+	alpha_beta->a = S16_mul(dq->d, c, 14) - S16_mul(dq->q, s, 14);
 	alpha_beta->b = S16_mul(dq->d, s, 14) + S16_mul(dq->q, c, 14);
 }
 
@@ -50,10 +50,10 @@ static __INLINE s16q14_t Circle_Limitation(DQ_t *vdq, s16q5_t vDC, s16q14_t modu
 	return S16Q14(1); // s16q5 32 means int 1
 }
 
-static __INLINE void FOC_Set_DqRamp(idq_Ctrl *c, s16q5_t target) {
+static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, s16q5_t target, int time) {	
 	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);
+	c->s_Step = (c->s_FinalTgt - cp) / time;
 	if (c->s_Step == 0) {
 		if (c->s_FinalTgt - cp > 0) {
 			c->s_Step = S32Q14(1);	
@@ -63,20 +63,40 @@ static __INLINE void FOC_Set_DqRamp(idq_Ctrl *c, s16q5_t target) {
 	}
 }
 
-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_FinalTgt) {
-			c->s_Cp = c->s_FinalTgt;
-		}
-	}else {
-		if (c->s_Cp > c->s_FinalTgt) {
-			c->s_Cp = c->s_FinalTgt;
+static __INLINE s32q14_t FOC_Get_DqRamp(dq_Rctrl *c) {
+	if (++c->n_StepCount == c->n_CtrlCount) {
+		c->s_Cp += c->s_Step;
+		if (c->s_Step < 0) {
+			if (c->s_Cp < c->s_FinalTgt) {
+				c->s_Cp = c->s_FinalTgt;
+			}
+		}else {
+			if (c->s_Cp > c->s_FinalTgt) {
+				c->s_Cp = c->s_FinalTgt;
+			}
 		}
+		c->n_StepCount = 0;
 	}
 	return c->s_Cp;
 }
 
+static __INLINE void FOC_DqRamp_init(dq_Rctrl *c, int count) {
+	c->n_CtrlCount = count;
+	c->n_StepCount = 0;
+	c->s_Cp = 0;
+	c->s_FinalTgt = 0;
+	c->s_Step = 0;
+}
+
+static __INLINE void FOC_Set_iDqRamp(dq_Rctrl *c, s16q5_t target) {
+	FOC_Set_DqRamp(c, target, (IDQ_CTRL_TS/SPD_CTRL_TS - 1));
+}
+
+static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, s16q5_t target) {
+	FOC_Set_DqRamp(c, target, (VDQ_RAMP_FINAL_TIME/VDQ_RAMP_TS));
+}
+
+
 static void PMSM_FOC_Reset_PID(void) {
 	PI_Controller_Reset(_gFOC_Ctrl.id_ctl, 0);
 	PI_Controller_Reset(_gFOC_Ctrl.iq_ctl, 0);
@@ -101,8 +121,11 @@ void PMSM_FOC_CoreInit(void) {
 	_gFOC_Ctrl.out.f_vdqRation = S16Q14(0.9f);
 	_gFOC_Ctrl.in.s_manualAngle = 0xFFFF;
 
-	FOC_Set_DqRamp(_gFOC_Ctrl.dq_ctl, 0);
-	FOC_Set_DqRamp(_gFOC_Ctrl.dq_ctl + 1, 0);
+	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));	
 	PMSM_FOC_Reset_PID();
 }
 
@@ -113,8 +136,7 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 		_gFOC_Ctrl.in.s_motAngle = hall_sensor_get_theta();
 	}
 	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed();
-
-		//sample current
+	//sample current
 	phase_current_get(_gFOC_Ctrl.in.s_iABC);
 
 	_gFOC_Ctrl.in.s_vDC = get_vbus_sfix5();
@@ -124,11 +146,11 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 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);
+	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;
 void PMSM_FOC_Schedule(void) {
 	AB_t vAB;
 	s16q5_t *iabc = _gFOC_Ctrl.in.s_iABC;
@@ -140,11 +162,14 @@ void PMSM_FOC_Schedule(void) {
 
 		Park(&vAB, _gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.s_RealIdq);
 
-		s32q14_t err = FOC_Get_DqRamp(_gFOC_Ctrl.dq_ctl) - (_gFOC_Ctrl.out.s_RealIdq.d << 9);
+		s32q14_t err = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[0]) - (_gFOC_Ctrl.out.s_RealIdq.d << 9);
 		_gFOC_Ctrl.in.s_targetVdq.d = PI_Controller_run(_gFOC_Ctrl.id_ctl, err);
 		
-		err = FOC_Get_DqRamp(_gFOC_Ctrl.dq_ctl+1) - (_gFOC_Ctrl.out.s_RealIdq.q << 9);
+		err = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[1]) - (_gFOC_Ctrl.out.s_RealIdq.q << 9);
 		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_run(_gFOC_Ctrl.iq_ctl, err);
+	}else {
+		_gFOC_Ctrl.in.s_targetVdq.d = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[0]) >> 9;
+		_gFOC_Ctrl.in.s_targetVdq.q = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[1]) >> 9;
 	}
 	_gFOC_Ctrl.out.f_vdqRation = Circle_Limitation(&_gFOC_Ctrl.in.s_targetVdq, _gFOC_Ctrl.in.s_vDC, _gFOC_Ctrl.in.n_modulation, &_gFOC_Ctrl.out.s_OutVdq);
 
@@ -152,18 +177,21 @@ void PMSM_FOC_Schedule(void) {
 
 	SVM_Duty_Fix(&vAB, _gFOC_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &_gFOC_Ctrl.out);
 
+	if (_g_ctl_count++ % 10 == 0) {
+		plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
+		//plot_3data16(_gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.in.s_iABC[2]);
+	}
 	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_Sector);
-
-	plot_3data16(_gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.in.s_iABC[2]);
 }
 
 void PMSM_FOC_LogDebug(void) {
 	//sys_debug("Duty %d, %d, %d\n", _gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
-	//sys_debug("Vdq %f, %f-->%f, %f, %f\n", S16Q5toF(_gFOC_Ctrl.in.s_targetVdq.d), S16Q5toF(_gFOC_Ctrl.in.s_targetVdq.q), S16Q5toF(_gFOC_Ctrl.out.s_OutVdq.d), S16Q5toF(_gFOC_Ctrl.out.s_OutVdq.q), S16Q14toF(_gFOC_Ctrl.out.f_vdqRation));
-	sys_debug("iABC %f, %f, %f\n", S16Q5toF(_gFOC_Ctrl.in.s_iABC[0]), S16Q5toF(_gFOC_Ctrl.in.s_iABC[1]), S16Q5toF(_gFOC_Ctrl.in.s_iABC[2]));
+	sys_debug("Vdq %f, %f-->%f, %f, %f\n", S16Q5toF(_gFOC_Ctrl.in.s_targetVdq.d), S16Q5toF(_gFOC_Ctrl.in.s_targetVdq.q), S16Q5toF(_gFOC_Ctrl.out.s_OutVdq.d), S16Q5toF(_gFOC_Ctrl.out.s_OutVdq.q), S16Q14toF(_gFOC_Ctrl.out.f_vdqRation));
+	sys_debug("VBUS: %f, %d, %d\n", S16Q5toF(get_vbus_sfix5()), _gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2);
+	//sys_debug("iABC %f, %f, %f\n", S16Q5toF(_gFOC_Ctrl.in.s_iABC[0]), S16Q5toF(_gFOC_Ctrl.in.s_iABC[1]), S16Q5toF(_gFOC_Ctrl.in.s_iABC[2]));
 	//plot_1data16(_gFOC_Ctrl.in.s_iABC[0]);
 	//sys_debug("sample %d, %d\n", _gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2);
 }
@@ -240,8 +268,8 @@ void PMSM_FOC_SetCtrlMode(u8 mode) {
 }
 
 void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq) {
-	_gFOC_Ctrl.in.s_targetVdq.d = vd;
-	_gFOC_Ctrl.in.s_targetVdq.q = vq;
+	FOC_Set_vDqRamp(&_gFOC_Ctrl.vdq_ctl[0], vd);
+	FOC_Set_vDqRamp(&_gFOC_Ctrl.vdq_ctl[1], vq);
 }
 
 
@@ -286,7 +314,7 @@ bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 }
 
 void PMSM_FOC_Set_Angle(s16 angle) {
-	_gFOC_Ctrl.in.s_manualAngle = S16Q14(angle);
+	_gFOC_Ctrl.in.s_manualAngle = S16Q5(angle);
 }
 
 s32q4_t PMSM_FOC_GetSpeed(void) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -52,7 +52,9 @@ typedef struct {
 	s32q14_t s_FinalTgt;
 	s32q14_t s_Cp;
 	s32q14_t s_Step;
-}idq_Ctrl;
+	int      n_CtrlCount;
+	int      n_StepCount;
+}dq_Rctrl; //dq ramp ctrl
 
 typedef struct {
 	PI_Controller *id_ctl;
@@ -61,7 +63,8 @@ typedef struct {
 	PI_Controller *fw_ctl;
 	FOC_InP       in;
 	FOC_OutP      out;
-	idq_Ctrl      dq_ctl[2];
+	dq_Rctrl      idq_ctl[2];
+	dq_Rctrl      vdq_ctl[2];
 }PMSM_FOC_Ctrl;
 
 typedef enum {

Applications/foc/core/svpwm.c

@@ -1,6 +1,6 @@
 #include "foc/core/svpwm.h"
 #include "math/fast_math.h"
-
+#include "libs/logger.h"
 
 #if 0
 static void __inline ModuleTime(u32 *T4, u32 *T6, u32 PWM_Period) {
@@ -376,7 +376,7 @@ void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_perio
 }
 #endif
 
-
+#if 0
 void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 	s16q5_t alpha = S16_mul(alb->a, SQRT3_BY_2_Q14, 14);
 	s16q5_t beta  = S16_mul(alb->b, SQRT3_BY_2_Q14, 14);
@@ -384,9 +384,13 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 	u8 sector = 0xFF;
 	u32 tA, tB, tC;
 	u32 low, midle;
-	u32 X, Y, Z;
-	s16q5_t mod = ((PWM_Period << 10) / vbus) >> 5;
-
+	s32 X, Y, Z;
+	//static int _g_count = 0;
+#if 1
+	s16q5_t mod = ((PWM_Period << 10) / vbus);
+#else
+	float mod =  (float)(PWM_Period) / S16Q5toF(vbus);
+#endif
 	if (beta >= 0) {
 		if (alpha >= 0) {
 			//quadrant I
@@ -420,10 +424,17 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 			}
 		}
 	}
+#if 1
 	s16q5_t temp = S16_mul(ONE_BY_SQRT3_Q14, beta, 14);
-	X = S16_mul(TWO_BY_SQRT3_Q14, S16_mul(beta, mod, 5), 14);
-	Y = S16_mul((alpha + temp), mod, 5);
-	Z = S16_mul((-alpha + temp) ,mod, 5);
+	X = S16_mul(S16_mul(TWO_BY_SQRT3_Q14, beta, 14), mod, 10);
+	Y = S16_mul((alpha + temp), mod, 10);
+	Z = S16_mul((-alpha + temp) ,mod, 10);
+#else
+	float temp =  ONE_BY_SQRT3 * S16Q5toF(beta);
+	X = TWO_BY_SQRT3 * S16Q5toF(beta) * (mod);
+	Y = (S16Q5toF(alpha) + temp) * (mod);
+	Z = (-S16Q5toF(alpha) + temp) * (mod);
+#endif
 	switch(sector) {
 		case SECTOR_1: // 3
 		{	u32 T4 = -Z;
@@ -504,8 +515,145 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 	out->n_Duty[2] = tC;
 	out->n_lowDuty = low;
 	out->n_midDuty = midle;
-	//out->n_highDuty = high;
 	out->n_Sector = sector;
+#if 0
+	if (_g_count++ % 10 == 0) {
+		plot_3data16(X, Y, Z);
+	}
+#endif
 }
 
+#else
+void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
+	float alpha = S16Q5toF(alb->a) * SQRT3_BY_2;
+	float beta  = S16Q5toF(alb->b)  * SQRT3_BY_2;
+	u32   PWM_Period = PWM_half_period * 2;
+	u8 sector = 0xFF;
+	u32 tA, tB, tC;
+	u32 low, midle;
+	float X, Y, Z;
+	float modu = (float)(PWM_Period) / S16Q5toF(vbus);
+//	static int _g_count = 0;
+
+	if (beta >= 0.0f) {
+		if (alpha >= 0.0f) {
+			//quadrant I
+			if (ONE_BY_SQRT3 * beta > alpha) {
+				sector = SECTOR_2;
+			} else {
+				sector = SECTOR_1;
+			}
+		} else {
+			//quadrant II
+			if (-ONE_BY_SQRT3 * beta > alpha) {
+				sector = SECTOR_3;
+			} else {
+				sector = SECTOR_2;
+			}
+		}
+	} else {
+		if (alpha >= 0.0f) {
+			//quadrant IV5
+			if (-ONE_BY_SQRT3 * beta > alpha) {
+				sector = SECTOR_5;
+			} else {
+				sector = SECTOR_6;
+			}
+		} else {
+			//quadrant III
+			if (ONE_BY_SQRT3 * beta > alpha) {
+				sector = SECTOR_4;
+			} else {
+				sector = SECTOR_5;
+			}
+		}
+	}
+	//X = SQRT3 * beta * modu;
+	X = TWO_BY_SQRT3 * beta * modu;
+	//Y = (1.5f * alpha + SQRT3_BY_2 * beta) * modu;
+	Y = (alpha + ONE_BY_SQRT3 * beta) * modu;
+	//Z = (-1.5f * alpha + SQRT3_BY_2 * beta) * modu;
+	Z = (-alpha + ONE_BY_SQRT3 * beta) * modu;
+	switch(sector) {
+		case SECTOR_1: // 3
+		{	u32 T4 = -Z;
+			u32 T6 = X;
+			tC = (PWM_Period - T4 - T6)/4;
+        	tB = tC + T6/2;
+        	tA = tB + T4/2;
+			low = tA;
+			midle = tB;
+			break;
+		}
+		case SECTOR_2: // 1
+		{
+			u32 T6 = Y;
+			u32 T2 = Z;
+			tC = (PWM_Period - T6 - T2)/4;
+        	tA = tC + T6/2;
+        	tB = tA + T2/2;
+			low = tB;
+			midle = tA;
+			break;
+		}
+		case SECTOR_3: // 5
+		{
+			u32 T2 = X;
+			u32 T3 = -Y;
+			tA = (PWM_Period - T2 - T3)/4;
+			tC = tA + T3/2;
+			tB = tC + T2/2;
+			low = tB;
+			midle = tC;
+			break;
+		}
+		case SECTOR_4: // 4
+		{
+			u32 T1 = -X;
+			u32 T3 = Z;
+			tA = (PWM_Period - T1 - T3)/4;
+			tB = tA + T3/2;
+			tC = tB + T1/2;
+			low = tC;
+			midle = tB;
+			break;
+		}
+		case SECTOR_5: // 6
+		{
+			u32 T1 = -Y;
+			u32 T5 = -Z;
+			tB = (PWM_Period - T1 - T5)/4;
+			tA = tB + T5/2;
+			tC = tA + T1/2;
+			low = tC;
+			midle = tA;
+			break;
+		}					
+		case SECTOR_6: // 2
+		{
+			u32 T4 = Y;
+			u32 T5 = -X;
+			tB = (PWM_Period - T4 - T5)/4;
+			tC = tB + T5/2;
+			tA = tC + T4/2;
+			low = tA;
+			midle = tC;
+			break;
+		}
+		default:
+			break;
+	}	
+	out->n_Duty[0] = tA;
+	out->n_Duty[1] = tB;
+	out->n_Duty[2] = tC;
+	out->n_lowDuty = low;
+	out->n_midDuty = midle;
+	out->n_Sector = sector;
+#if 0	
+	if (_g_count++ % 10 == 0) {
+		plot_3data16(X, Y, Z);
+	}
+#endif	
+}
+#endif
 

Applications/foc/foc_config.h

@@ -28,6 +28,8 @@
 
 #define IDQ_CTRL_TS FOC_PWM_FS
 #define SPD_CTRL_TS 100
-#define SPD_CTRL_MS (1000/SPD_CTRL_TS)
+#define SPD_CTRL_MS (1000/SPD_CTRL_TS * 100)
+#define VDQ_RAMP_TS 100
+#define VDQ_RAMP_FINAL_TIME 3000
 #endif /* _FOC_CONFIG_H__ */
 

Applications/foc/motor/motor.c

@@ -192,15 +192,25 @@ void ADC_IRQHandler(void) {
 }
 
 /*FOC 的部分处理，比如速度环，状态机，转把采集等*/
+static s16 _g_test_angle = 0;
 void Sched_MC_mTask(void) {
 	u8 runMode = PMSM_FOC_CtrlMode();
+	if (_motor_started) {
+		PMSM_FOC_Set_Angle(_g_test_angle);
+		if (++_g_test_angle >= 360) {
+			_g_test_angle = 0;
+		}
+	}
 	if (runMode != OPEN_MODE) {
 		if (get_throttle_float() != _motor_throttle) {
 			_motor_throttle = get_throttle_float();
-			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_Trque(torque_idq);
+			if (runMode == SPD_MODE) {
+				float speed_Ref = _get_speed_from_throttle(_motor_throttle);
+				PMSM_FOC_Set_Speed(speed_Ref);
+			}else if (runMode == TRQ_MODE) {
+				float torque_idq = _get_idq_from_throttle(_motor_throttle);
+				PMSM_FOC_Set_Trque(torque_idq);
+			}
 		}		
 		PMSM_FOC_idqCalc();
 	}

Applications/math/fast_math.h

@@ -9,9 +9,9 @@
 #define SQRT2_BY_SQRT3          (0.8164966f)
 #define M_PI (3.14159265f)
 
-#define ONE_BY_SQRT3_Q14  (9459) //0.57735026919 * 16384.0F
-#define SQRT3_BY_2_Q14    (14189)//0.86602540378 * 16384.0F
-#define TWO_BY_SQRT3_Q14  (28378)
+#define ONE_BY_SQRT3_Q14  (9459L) //0.57735026919 * 16384.0F
+#define SQRT3_BY_2_Q14    (14189L)//0.86602540378 * 16384.0F
+#define TWO_BY_SQRT3_Q14  (18918L)
 
 #ifndef SQ
 #define SQ(x) ((x)*(x))

Project/GD32_DEMO.uvoptx

@@ -117,11 +117,6 @@
         <pMon>Segger\JL2CM3.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
-        <SetRegEntry>
-          <Number>0</Number>
-          <Key>DLGUARM</Key>
-          <Name>0</Name>
-        </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>ARMRTXEVENTFLAGS</Key>

Project/version.cfg

@@ -1,3 +1,3 @@
 project: MC100
 version: 01
-debug: 1
+debug: 0