粗调电流环OK

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

Applications/app/app.c

@@ -31,7 +31,7 @@ void fetch_jtag_cmd(void) {
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 3) {
-		PMSM_FOC_SetOpenVdq(0, S16Q5(jtag_data));
+		PMSM_FOC_SetOpenVdq(0, (jtag_data));
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 4) {
 		foc_cmd.cmd = Foc_Cali_Hall_Phase;
@@ -39,6 +39,9 @@ void fetch_jtag_cmd(void) {
 		encode_s16(cmd_data, jtag_data);
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
+	}else if (jtag_cmd == 5) {
+		PMSM_FOC_Set_Trque(1.0f);
+		jtag_cmd = 0;
 	}
 }
 #else

Applications/bsp/bsp.h

@@ -24,11 +24,11 @@
 #define ADC_CLOCK_MHz (30u)
 #define NS_PER_TCLK (8u) /* (1/120000000 * 1000000000) */
 #define NS_2_TCLK(ns) (((ns)/NS_PER_TCLK) + 1u) //ns תΪpwmʹ�õ��Ǹ�TIM��clk count
-#define FOC_PWM_FS (20000u)
+#define FOC_PWM_FS (16000u)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 
-#define FOC_CTRL_US (1000000u/FOC_PWM_FS)
+#define FOC_CTRL_US (1.0f/(float)FOC_PWM_FS)
 
 #define ADC_TRIG_CONV_LATENCY_CYCLES 12.5f
 #define ADC_SAMPLING_CYCLES 7.5f
@@ -80,12 +80,12 @@
 //#define ENABLE_AUX_TIMER 1
 
 #ifdef GD32_FOC_DEMO
-#define ADC_TO_CURR_ceof S16Q14(0.008f)
-#define VBUS_VOL_CEOF S16Q14(ADC_REFERENCE_VOLTAGE*16/4096.0f)
+#define ADC_TO_CURR_ceof (0.004f)
+#define VBUS_VOL_CEOF (ADC_REFERENCE_VOLTAGE*16/4096.0f)
 #else
-#define ADC_TO_CURR_ceof S16Q14(0.0942f)
-#define VBUS_VOL_CEOF S16Q14(ADC_REFERENCE_VOLTAGE*45/4096.0f)
-#define THROTTLE_VOL_CEOF S16Q14(1)
+#define ADC_TO_CURR_ceof (0.0942f)
+#define VBUS_VOL_CEOF (ADC_REFERENCE_VOLTAGE*45/4096.0f)
+#define THROTTLE_VOL_CEOF (1)
 #endif
 void bsp_init(void);
 void wdog_reload(void);

Applications/bsp/mc_hall_gpio.c

@@ -24,6 +24,7 @@ int get_hall_stat(int samples) {
 		h1 += READ_HALL1();
 		h2 += READ_HALL2();
 		h3 += READ_HALL3();
+		task_udelay(5);
 	}
 #if HALL_PLACE==DEGREES_60	
 	return (((h2>tres)^1) << 2) | ((h3 > tres) << 1) | (h2 > tres);

Applications/bsp/uart.c

@@ -135,11 +135,11 @@ 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 == 1) && (DMA_CHCNT(SHARK_UART0_tx_dma, uart->tx_dma_ch) == 0))) {
+			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
 			}
 		}
-
+		dma_flag_clear(SHARK_UART0_tx_dma, uart->tx_dma_ch, DMA_FLAG_FTF);
 		byte_queue_skip(&uart->tx_queue, uart->tx_length);
 		DMA_CHCTL(SHARK_UART0_tx_dma, uart->tx_dma_ch) = value & (~DMA_CHXCTL_CHEN);
 	}
@@ -148,9 +148,6 @@ static void shark_uart_dma_tx(shark_uart_t *uart)
 	if (uart->tx_length > 0) {
 		DMA_CHCNT(SHARK_UART0_tx_dma, uart->tx_dma_ch) = uart->tx_length;
 		DMA_CHMADDR(SHARK_UART0_tx_dma, uart->tx_dma_ch) = (u32) byte_queue_head(&uart->tx_queue);
-
-		dma_flag_clear(SHARK_UART0_tx_dma, uart->tx_dma_ch, DMA_FLAG_FTF);
-		dma_flag_clear(SHARK_UART0_tx_dma, uart->tx_dma_ch, DMA_FLAG_HTF);
 		DMA_CHCTL(SHARK_UART0_tx_dma, uart->tx_dma_ch) = value | DMA_CHXCTL_CHEN;
 	}
 }

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(OPEN_MODE);
+				success = mc_start(TRQ_MODE);
 			}else if (scmd->start_stop == Foc_Stop) {
 				success = mc_stop();
 			}
@@ -81,14 +81,14 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Cali_Hall_Phase:
 		{
 			s16 vd = decode_s16((u8 *)command->data);
-			mc_hall_calibrate(S16Q5(vd));
+			mc_hall_calibrate((vd));
 			break;
 		}
 		case Foc_Set_Open_Dq_Vol:
 		{
 			s16 vq = decode_s16(((u8 *)command->data) + 2);
 			sys_debug("set v_q %d\n", vq);
-			PMSM_FOC_SetOpenVdq(S16Q5(0), S16Q5(vq));
+			PMSM_FOC_SetOpenVdq((0), (vq));
 			break;
 		}
 		case Foc_Conf_Pid:

Applications/foc/core/PI_Controller.h

@@ -2,6 +2,7 @@
 #define _PI_Contrller_H__
 #include "math/fix_math.h"
 #include "bsp/bsp.h"
+#if 0
 typedef struct {
 	s16q5_t  kp;
 	s16q5_t  ki;
@@ -23,6 +24,37 @@ static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s16q5_t err) {
 	s32q14_t out = pi->Ui + kp_err;
 	return MATH_sat(out, pi->min, pi->max);
 }
+#else
+typedef struct {
+	float  kp;
+	float  ki;
+	float  max;
+	float  min;
+	float  Ui;
+	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;
+	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);
+}
+
+static __INLINE float PI_Controller_run(PI_Controller *pi, float err) {
+	float kp_err = (err) * pi->kp;//S16_mul(err,pi->kp, 5);
+	float ki_err = (err) * pi->ki;
+	float integral = ki_err * pi->DT;
+	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
+	float out = pi->Ui + kp_err;
+	return (MATH_sat(out, pi->min, pi->max));
+}
 
+#endif
 #endif	/*_PI_Contrller_H__*/
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -11,59 +11,58 @@
 #include "bsp/pwm.h"
 #include "libs/logger.h"
 
-static PMSM_FOC_Ctrl _gFOC_Ctrl;
+PMSM_FOC_Ctrl _gFOC_Ctrl;
 
-
-static __INLINE void RevPark(DQ_t *dq, s16q5_t angle, AB_t *alpha_beta) {
-	s16q14_t c,s;
+static __INLINE void RevPark(DQ_t *dq, float angle, AB_t *alpha_beta) {
+	float c,s;
 	SinCos_Lut(angle, &s, &c);
 
-	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);
+	alpha_beta->a = dq->d * c - dq->q * s;
+	alpha_beta->b = dq->d * s + dq->q * c;
 }
 
-static __INLINE void Clark(s16q5_t A, s16q5_t B, s16q5_t C, AB_t *alpha_beta){
-	alpha_beta->a = (2 * A - B - C) / 3;
-	alpha_beta->b = S16_mul(ONE_BY_SQRT3_Q14, (B - C), 14);
+static __INLINE void Clark(float A, float B, float C, AB_t *alpha_beta){
+	alpha_beta->a = A;
+	alpha_beta->b = ONE_BY_SQRT3 * (B - C);
 }
 
-static __INLINE void Park(AB_t *alpha_beta, s16q5_t angle, DQ_t *dq) {
-	s16q14_t c,s;
+static __INLINE void Park(AB_t *alpha_beta, float angle, DQ_t *dq) {
+	float c,s;
 	SinCos_Lut(angle, &s, &c);
 
-	dq->d = S16_mul(alpha_beta->a, c, 14) + S16_mul(alpha_beta->b, s, 14);
-	dq->q = S16_mul(-alpha_beta->a, s, 14) + S16_mul(alpha_beta->b, c, 14);
+	dq->d = alpha_beta->a * c + alpha_beta->b * s;
+	dq->q = -alpha_beta->a * s + alpha_beta->b * c;
 }
 
-static __INLINE s16q14_t Circle_Limitation(DQ_t *vdq, s16q5_t vDC, s16q14_t module, DQ_t *out) {
-	u32 sq_vdq = (u32)vdq->d * vdq->d + (u32)vdq->q * vdq->q;
-	s16q5_t vDC_m = S16_mul(vDC, module, 14);
-	u32 sq_vDC = (u32)vDC_m * vDC_m;
+static __INLINE float Circle_Limitation(DQ_t *vdq, float vDC, float module, DQ_t *out) {
+	float sq_vdq = vdq->d * vdq->d + vdq->q * vdq->q;
+	float vDC_m = vDC * module;
+	float sq_vDC = vDC_m * vDC_m;
 	if (sq_vdq > sq_vDC) {
-		s16q14_t r = S16Q14(sqrtf((float)sq_vDC / (float)sq_vdq));
-		out->d = S16_mul(vdq->d, r, 14);
-		out->q = S16_mul(vdq->q, r, 14);
+		float r = sqrtf(sq_vDC / sq_vdq);
+		out->d = vdq->d * r;
+		out->q = vdq->q * r;
 		return r;
 	}
 	out->d = vdq->d;
 	out->q = vdq->q;
-	return S16Q14(1); // s16q5 32 means int 1
+	return 1.0f; // s16q5 32 means int 1
 }
 
-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
+static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, float target, int time) {	
+	float cp = c->s_Cp;
+	c->s_FinalTgt = target; 
 	c->s_Step = (c->s_FinalTgt - cp) / time;
-	if (c->s_Step == 0) {
+	if ((c->s_Step == 0) && (c->s_FinalTgt - cp > 0)) {
 		if (c->s_FinalTgt - cp > 0) {
-			c->s_Step = S32Q14(1);	
-		}else {
-			c->s_Step = S32Q14(-1);
+			c->s_Step = 0.001;	
+		}else if (c->s_FinalTgt - cp < 0){
+			c->s_Step = 0.001;
 		}
 	}
 }
 
-static __INLINE s32q14_t FOC_Get_DqRamp(dq_Rctrl *c) {
+static __INLINE float FOC_Get_DqRamp(dq_Rctrl *c) {
 	if (++c->n_StepCount == c->n_CtrlCount) {
 		c->s_Cp += c->s_Step;
 		if (c->s_Step < 0) {
@@ -88,39 +87,46 @@ static __INLINE void FOC_DqRamp_init(dq_Rctrl *c, int count) {
 	c->s_Step = 0;
 }
 
-static __INLINE void FOC_Set_iDqRamp(dq_Rctrl *c, s16q5_t target) {
+static __INLINE void FOC_Set_iDqRamp(dq_Rctrl *c, float 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) {
+static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, float 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);
-	PI_Controller_Reset(_gFOC_Ctrl.spd_ctl, 0);
-	PI_Controller_Reset(_gFOC_Ctrl.fw_ctl, 0);
+	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_id, 0);
+	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);
 }
 
+
 void PMSM_FOC_CoreInit(void) {
-	_gFOC_Ctrl.id_ctl = &PI_Ctrl_ID;
-	_gFOC_Ctrl.iq_ctl = &PI_Ctrl_IQ;
-	_gFOC_Ctrl.spd_ctl = &PI_Ctrl_Spd;
-	_gFOC_Ctrl.fw_ctl = &PI_Ctrl_fw;
+	_gFOC_Ctrl.pi_ctl_id = &PI_Ctrl_ID;
+	_gFOC_Ctrl.pi_ctl_iq = &PI_Ctrl_IQ;
+	_gFOC_Ctrl.pi_ctl_spd = &PI_Ctrl_Spd;
+	_gFOC_Ctrl.pi_ctl_fw = &PI_Ctrl_fw;
 	memset(&_gFOC_Ctrl.in, 0, sizeof(_gFOC_Ctrl.in));
 	memset(&_gFOC_Ctrl.out, 0, sizeof(_gFOC_Ctrl.out));
-	_gFOC_Ctrl.in.n_poles = 4;
-	_gFOC_Ctrl.in.s_maxiDC = S16Q5(MAX_iDQ);
-	_gFOC_Ctrl.in.s_maxiDC = S16Q5(MAX_iDC);
-	_gFOC_Ctrl.in.s_maxRPM = S32Q14(MAX_SPEED);
-	_gFOC_Ctrl.in.s_vDC    = S16Q5(MAX_vDC);
-	_gFOC_Ctrl.in.n_modulation = S16Q14(0.95f);
+	_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.maxvDQ.d = MAX_vDC;
+	_gFOC_Ctrl.params.minvDQ.d = -MAX_vDC;
+	_gFOC_Ctrl.params.maxvDQ.q = MAX_vDC;
+	_gFOC_Ctrl.params.minvDQ.q = -MAX_vDC;
+	_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.f_vdqRation = S16Q14(0.9f);
+	_gFOC_Ctrl.out.f_vdqRation = 0;
 	_gFOC_Ctrl.in.s_manualAngle = 0x3D00;
-	_gFOC_Ctrl.in.n_PhaseFilterCeof = S16Q10(0.2f);
+	_gFOC_Ctrl.in.s_vDC = (MAX_vDC);
 	
 	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[0], 1);
 	FOC_DqRamp_init(&_gFOC_Ctrl.idq_ctl[1], 1);
@@ -140,44 +146,77 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 		_gFOC_Ctrl.in.s_motAngle = _gFOC_Ctrl.in.s_hallAngle;
 	}
 
-	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed();
+	_gFOC_Ctrl.in.s_motRPM = hall_sensor_get_speed() / _gFOC_Ctrl.params.n_poles;
+	_gFOC_Ctrl.in.s_vDC = get_vbus_float();
 	//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();
+#ifdef PHASE_LFP
+	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[0], _gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.params.n_PhaseFilterCeof);
+	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[1], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.params.n_PhaseFilterCeof);
+	LowPass_Filter(_gFOC_Ctrl.in.s_iABCFilter[2], _gFOC_Ctrl.in.s_iABC[2], _gFOC_Ctrl.params.n_PhaseFilterCeof);
+#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;
+	}
+	/* 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;
+	}
+	if (_gFOC_Ctrl.params.minvDQ.d != _gFOC_Ctrl.pi_ctl_id->min) {
+		_gFOC_Ctrl.pi_ctl_id->min = _gFOC_Ctrl.params.minvDQ.d;
+	}
+	/* update iq pi ctrl */
+	if (_gFOC_Ctrl.params.maxvDQ.q != _gFOC_Ctrl.pi_ctl_iq->max) {
+		_gFOC_Ctrl.pi_ctl_iq->max = _gFOC_Ctrl.params.maxvDQ.q;
+	}
+	if (_gFOC_Ctrl.params.minvDQ.q != _gFOC_Ctrl.pi_ctl_iq->min) {
+		_gFOC_Ctrl.pi_ctl_iq->min = _gFOC_Ctrl.params.minvDQ.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_iABCFilter;
+#ifdef PHASE_LFP	
+	float *iabc = _gFOC_Ctrl.in.s_iABCFilter;
+#else
+	float *iabc = _gFOC_Ctrl.in.s_iABC;
+#endif
+	_gFOC_Ctrl.ctrl_count++;
 
 	PMSM_FOC_Update_Hardware();
 
 	if (_gFOC_Ctrl.out.n_RunMode != OPEN_MODE) {
+
+		PMSM_FOC_Update_PI_Ctrls();
+	
 		Clark(iabc[0], iabc[1], iabc[2], &vAB);
 
 		Park(&vAB, _gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.s_RealIdq);
 
-		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.idq_ctl[1]) - (_gFOC_Ctrl.out.s_RealIdq.q << 9);
-		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_run(_gFOC_Ctrl.iq_ctl, err);
+		float target_d = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[0]);
+		float err = target_d - _gFOC_Ctrl.out.s_RealIdq.d;
+		_gFOC_Ctrl.in.s_targetVdq.d = PI_Controller_run(_gFOC_Ctrl.pi_ctl_id, err);
+
+		float target_q = FOC_Get_DqRamp(&_gFOC_Ctrl.idq_ctl[1]);
+		err = target_q - (_gFOC_Ctrl.out.s_RealIdq.q);
+		_gFOC_Ctrl.in.s_targetVdq.q = PI_Controller_run(_gFOC_Ctrl.pi_ctl_iq, 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.in.s_targetVdq.d = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[0]);
+		_gFOC_Ctrl.in.s_targetVdq.q = FOC_Get_DqRamp(&_gFOC_Ctrl.vdq_ctl[1]);
 	}
-	_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);
-
+	_gFOC_Ctrl.out.f_vdqRation = Circle_Limitation(&_gFOC_Ctrl.in.s_targetVdq, _gFOC_Ctrl.in.s_vDC, _gFOC_Ctrl.params.n_modulation, &_gFOC_Ctrl.out.s_OutVdq);
+	
 	RevPark(&_gFOC_Ctrl.out.s_OutVdq, _gFOC_Ctrl.in.s_motAngle, &vAB);
-
+	
 	SVM_Duty_Fix(&vAB, _gFOC_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &_gFOC_Ctrl.out);
 
 	phase_current_point(&_gFOC_Ctrl.out);
@@ -185,12 +224,13 @@ 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);
 
-	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(_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);
+	if (_gFOC_Ctrl.ctrl_count % 5 == 0) {
+		//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(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_1data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle));
 	}	
 }
 
@@ -228,10 +268,10 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 /*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();
+		_gFOC_Ctrl.in.s_targetTrq = (eCtrl_get_RefTorque());
 	}else {
-		s32q14_t errRef = eCtrl_get_RefSpd() - _gFOC_Ctrl.in.s_motRPM;
-		_gFOC_Ctrl.in.s_targetTrq = PI_Controller_run(_gFOC_Ctrl.spd_ctl, errRef);
+		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);
 		
 	}
 	PMSM_FOC_idq_Assign();
@@ -257,19 +297,19 @@ bool PMSM_FOC_Is_Start(void) {
 	return _gFOC_Ctrl.in.b_motEnable;
 }
 
-void PMSM_FOC_iBusLimit(s16q5_t ibusLimit) {
-	_gFOC_Ctrl.in.s_maxiDC = (ibusLimit);
+void PMSM_FOC_iBusLimit(float ibusLimit) {
+	_gFOC_Ctrl.params.s_maxiDC = (ibusLimit);
 }
 
-void PMSM_FOC_SpeedLimit(s32q4_t speedLimit) {
-	_gFOC_Ctrl.in.s_maxRPM = (speedLimit);
+void PMSM_FOC_SpeedLimit(float speedLimit) {
+	_gFOC_Ctrl.params.s_maxRPM = (speedLimit);
 }
 
 s32q4_t PMSM_FOC_GetSpeedLimit(void) {
-	return _gFOC_Ctrl.in.s_maxRPM;
+	return _gFOC_Ctrl.params.s_maxRPM;
 }
 
-void PMSM_FOC_VbusVoltage(s16q5_t vbusVol) {
+void PMSM_FOC_VbusVoltage(float vbusVol) {
 	_gFOC_Ctrl.in.s_vDC = vbusVol;
 }
 
@@ -277,7 +317,7 @@ void PMSM_FOC_SetCtrlMode(u8 mode) {
 	_gFOC_Ctrl.in.n_ctlMode = mode;
 }
 
-void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq) {
+void PMSM_FOC_SetOpenVdq(float vd, float vq) {
 	FOC_Set_vDqRamp(&_gFOC_Ctrl.vdq_ctl[0], vd);
 	FOC_Set_vDqRamp(&_gFOC_Ctrl.vdq_ctl[1], vq);
 }
@@ -301,7 +341,7 @@ bool PMSM_FOC_Is_CruiseEnabled(void) {
 	return (_gFOC_Ctrl.in.b_cruiseEna && (_gFOC_Ctrl.out.n_RunMode == SPD_MODE));
 }
 
-bool PMSM_FOC_Set_Speed(s32q4_t rpm) {
+bool PMSM_FOC_Set_Speed(float rpm) {
 	if (_gFOC_Ctrl.in.b_cruiseEna) {
 		return false;
 	}
@@ -323,8 +363,8 @@ bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 	return false;
 }
 
-void PMSM_FOC_Set_Angle(s16 angle) {
-	_gFOC_Ctrl.in.s_manualAngle = S16Q5(angle);
+void PMSM_FOC_Set_Angle(float angle) {
+	_gFOC_Ctrl.in.s_manualAngle = (angle);
 }
 
 s32q4_t PMSM_FOC_GetSpeed(void) {
@@ -337,24 +377,24 @@ void PMSM_FOC_LockMotor(bool lock) {
 }
 
 void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min) {
-	_gFOC_Ctrl.spd_ctl->kp = S32Q14(kp);
-	_gFOC_Ctrl.spd_ctl->ki = S32Q14(ki);
-	_gFOC_Ctrl.spd_ctl->max = S32Q14(max);
-	_gFOC_Ctrl.spd_ctl->min = S32Q14(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.id_ctl->kp = S32Q14(kp);
-	_gFOC_Ctrl.id_ctl->ki = S32Q14(ki);
-	_gFOC_Ctrl.id_ctl->max = S32Q14(max);
-	_gFOC_Ctrl.id_ctl->min = S32Q14(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.iq_ctl->kp = S32Q14(kp);
-	_gFOC_Ctrl.iq_ctl->ki = S32Q14(ki);
-	_gFOC_Ctrl.iq_ctl->max = S32Q14(max);
-	_gFOC_Ctrl.iq_ctl->min = S32Q14(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) {
@@ -362,10 +402,10 @@ 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.fw_ctl->kp = S32Q14(kp);
-	_gFOC_Ctrl.fw_ctl->ki = S32Q14(ki);
-	_gFOC_Ctrl.fw_ctl->max = S32Q14(max);
-	_gFOC_Ctrl.fw_ctl->min = S32Q14(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) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -3,38 +3,45 @@
 #include "math/fix_math.h"
 #include "foc/core/PI_Controller.h"
 typedef struct {
-	s16q5_t a;
-	s16q5_t b;
+	float a;
+	float b;
 }AB_t;
 
 typedef struct {
-	s16q5_t d;
-	s16q5_t q;
+	float d;
+	float q;
 }DQ_t;
 
 typedef struct {
-	u8 		n_poles;
-	s16q5_t s_maxIdq;
-	s16q5_t s_maxiDC;
-	s32q5_t  s_maxRPM;
-	s16q5_t s_iABC[3];
-	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;
+	u8 	  n_poles;
+	float s_maxIdq;
+	float s_minIdq;
+	float s_maxiDC;
+	float s_maxvDC;
+	DQ_t  maxvDQ;
+	DQ_t  minvDQ;
+	float s_maxRPM;
+	float n_modulation;
+	float n_PhaseFilterCeof;
+}FOC_Params;
+
+typedef struct {
+	float 	s_iABC[3];
+	float 	s_iABCFilter[3];
+	float   s_motRPM;   //from hall or encoder
+	float 	s_motAngle; //from hall or encoder
+	float 	s_hallAngle;
+	float   s_targetRPM;
+	float 	s_targetTrq;
 	DQ_t    s_targetIdq;
 	DQ_t    s_targetVdq;
-	s16q5_t s_vDC;
-	s16q14_t n_modulation;
-	s16q14_t n_PhaseFilterCeof;
+	float 	s_vDC;
 	u8      n_ctlMode;
 	bool    b_motEnable;
 	bool    b_cruiseEna;
 	bool    b_motLock;
 	bool    b_eBrake;
-	s16q5_t s_manualAngle; //mainly used when calibrate hall
+	float   s_manualAngle; //mainly used when calibrate hall
 }FOC_InP;
 
 typedef struct {
@@ -48,27 +55,29 @@ typedef struct {
 	u8    n_RunMode;
 	DQ_t  s_OutVdq;
 	DQ_t  s_RealIdq;
-	s16q14_t f_vdqRation;
+	float f_vdqRation;
 	u8    n_Error;
 }FOC_OutP;
 
 typedef struct {
-	s32q14_t s_FinalTgt;
-	s32q14_t s_Cp;
-	s32q14_t s_Step;
+	float s_FinalTgt;
+	float s_Cp;
+	float s_Step;
 	int      n_CtrlCount;
 	int      n_StepCount;
 }dq_Rctrl; //dq ramp ctrl
 
 typedef struct {
-	PI_Controller *id_ctl;
-	PI_Controller *iq_ctl;
-	PI_Controller *spd_ctl;
-	PI_Controller *fw_ctl;
+	PI_Controller *pi_ctl_id;
+	PI_Controller *pi_ctl_iq;
+	PI_Controller *pi_ctl_spd;
+	PI_Controller *pi_ctl_fw;
+	dq_Rctrl      idq_ctl[2];
+	dq_Rctrl      vdq_ctl[2];	
 	FOC_InP       in;
 	FOC_OutP      out;
-	dq_Rctrl      idq_ctl[2];
-	dq_Rctrl      vdq_ctl[2];
+	FOC_Params    params;
+	int           ctrl_count;
 }PMSM_FOC_Ctrl;
 
 typedef enum {
@@ -117,14 +126,14 @@ u8 PMSM_FOC_CtrlMode(void);
 void PMSM_FOC_idqCalc(void);
 void PMSM_FOC_Start(u8 nCtrlMode);
 void PMSM_FOC_Stop(void);
-void PMSM_FOC_iBusLimit(s16q5_t ibusLimit);
-void PMSM_FOC_SpeedLimit(s32q4_t speedLimit);
+void PMSM_FOC_iBusLimit(float ibusLimit);
+void PMSM_FOC_SpeedLimit(float speedLimit);
 s32q4_t PMSM_FOC_GetSpeedLimit(void);
-void PMSM_FOC_VbusVoltage(s16q5_t vbusVol);
+void PMSM_FOC_VbusVoltage(float vbusVol);
 void PMSM_FOC_SetCtrlMode(u8 mode);
-void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq);
+void PMSM_FOC_SetOpenVdq(float vd, float vq);
 bool PMSM_FOC_EnableCruise(bool enable);
-bool PMSM_FOC_Set_Speed(s32q4_t rpm);
+bool PMSM_FOC_Set_Speed(float rpm);
 bool PMSM_FOC_Set_Trque(float current);
 bool PMSM_FOC_Set_CruiseSpeed(float rpm);
 s32q4_t PMSM_FOC_GetSpeed(void);
@@ -137,10 +146,9 @@ void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min);
 void PMSM_FOC_SetIQPid(float kp, float ki, float max, float min);
 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);
-void PMSM_FOC_Set_Angle(s16 angle);
+void PMSM_FOC_Set_Angle(float angle);
 bool PMSM_FOC_Is_Start(void);
 void PMSM_FOC_SetErrCode(u8 error);
 u8 PMSM_FOC_GetErrCode(void);
-
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/core/PMSM_FOC_Params.h

@@ -1,41 +1,40 @@
 #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
+#include "foc/motor/motor_param.h"
+
 static PI_Controller PI_Ctrl_ID = {
-	.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),
+	.kp = (CURRENT_BANDWITH * MOTOR_Ld),
+	.ki = (CURRENT_BANDWITH * MOTOR_R),
+	.max = (MAX_vDC),
+	.min = (-MAX_vDC),
+	.DT  = (1.0f/(float)IDQ_CTRL_TS),
 	.Ui = 0,
 };
 
 static PI_Controller PI_Ctrl_IQ = {
-	.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),
+	.kp = (CURRENT_BANDWITH * MOTOR_Lq),
+	.ki = (CURRENT_BANDWITH * MOTOR_R),
+	.max = (MAX_vDC),
+	.min = (-MAX_vDC),
+	.DT  = (1.0f/(float)IDQ_CTRL_TS),
 	.Ui = 0,
 };
 
 static PI_Controller PI_Ctrl_Spd = {
-	.kp = S16Q5(0.001f),
-	.ki = S16Q5(0.003f),
-	.max = S16Q5(MAX_iDQ),
-	.min = S16Q5(-MAX_iDQ),
-	.DT  = S16Q14(1.0f/(float)SPD_CTRL_TS),
+	.kp = (0.001f),
+	.ki = (0.003f),
+	.max = (MAX_iDQ),
+	.min = (-MAX_iDQ),
+	.DT  = (1.0f/(float)SPD_CTRL_TS),
 	.Ui = 0,
 };
 
 static PI_Controller PI_Ctrl_fw = {
-	.kp = S16Q5(0.001f),
-	.ki = S16Q5(0.003f),
-	.max = S16Q5(20),
-	.min = S16Q5(0),
-	.DT  = S16Q14(1.0f/(float)SPD_CTRL_TS),
+	.kp = (0.001f),
+	.ki = (0.003f),
+	.max = (20),
+	.min = (0),
+	.DT  = (1.0f/(float)SPD_CTRL_TS),
 	.Ui = 0,
 };
 

Applications/foc/core/e_ctrl.c

@@ -1,7 +1,11 @@
 #include "e_ctrl.h"
 #include "foc/foc_config.h"
+#include "math/fix_math.h"
+#include "libs/logger.h"
 
 static e_Ctrl g_eCtrl;
+static void _eCtrl_set_TrqCurrent(float c);
+static void _eCtrl_set_TargetSpeed(float s);
 
 void eCtrl_init(u16 ebrk_time, u16 accl_time){
 	g_eCtrl.ebrk_time = ebrk_time;
@@ -12,101 +16,92 @@ void eCtrl_init(u16 ebrk_time, u16 accl_time){
 	if (g_eCtrl.ebrk_time == 0) {
 		g_eCtrl.ebrk_time = DEFAULT_D_TIME;
 	}
-	ramp_ctrl_init(&g_eCtrl.torque_ramp);
-	ramp_ctrl_init(&g_eCtrl.speed_ramp);
+	g_eCtrl.ebrk_shadow = g_eCtrl.ebrk_time;
+	g_eCtrl.accl_shadow = g_eCtrl.accl_time;
 
-	ramp_set_points(&g_eCtrl.torque_ramp, THROTTLE_MIN_IDQ, THROTTLE_MIN_IDQ);
-	ramp_set_step_value(&g_eCtrl.torque_ramp, TORQUE_STEP);
+	eRamp_init(&g_eCtrl.torque);
+	eRamp_init(&g_eCtrl.speed);
+}
 
-	ramp_set_points(&g_eCtrl.speed_ramp, THROTTLE_MIN_RPM, THROTTLE_MIN_RPM);
-	ramp_set_step_value(&g_eCtrl.speed_ramp, SPEED_STEP);
+void eCtrl_set_accl_brk(u16 accl_time, u16 ebrk_time) {
+	g_eCtrl.accl_shadow = accl_time;
+	g_eCtrl.ebrk_shadow = ebrk_time;
 }
 
 
 void eCtrl_set_TrqCurrent(float c) {
-	ramp_t *ramp = &g_eCtrl.torque_ramp;
-	float c_now = ramp_get_interpolation(ramp);
-	float step_val = TORQUE_STEP;
-	int   sign = 1;
+	g_eCtrl.torque_shadow = c;
+}
+void eCtrl_set_TargetSpeed(float s) {
+	g_eCtrl.speed_shadow = s;
+}
 
-	if (c < c_now) {
-		sign = -1;
+void eCtrl_Running(void) {
+	bool etime_changed = false;
+	if (g_eCtrl.accl_shadow != g_eCtrl.accl_time || g_eCtrl.ebrk_shadow != g_eCtrl.ebrk_time) {
+		g_eCtrl.ebrk_time = g_eCtrl.ebrk_shadow;
+		g_eCtrl.accl_time = g_eCtrl.accl_shadow;
+		etime_changed = true;
 	}
-	u32 step_count = 1; 
-	u32 step_ms = eCTRL_STEP_TS;	
-	if (sign > 0) { //增加扭矩
-		step_count = (c - c_now)/TORQUE_STEP + 1;
-		step_ms = g_eCtrl.accl_time / step_count;	
-		if (step_ms < eCTRL_STEP_TS) {
-			step_ms = eCTRL_STEP_TS;
-			step_val = (c - c_now)/(g_eCtrl.accl_time / step_ms);
-		}
-	}else if (sign < 0) {
-		step_count = (c_now - c)/TORQUE_STEP + 1;
-		step_ms = g_eCtrl.ebrk_time / step_count;
-		if (step_ms < eCTRL_STEP_TS) {
-			step_ms = eCTRL_STEP_TS;
-			step_val = (c_now - c)/(g_eCtrl.ebrk_time / step_ms);
-		}		
+	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target || etime_changed) {
+		_eCtrl_set_TrqCurrent(g_eCtrl.torque_shadow);
 	}
-	ramp_set_step_value(ramp, sign * step_val);
-	ramp_set_step_time(ramp, step_ms);
-	ramp_set_target(ramp, c);
-	
-	ramp_exc(ramp);
+	if (g_eCtrl.speed_shadow != g_eCtrl.speed.target || etime_changed) {
+		_eCtrl_set_TargetSpeed(g_eCtrl.speed_shadow);
+	}
+	eRamp_running(&g_eCtrl.torque);
+	eRamp_running(&g_eCtrl.speed);
 }
 
-
-void eCtrl_set_TargetSpeed(float s) {
-	ramp_t *ramp = &g_eCtrl.speed_ramp;
-	float s_now = ramp_get_interpolation(ramp);
-	float step_val = SPEED_STEP;
+static void _eCtrl_set_target(e_Ramp *ramp, float c) {
+	float c_now = eRamp_get_intepolation(ramp);
+	float step_val = 0;
 	int   sign = 1;
 
-	if (s < s_now) {
+	if (c < c_now) {
 		sign = -1;
 	}
-	u32 step_count = 1; 
 	u32 step_ms = eCTRL_STEP_TS;	
-	if (sign > 0) { //加速
-		step_count = (s - s_now)/SPEED_STEP + 1;
-		step_ms = g_eCtrl.accl_time / step_count;	
-		if (step_ms < eCTRL_STEP_TS) {
-			step_ms = eCTRL_STEP_TS;
-			step_val = (s - s_now)/(g_eCtrl.accl_time / step_ms);
+	if (sign > 0) { //增加扭矩
+		step_val = (c_now - c)/(g_eCtrl.accl_time/step_ms);
+		if (step_val < MIN_S16Q5) {
+			step_val = MIN_S16Q5;
 		}
 	}else if (sign < 0) {
-		step_count = (s_now - s)/SPEED_STEP + 1;
-		step_ms = g_eCtrl.ebrk_time / step_count;
-		if (step_ms < eCTRL_STEP_TS) {
-			step_ms = eCTRL_STEP_TS;
-			step_val = (s_now - s)/(g_eCtrl.ebrk_time / step_ms);
-		}		
+		step_val = (c_now - c)/(g_eCtrl.ebrk_time/step_ms);
+		if (step_val < MIN_S16Q5) {
+			step_val = MIN_S16Q5;
+		}
 	}
-	ramp_set_step_value(ramp, sign * step_val);
-	ramp_set_step_time(ramp, step_ms);
-	ramp_set_target(ramp, s);
-	
-	ramp_exc(ramp);
+	eRamp_set_target(ramp, c);
+	eRamp_set_step(ramp, step_val);
+
+}
+static void _eCtrl_set_TrqCurrent(float c) {
+	_eCtrl_set_target(&g_eCtrl.torque, c);
+}
+
+static void _eCtrl_set_TargetSpeed(float s) {
+	_eCtrl_set_target(&g_eCtrl.speed, s);
 }
 
 
 
 float eCtrl_get_RefSpd(void) {
-	return ramp_get_interpolation(&g_eCtrl.speed_ramp);
+	return eRamp_get_intepolation(&g_eCtrl.speed);
 }
 
 float eCtrl_get_RefTorque(void) {
-	return ramp_get_interpolation(&g_eCtrl.torque_ramp);
+	return eRamp_get_intepolation(&g_eCtrl.torque);
 }
 
 
 float eCtrl_get_FinalSpd(void) {
-	return ramp_get_target(&g_eCtrl.speed_ramp);
+	return eRamp_get_target(&g_eCtrl.speed);
 }
 
 float eCtrl_get_FinalTorque(void) {
-	return ramp_get_target(&g_eCtrl.torque_ramp);
+	return eRamp_get_target(&g_eCtrl.torque);
 }
 
 void eCtrl_brake_signal(bool hw_brake) {

Applications/foc/core/e_ctrl.h

@@ -3,17 +3,62 @@
 #include "os/os_types.h"
 #include "foc/core/ramp_ctrl.h"
 
+typedef struct {
+	float start;
+	float target;
+	float interpolation;
+	float step_val;	
+}e_Ramp;
 
 typedef struct {
 	u16  ebrk_time; //能量回收，时间越短，刹车性能或者回收越好
 	u16  accl_time; //加速时间（ms），时间越短，加速性能越好
 	bool hw_brake;
 	u64  brake_ts;//检测到刹车开始时间
-	ramp_t torque_ramp;
-	ramp_t speed_ramp;
+	e_Ramp torque;
+	e_Ramp speed;
+	u16  ebrk_shadow;
+	u16  accl_shadow;
+	float torque_shadow;
+	float speed_shadow;
 }e_Ctrl;
+static void eRamp_init(e_Ramp *r) {
+	r->start = 0;
+	r->target = 0;
+	r->interpolation = 0;
+	r->step_val = 0;
+}
+static void eRamp_set_target(e_Ramp *r, float target) {
+	r->target = target;
+}
+static void eRamp_set_step(e_Ramp *r, float step) {
+	r->step_val = step;
+}
+
+static void eRamp_running(e_Ramp *r) {
+	float target = r->interpolation + r->step_val;
+	if (r->step_val < 0) {
+		if (target < r->target) {
+			target = r->target;
+		}
+	}else {
+		if (target > r->target) {
+			target = r->target;
+		}
+	}
+	r->interpolation = target;	
+}
+
+static float eRamp_get_intepolation(e_Ramp *r) {
+	return r->interpolation;
+}
+
+static float eRamp_get_target(e_Ramp *r) {
+	return r->target;
+}
 
 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);
@@ -21,6 +66,7 @@ float eCtrl_get_RefSpd(void);
 float eCtrl_get_RefTorque(void);
 float eCtrl_get_FinalSpd(void);
 float eCtrl_get_FinalTorque(void);
+void eCtrl_Running(void);
 
 #endif /* EBRAKE_CTRL_H__ */
 

Applications/foc/core/svpwm.c

@@ -525,14 +525,14 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 
 #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;
+	float alpha = (alb->a) * SQRT3_BY_2;
+	float beta  = (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);
+	float modu = (float)(PWM_Period) / (vbus);
 //	static int _g_count = 0;
 
 	if (beta >= 0.0f) {

Applications/foc/foc_config.h

@@ -31,5 +31,9 @@
 #define SPD_CTRL_MS (1000/SPD_CTRL_TS * 100)
 #define VDQ_RAMP_TS 100
 #define VDQ_RAMP_FINAL_TIME 3000
+#define CURRENT_BANDWITH 100 /* 电流环带宽 */
+
+#define SVM_Modulation (0.96f)
+
 #endif /* _FOC_CONFIG_H__ */
 

Applications/foc/motor/current.c

@@ -94,7 +94,7 @@ bool phase_current_offset(void) {
 }
 
 
-void phase_current_get(s16 *iABC){
+void phase_current_get(float *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] = 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);
+	iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;
+	iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof;
+	iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
 }
 
 #if 0

Applications/foc/motor/current.h

@@ -46,7 +46,7 @@ typedef struct current_sample {
 }current_samp_t;
 
 void phase_current_init(void);
-void phase_current_get(s16 *iABC);
+void phase_current_get(float *iABC);
 bool phase_current_offset(void);
 void phase_current_point(void *);
 void phase_current_adc_triger(void);

Applications/foc/motor/hall.c

@@ -19,7 +19,7 @@ static u32 _hall_detect_task(void *args);
 static void _hall_init_el_angle(void);
 
 
-#define HALL_PLACE_OFFSET (230 << 19) //(345) //315
+#define HALL_PLACE_OFFSET (230) //(345) //315
 /* 
 4,5,1,3,2,6,4
 */
@@ -29,11 +29,11 @@ hall_t _sensor_hander;
 measure_time_t g_meas_hall = {.exec_max_time = 6,};
 
 //#define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
-#define us_2_s(tick) S32Q14((float)tick / 1000000.0f) //s32q14
+#define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
 
 #define rand_angle(a) {if (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;else if (a < 0) a +=PHASE_360_DEGREE;};
 
-static void __inline _hall_put_sample(u32 ticks, s32q19_t angle) {
+static void __inline _hall_put_sample(u32 ticks, float angle) {
 	hall_sample_t *s = &_sensor_hander.samples;
 	s->ticks_sum -= s->ticks[s->index];
 	s->angles_sum -= s->angles[s->index];
@@ -57,7 +57,7 @@ static u32 __inline__ _hall_get_angle_ticks(void) {
 	}
 }
 
-static s32q5_t __inline _hall_angle_speed(void){
+static float __inline _hall_angle_speed(void){
 	hall_sample_t *s = &_sensor_hander.samples;
 	if (s->ticks_sum == 0) {
 		return 0.0f;
@@ -109,22 +109,23 @@ static u32 _hall_detect_task(void *args) {
 		u32 ticks_now = timer_count32_get();
 		if (ticks_now > _sensor_hander.hall_ticks) {
 			if (timer_count32_delta(ticks_now, _sensor_hander.hall_ticks) > 2000*1000) {
-				hall_sensor_clear();
+				//hall_sensor_clear();
 			}
 		}
 	}
 	return 0;
 }
 
-s16q5_t hall_sensor_get_theta(void){
-	s32q19_t angle_add = _sensor_hander.delta_angle_ts;
+#if 1
+float hall_sensor_get_theta(void){
+	float angle_add = _sensor_hander.delta_angle_ts;
 	if (_sensor_hander.comp_count > 0) {
 		_sensor_hander.comp_count--;
 		angle_add += _sensor_hander.angle_comp_ts;
 	}
 	_sensor_hander.estimate_delta_angle += angle_add;
 
-	s32q19_t el_angle = _sensor_hander.estimate_delta_angle;
+	float el_angle = _sensor_hander.estimate_delta_angle;
 	if (el_angle > _sensor_hander.next_delta_angle) {
 		el_angle = _sensor_hander.next_delta_angle;
 	}
@@ -136,12 +137,37 @@ s16q5_t hall_sensor_get_theta(void){
 
 	rand_angle(el_angle);
 
-	return (el_angle >> 14);
+	return (el_angle);
+}
+#else
+static int test_loop = 0;
+float hall_sensor_get_theta(void){
+	float angle_add = _sensor_hander.delta_angle_ts;
+	float comp_angle = 0;
+	if (_sensor_hander.comp_count > 0) {
+		_sensor_hander.comp_count--;
+		comp_angle = _sensor_hander.angle_comp_ts;
+	}
+
+	if (_sensor_hander.direction == POSITIVE) {
+		_sensor_hander.estimate_delta_angle += angle_add;
+		_sensor_hander.estimate_el_angle += (angle_add + comp_angle);
+	}else {
+		_sensor_hander.estimate_delta_angle -= angle_add;
+		_sensor_hander.estimate_el_angle -= (angle_add + comp_angle);
+	}
+	rand_angle(_sensor_hander.estimate_el_angle);
+	test_loop ++;
+	if (test_loop % 20 == 0) {
+		//plot_3data16(_sensor_hander.estimate_el_angle, _sensor_hander.angle_comp_ts * 1000, _sensor_hander.comp_count);
+	}
+	return ( _sensor_hander.estimate_el_angle);
 }
 
+#endif
 
-s32q5_t hall_sensor_get_speed(void) {
-	return _sensor_hander.rpm;
+float hall_sensor_get_speed(void) {
+	return (_sensor_hander.rpm);
 }
 
 int hall_offset_increase(int inc) {
@@ -248,7 +274,7 @@ static void _hall_init_el_angle(void) {
 	_sensor_hander.hall_ticks = timer_count32_get();
 }
 
-static __inline__ s32 _get_angle(u8 state, s32 added) {
+static __inline__ float _get_angle(u8 state, float added) {
 #ifdef USE_DETECTED_ANGLE
 	return _sensor_hander.phase_offset + _sensor_hander.angle_table[state];
 #else
@@ -256,8 +282,8 @@ static __inline__ s32 _get_angle(u8 state, s32 added) {
 #endif
 }
 /* 4,5,1,3,2,6,4 */
-static s32 _hall_position(u8 state_now, u8 state_prev) {
-	s32 theta_now =  0xFFFFFFFF;
+static float _hall_position(u8 state_now, u8 state_prev) {
+	float theta_now =  0xFFFFFFFF;
 	switch (state_now) {
 		case STATE_1:
 			if (state_prev == STATE_5) {
@@ -391,7 +417,7 @@ void HALL_IRQHandler(void) {
 	u32 hall_ticks_now = timer_count32_get();	
 
 	/*获取当前转子角度*/
-	s32 theta_now = _hall_position(hall_stat_now, hall_stat_prev);
+	float theta_now = _hall_position(hall_stat_now, hall_stat_prev);
 	if (theta_now == 0xFFFFFFFF) {
 		return;
 	}
@@ -403,16 +429,16 @@ void HALL_IRQHandler(void) {
 	}
 	//获取两次中断之间转子转过的角度,获取预期的下次hall状态变换转过的角度
 #ifdef USE_DETECTED_ANGLE
-	s32 delta_angle = _get_delta_angle(hall_stat_prev, hall_stat_now);
-	s32 next_delta_angle = _get_delta_angle(hall_stat_now, _next_hall(hall_stat_now));
+	float delta_angle = _get_delta_angle(hall_stat_prev, hall_stat_now);
+	float next_delta_angle = _get_delta_angle(hall_stat_now, _next_hall(hall_stat_now));
 #else
-	s32 delta_angle = PHASE_60_DEGREE;
-	s32 next_delta_angle = delta_angle;
+	float delta_angle = PHASE_60_DEGREE;
+	float next_delta_angle = delta_angle;
 #endif
-	s32 delta_time = us_2_s(delta_us);
-	s32 prev_imme_el_speed = _sensor_hander.immediately_el_speed + 1;
+	float delta_time = us_2_s(delta_us);
+	float prev_imme_el_speed = _sensor_hander.immediately_el_speed + 1;
 	_sensor_hander.immediately_el_speed = delta_angle/delta_time; //s32q5
-	s32 delta_el_speed = abs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
+	float delta_el_speed = abs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
 	if (delta_el_speed*100/prev_imme_el_speed >= 40) { //即时速度增加40%,认为不稳定，需要使用即时速度估计转子位置
 		_sensor_hander.trns_detect = true;
 	}else {
@@ -421,20 +447,20 @@ void HALL_IRQHandler(void) {
 	_hall_put_sample(delta_us, delta_angle);
 	os_disable_irq();
 	if (_sensor_hander.samples.full) {
-		//s32 estimate_delta_angle =  _sensor_hander.next_delta_angle - _sensor_hander.estimate_delta_angle;
+		//float estimate_delta_angle =  _sensor_hander.next_delta_angle - _sensor_hander.estimate_delta_angle;
 		//plot_2data16(estimate_delta_angle>>19, (estimate_delta_angle/((s32)(delta_us/FOC_CTRL_US)))>>10);//, _sensor_hander.estimate_delta_angle>>19);
 		/*通过上次预估的转子位置，对当前的预估速度进行补偿*/
-		_sensor_hander.comp_count = 0;//(s32)(delta_us/FOC_CTRL_US)/2;
-		_sensor_hander.angle_comp_ts = 0;//estimate_delta_angle/_sensor_hander.comp_count;
-		_sensor_hander.estimate_el_angle = theta_now;
 		delta_us = _hall_get_angle_ticks();
+		//_sensor_hander.comp_count = 20;
+		//_sensor_hander.angle_comp_ts = estimate_delta_angle/(float)_sensor_hander.comp_count;
+		_sensor_hander.estimate_el_angle = theta_now;
 	}else {
 		_sensor_hander.comp_count = 0;
 		_sensor_hander.angle_comp_ts = 0;
 		_sensor_hander.estimate_el_angle = theta_now;
 	}
 	_sensor_hander.estimate_delta_angle = 0;
-	_sensor_hander.delta_angle_ts = (next_delta_angle/((delta_us<<6)/FOC_CTRL_US))<<6;
+	_sensor_hander.delta_angle_ts = (next_delta_angle/(us_2_s(delta_us)/FOC_CTRL_US));
 	_sensor_hander.next_delta_angle = next_delta_angle;
 	_sensor_hander.measured_el_angle = theta_now;
 
@@ -445,6 +471,8 @@ void HALL_IRQHandler(void) {
 	_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);
+
+	//plot_3data16(delta_us/10, hall_stat_prev * 10, _sensor_hander.hall_stat * 10);
 }
 
 

Applications/foc/motor/hall.h

@@ -8,14 +8,22 @@
 #define POSITIVE          (int8_t)1
 
 //S32Q19 格式
-#define PHASE_0_DEGREE (0)
+#define PHASE_0_DEGREE (0.0f)
+#if 0
 #define PHASE_60_DEGREE (983040*32)
 #define PHASE_120_DEGREE (1966080*32)
 #define PHASE_180_DEGREE (2949120*32)
 #define PHASE_240_DEGREE (3932160*32)
 #define PHASE_300_DEGREE (4915200*32)
 #define PHASE_360_DEGREE (5898240*32)
-
+#else
+#define PHASE_60_DEGREE (60.0f)
+#define PHASE_120_DEGREE (120.0f)
+#define PHASE_180_DEGREE (180.0f)
+#define PHASE_240_DEGREE (240.0f)
+#define PHASE_300_DEGREE (300.0f)
+#define PHASE_360_DEGREE (360.0f)
+#endif
 #define STATE_0 (uint8_t)0
 #define STATE_1 (uint8_t)1
 #define STATE_2 (uint8_t)2
@@ -29,40 +37,40 @@
 #define SAMPLE_MAX_COUNT 4
 
 typedef struct {
-	u32        	ticks[SAMPLE_MAX_COUNT];
-	s32q19_t   	angles[SAMPLE_MAX_COUNT];
+	u32       	ticks[SAMPLE_MAX_COUNT];
+	float   	angles[SAMPLE_MAX_COUNT];
 	u32   		ticks_sum;
-	s32   		angles_sum;
+	float   	angles_sum;
 	u32   		index;
 	bool  		full;
 }hall_sample_t;
 
 typedef struct {
-	s32q19_t 		estimate_el_angle; //60度区间内的估计电角度
-	s32q19_t 		estimate_delta_angle;		
-	s32q19_t   		measured_el_angle; //hall测量到的电角度
-	s32q19_t 		next_delta_angle;
-	s32q19_t        delta_angle_ts;    //每个控制周期角度的增加量，通过上次的速度计算得到
-	s32q19_t        angle_comp_ts;     //每个控制周期角度增加的补偿量，主要用在加减速，hall角度和60度有偏差的情况
+	float 		estimate_el_angle; //60度区间内的估计电角度
+	float 		estimate_delta_angle;		
+	float   		measured_el_angle; //hall测量到的电角度
+	float 		next_delta_angle;
+	float        delta_angle_ts;    //每个控制周期角度的增加量，通过上次的速度计算得到
+	float        angle_comp_ts;     //每个控制周期角度增加的补偿量，主要用在加减速，hall角度和60度有偏差的情况
 	int             comp_count;
-	s32q5_t 		immediately_el_speed; //当前的即时速度,主要用来判断电机转动是否达到稳定
-	s32q5_t 		el_speed; 		//当前的平均效果的电角速度, 单位：rad/s
-	s32q5_t  		rpm;        		//当前的电速度, 单位：RPM
+	float 		immediately_el_speed; //当前的即时速度,主要用来判断电机转动是否达到稳定
+	float 		el_speed; 		//当前的平均效果的电角速度, 单位：rad/s
+	float  		rpm;        		//当前的电速度, 单位：RPM
 	bool  			trns_detect;     //速度变化超过阈值
 	u8    			hall_stat;
 	u32   			hall_ticks;	
 	s8    			direction;
-	s32q19_t   		phase_offset;
+	float   		phase_offset;
 	hall_sample_t 	samples;
 	u32  			sensor_error;
-	s16q5_t         manual_angle;
-	s32q19_t  		angle_table[8];
+	float         manual_angle;
+	s32  		angle_table[8];
 }hall_t;
 
 void hall_sensor_init(void);
 void hall_sensor_clear(void);
-s16q5_t hall_sensor_get_theta(void); //return degree
-s32q5_t hall_sensor_get_speed(void); //return rpm;
+float hall_sensor_get_theta(void); //return degree
+float hall_sensor_get_speed(void); //return rpm;
 int hall_offset_increase(int inc);
 s32 *hall_get_table(void);
 bool hall_detect_angle_finish(void);

Applications/foc/motor/motor.c

@@ -13,9 +13,10 @@
 #include "libs/logger.h"
 #include "bsp/sched_timer.h"
 #include "foc/motor/hall.h"
+#include "foc/core/e_ctrl.h"
+#include "foc/motor/motor_param.h"
 
-static bool _motor_started = false;
-static float _motor_throttle = 0;
+static motor_t motor;
 
 void mc_init(void) {
 	adc_init();
@@ -27,8 +28,12 @@ void mc_init(void) {
 	sched_timer_enable(SPD_CTRL_MS);
 }
 
+motor_t * mc_params(void) {
+	return &motor;
+}
+
 bool mc_start(u8 mode) {
-	if (_motor_started) {
+	if (motor.b_start) {
 		return true;
 	}
 	if (mode > TRQ_MODE) {
@@ -43,6 +48,7 @@ bool mc_start(u8 mode) {
 		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
 		return false;
 	}
+	eCtrl_init(1000, 3000);
 	hall_sensor_clear();
 	PMSM_FOC_Start(mode);
 	pwm_turn_on_low_side();
@@ -51,14 +57,14 @@ bool mc_start(u8 mode) {
 	pwm_start();
 	adc_start_convert();
 	phase_current_wait_cali();
-	_motor_throttle = 0;
-	_motor_started = true;
+	motor.throttle = 0;
+	motor.b_start = true;
 	gpio_led2_enable(true);
 	return true;
 }
 
 bool mc_stop(void) {
-	if (!_motor_started) {
+	if (!motor.b_start) {
 		return true;
 	}
 	if (PMSM_FOC_GetSpeed() > 10) {
@@ -72,11 +78,21 @@ bool mc_stop(void) {
 	adc_stop_convert();
 	pwm_stop();
 	PMSM_FOC_Stop();
-	_motor_started = false;
+	motor.b_start = false;
 	gpio_led2_enable(false);
 	return true;
 }
 
+void mc_set_spd_torque(s32 target) {
+	motor.b_ignor_throttle = true;
+	motor.s_targetFix = target;
+}
+
+void mc_use_throttle(void) {
+	motor.b_ignor_throttle = false;
+}
+
+
 void mc_hall_calibrate(s16 vd) {
 	if (PMSM_FOC_Is_Start()) {
 		return;
@@ -113,7 +129,7 @@ bool mc_lock_motor(bool lock) {
 		return false;
 	}
 	PMSM_FOC_LockMotor(lock); //if mot enabled, foc core will do lock
-	if (!_motor_started) {
+	if (!motor.b_start) {
 		if (lock) {
 			pwm_start();
 			pwm_update_duty(0, 0, 0);
@@ -130,8 +146,11 @@ bool mc_throttle_released(void) {
 	return get_throttle_float() < THROTTLE_LOW_VALUE;
 }
 
-static float _get_speed_from_throttle(float throttle) {
-	float f_throttle = (throttle);
+static float _get_speed_from_throttle(void) {
+	if (motor.b_ignor_throttle) {
+		return motor.s_targetFix;
+	}
+	float f_throttle = motor.throttle;
 	if (f_throttle <= (THROTTLE_LOW_VALUE)) {
 		return 0;
 	}
@@ -142,8 +161,12 @@ static float _get_speed_from_throttle(float throttle) {
 	return (THROTTLE_MIN_RPM + PMSM_FOC_GetSpeedLimit() * ration);
 }
 
-static float _get_idq_from_throttle(float throttle) {
-	float f_throttle = (throttle);
+static float _get_idq_from_throttle(void) {
+	if (motor.b_ignor_throttle) {
+		return motor.s_targetFix;
+	}
+
+	float f_throttle = motor.throttle;
 	if (f_throttle <= (THROTTLE_LOW_VALUE)) {
 		return 0;
 	}
@@ -201,12 +224,11 @@ void ADC_IRQHandler(void) {
 
 //#define ANGLE_TEST
 #ifdef ANGLE_TEST
-static s16 _g_test_angle = 0;
 static void _debug_angle(void) {
-	if (_motor_started) {
-		PMSM_FOC_Set_Angle(_g_test_angle);
-		if (++_g_test_angle >= 360) {
-			_g_test_angle = 0;
+	if (motor.b_start) {
+		PMSM_FOC_Set_Angle(motor.s_testAngle);
+		if (++motor.s_testAngle >= 360) {
+			motor.s_testAngle = 0;
 		}
 	}
 }
@@ -218,13 +240,14 @@ void Sched_MC_mTask(void) {
 	_debug_angle();
 #endif
 	if (runMode != OPEN_MODE) {
-		if (get_throttle_float() != _motor_throttle) {
-			_motor_throttle = get_throttle_float();
+		eCtrl_Running();
+		if (get_throttle_float() != motor.throttle) {
+			motor.throttle = get_throttle_float();
 			if (runMode == SPD_MODE) {
-				float speed_Ref = _get_speed_from_throttle(_motor_throttle);
+				float speed_Ref = _get_speed_from_throttle();
 				PMSM_FOC_Set_Speed(speed_Ref);
 			}else if (runMode == TRQ_MODE) {
-				float torque_idq = _get_idq_from_throttle(_motor_throttle);
+				float torque_idq = _get_idq_from_throttle();
 				PMSM_FOC_Set_Trque(torque_idq);
 			}
 		}		

Applications/foc/motor/motor.h

@@ -3,12 +3,23 @@
 #include "os/os_types.h"
 #include "foc/core/PMSM_FOC_Core.h"
 
+typedef struct {
+	bool   b_start;
+	float  throttle;
+	bool   b_ignor_throttle;
+	s16    s_testAngle;
+	s32    s_targetFix;
+}motor_t;
+
 void mc_init(void);
 bool mc_start(u8 mode);
 bool mc_stop(void);
 void mc_hall_calibrate(s16 vd);
 bool mc_throttle_released(void);
 bool mc_lock_motor(bool lock);
+void mc_set_spd_torque(s32 target);
+void mc_use_throttle(void);
+
 
 #endif /* _MOTOR_H__ */
 

Applications/foc/motor/motor_param.h

@@ -0,0 +1,9 @@
+#ifndef _MOTOR_PARAM_H__
+#define _MOTOR_PARAM_H__
+
+#define MOTOR_R   0.33F
+#define MOTOR_Ld   0.00136F
+#define MOTOR_Lq   0.00136F
+#define MOTOR_POLES     2
+
+#endif /* _MOTOR_PARAM_H__ */

Applications/foc/samples.c

@@ -8,9 +8,9 @@
 #include "bsp/delay.h"
 
 typedef struct {
-	s16q5_t value;
-	s16q5_t filted_value;
-	s16q14_t lowpass;
+	float value;
+	float filted_value;
+	float lowpass;
 }samples_t;
 
 static void sample_vbus(void);
@@ -22,14 +22,14 @@ static samples_t _vbus;
 static samples_t _throttle;
 #endif
 void samples_init(void){
-	_vbus.filted_value = S16Q5(MAX_vDC);
-	_vbus.value = S16Q5(MAX_vDC);
-	_vbus.lowpass = S16Q14(0.2f); 
+	_vbus.filted_value = (MAX_vDC);
+	_vbus.value = (MAX_vDC);
+	_vbus.lowpass = (0.2f); 
 
 #ifndef GD32_FOC_DEMO
 	_throttle.filted_value = (0);
 	_throttle.value = (0);
-	_throttle.lowpass = S16Q14(0.2f); 
+	_throttle.lowpass = (0.2f); 
 
 	sample_throttle();
 #endif
@@ -41,7 +41,7 @@ void samples_init(void){
 
 
 float get_vbus_float(void) {
-	return S16Q5toF(_vbus.filted_value);
+	return (_vbus.filted_value);
 }
 
 s16 get_vbus_sfix5(void){
@@ -61,16 +61,16 @@ u32 sapmple_delta;
 static void sample_vbus(void){
 	u32 ticks = task_ticks_abs();
 	s16 vadc = adc_sample_regular_channel(VBUS_V_CHAN, 3);
-	_vbus.value = S16_mul(vadc, VBUS_VOL_CEOF, 9);
-	LowPass_Filter_Fix(_vbus.filted_value, _vbus.value, _vbus.lowpass, 14);
+	_vbus.value = (float)vadc * VBUS_VOL_CEOF;
+	LowPass_Filter(_vbus.filted_value, _vbus.value, _vbus.lowpass);
 	sapmple_delta = task_ticks_rel(ticks);
 }
 
 static void sample_throttle(void){
 #ifndef GD32_FOC_DEMO	
 	s16 vadc = adc_sample_regular_channel(THROTTLE_CHAN, 16);
-	_throttle.value = S16_mul(vadc, THROTTLE_VOL_CEOF, 9);
-	LowPass_Filter_Fix(_throttle.filted_value, _throttle.value, _throttle.lowpass, 14);
+	_throttle.value = (float)vadc * THROTTLE_VOL_CEOF;
+	LowPass_Filter(_throttle.filted_value, _throttle.value, _throttle.lowpass);
 #endif
 }
 

Applications/math/fast_math.h

@@ -47,7 +47,7 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
  * 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 = (sample - value) * filter_constant + value)
+#define LowPass_Filter(value, sample, filter_constant)	(value = ((float)sample - (float)value) * filter_constant + value)
 
 #endif /* _Fast_Math_H__ */
 

Applications/math/fix_math.h

@@ -26,19 +26,26 @@ typedef int32_t   s32q19_t;
 #define S32Q4(A) (signed int)((A) * 16.0F)
 #define S32Q4toF(A) ((float)(A)/16.0f)
 
+#define S32Q5(A) (signed int)((A) * 32.0F)
+#define S32Q5toF(A) ((float)(A)/32.0f)
+
+
 #define S32Q14(A) (signed int)((A) * 16384.0F)
 #define S32Q14toF(A) ((float)(A)/16384.0f)
 
 #define S32Q19(A) (signed int)((A) * (16384.0F * 32.0F))
 #define S32Q19toF(A) ((float)(A)/(16384.0F * 32.0F))
 
-
 #define S32Q14_MUL(a, b) (((a)*(b)) >>14)
 
 #define S16_mul(a, b, q) (((s32)(a)*(b)) >> q)
 
+#define FtoS16(f) ((s16)f)
+#define FtoS16x1000(f) ((s16)(f * 1000))
 #define LowPass_Filter_Fix(value, sample, filter_constant, shift)	(value = ((s32)value * (S16Q14(1) - filter_constant) + (s32)sample * filter_constant) >> (shift))
 
+#define MIN_S16Q5 (1.0F/32.0F)
+
 /*
 static __INLINE float S16Q4toF(s16q4_t v) {
 	s16 num = (v >> 4) & 0xFFFF;
@@ -91,7 +98,7 @@ static __INLINE float S32Q4toF(s32q4_t v) {
 } 
 */
 
-#define MATH_sat(in, minOut, maxOut) (min((in), MAX((in), (minOut))))
+#define MATH_sat(in, minOut, maxOut) (min((maxOut), MAX((in), (minOut))))
 
 static __INLINE u16 Sqrt_Fix( u32 wInput )
 {
@@ -117,6 +124,6 @@ static __INLINE u16 Sqrt_Fix( u32 wInput )
   	return ( wtemprootnew );
 }
 
-void SinCos_Lut(s16q5_t angle, s16q14_t *s, s16q14_t *c);
+void SinCos_Lut(float angle, float *s, float *c);
 
 #endif /* _FIX_MATH_H__ */

Applications/math/sin_table.c

@@ -49,15 +49,15 @@ static s16 sinTable[] =
   -5334, -5063, -4790, -4516, -4240, -3964, -3686, -3406, -3126, -2845, -2563,
   -2280, -1997, -1713, -1428, -1143, -857, -572, -286, 0 };
 
-void SinCos_Lut(s16q5_t angle, s16q14_t *s, s16q14_t *c) {
-	u16 angle_degree = angle >> 5; //去掉小数部分
-	if ((angle & 0x1F) > 16) {
+void SinCos_Lut(float angle, float *s, float *c) {
+	s16 angle_degree = (s16)angle; //去掉小数部分
+	if ((angle - angle_degree) > 0.5f) {
 		angle_degree += 1;
 	}
 	angle_degree = angle_degree % 360;
-	*s = sinTable[angle_degree];
+	*s = S16Q14toF(sinTable[angle_degree]);
 	//cosx = sin[(pi/2)+x]
 	angle_degree = (angle_degree + 90) % 360;
-	*c = sinTable[angle_degree];
+	*c = S16Q14toF(sinTable[angle_degree]);
 }
 

Project/GD32_DEMO.uvoptx

@@ -120,6 +120,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
+          <Name>*</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -204,6 +205,11 @@
           <WinNumber>1</WinNumber>
           <ItemText>g_cs,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>g_eCtrl,0x0A</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -249,8 +255,8 @@
       <pMultCmdsp></pMultCmdsp>
       <SystemViewers>
         <Entry>
-          <Name>System Viewer\TIMER0</Name>
-          <WinId>35905</WinId>
+          <Name>System Viewer\DMA0</Name>
+          <WinId>35903</WinId>
         </Entry>
       </SystemViewers>
     </TargetOption>