1. 手动编写代码，simulink生成的代码效率低一些

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

Applications/app/app.c

@@ -12,47 +12,34 @@
 #include "app/nv_storage.h"
 
 static u32 _app_low_task(void *args);
-static void print_backtrace(void);
+
 extern measure_time_t g_meas_hall;
 extern measure_time_t g_meas_foc;
+
+static void mc_exec_log(void) {
+	sys_debug("intval = %d, exec = %d, count = %d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.exec_count);
+}
+
 void app_start(void){
 	set_log_level(MOD_SYSTEM, L_debug);
 	can_message_init();
 	restore_config();
-	samples_init();
 	mc_init();
 	shark_task_create(_app_low_task, NULL);
-	
-	print_backtrace();
-	
+
+	sys_debug("mc start\n");
 	shark_task_run();
 }
 
 static void _can_report_info(void) {
-	can_report_speed(0x45, PMSM_FOC_GetSpeed());
+	//can_report_speed(0x45, PMSM_FOC_GetSpeed());
 }
 
-static void print_backtrace(void){
-    if (gd32_bkp_btrace_valid()){
-		uint32_t bt[16];
-		uint32_t bt_over;
-		uint32_t bt_dep;
-		uint16_t line;
-		gd32_bkp_get_backtrace(bt, &bt_over, &bt_dep, &line);
-		sys_error("system backtrace:\n");
-		sys_error("stack overflow %d, stack dep = %d, line = %d\n", bt_over, bt_dep, line);
-		for(bt_over = 0; bt_over < bt_dep; bt_over++){
-			sys_error("0x%x ", bt[bt_over]);
-		}
-		sys_error("system backtrace end!\n");
-	}else{
-		sys_error("no backtrace\n");
-	}
-}
 
 static u32 _app_low_task(void *args) {
 	wdog_reload();
 	_can_report_info();
-	return 100;
+	mc_exec_log();
+	return 1000;
 }
 

Applications/bsp/adc.c

@@ -121,6 +121,7 @@ void adc_start_convert(void) {
 #else
 	adc_interrupt_enable(ADC1, ADC_INT_EOIC);
 #endif
+	adc_update_ext_trigger(ADC_TRIGGER_PHASE);
 
 }
 

Applications/bsp/bsp.h

@@ -24,7 +24,7 @@
 #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 (16000u)
+#define FOC_PWM_FS (20000u)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 

Applications/bsp/can.c

@@ -38,7 +38,7 @@ void can_rx_poll(void){
 	return ;
 }
 
-static void can_tx_poll(void){
+void can_tx_poll(void){
 	can_trasnmit_message_struct can_tr_m;
 	while (can_get_mailbox(CAN0) != CAN_NOMAILBOX) {
 		if (circle_get_data(&g_tx_circle, (uint8_t * )&can_tr_m, sizeof(can_tr_m)) != sizeof(can_tr_m)) {

Applications/bsp/mc_hall_gpio.c

@@ -43,9 +43,9 @@ int get_hall_stat(int samples) {
 
 
 u32 hall_get_hwcount(u8 *hall) {
-	hall[0] = READ_HALL1();
-	hall[1] = READ_HALL2();
-	hall[2] = READ_HALL3();
+	hall[0] = 1;//READ_HALL1();
+	hall[1] = 0;//READ_HALL2();
+	hall[2] = 1;//READ_HALL3();
 	if (g_hall.is_edged) {
 		g_hall.is_edged = 0;
 		return g_hall.hall_time;

Applications/bsp/mc_irqs.c

@@ -73,17 +73,19 @@ void DebugMon_Handler(void)
 {
 }
 
-extern void mc_phase_current_handler(void);
 extern void hall_sensor_handler(void);
 extern void mc_brake_handler(void);
 extern void mc_pwm_up_handler(void);
 
+__weak void ADC_IRQHandler(void) {
+
+}
+
 
 void ADC0_1_IRQHandler(void)
 {
 	adc_disable_ext_trigger();
-	mc_phase_current_handler();
-	//adc_enable_ext_trigger();
+	ADC_IRQHandler();
     /* clear the ADC flag */
 	adc_clear_irq_flags();	
 }

Applications/bsp/pwm.c

@@ -345,12 +345,11 @@ void pwm_start(void){
 }
 
 void pwm_stop(void){
-	dma_channel_disable(DMA0, DMA_CH4);
 	timer_primary_output_config(pwm_timer,DISABLE);
 #ifdef ENABLE_AUX_TIMER	
 	timer_primary_output_config(adc_timer,DISABLE);
 #endif
-	//timer_interrupt_disable(pwm_timer, TIMER_INT_UP);
+	timer_interrupt_disable(pwm_timer, TIMER_INT_UP);
 	/* wait for a new PWM period to flush last HF task */
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
 	//while ( timer_flag_get(pwm_timer, TIMER_FLAG_UP) == RESET ){}

Applications/bsp/timer_count32.c

@@ -4,11 +4,11 @@
 static void init_master_timer(void);
 static void init_slave_timer(void);
 
-#define MASTER TIMER1
-#define MASTER_CK RCU_TIMER1
+#define MASTER TIMER3
+#define MASTER_CK RCU_TIMER3
 
-#define SLAVE  TIMER2
-#define SLAVE_CK RCU_TIMER2
+#define SLAVE  TIMER4
+#define SLAVE_CK RCU_TIMER4
 
 void timer_count32_init(void) {
 

Applications/foc/commands.c

@@ -59,7 +59,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(OPEN_MODE);
 			}else if (scmd->start_stop == Foc_Stop) {
 				success = mc_stop();
 			}

Applications/foc/core/PI_Controller.h

@@ -0,0 +1,24 @@
+#ifndef _PI_Contrller_H__
+#define _PI_Contrller_H__
+#include "math/fix_math.h"
+typedef struct {
+	s32q14_t  kp;
+	s32q14_t  ki;
+	s32q14_t  max;
+	s32q14_t  min;
+	s32q14_t  Ui;
+}PI_Controller;
+
+static __INLINE void PI_Controller_Reset(PI_Controller *pi, s32q14_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);
+	s32q14_t out = pi->Ui + kp_err;
+	return MATH_sat(out, pi->min, pi->max) >> 9;
+}
+
+#endif	/*_PI_Contrller_H__*/
+

Applications/foc/core/PMSM_FOC_Params.c

@@ -0,0 +1,32 @@
+#include "foc/core/PI_Controller.h"
+
+#define cf_iBand  (2.0f * PI * FOC_PWM_FS / 20.0f * 3.0f);
+#define Ld 0.0009262f 
+#define Lq 0.001024f
+#define Rs 0.0918f
+
+PI_Controller PI_Ctrl_ID = {
+	.kp = S32Q14(0.001f),
+	.ki = S32Q14(0.003f),
+	.max = S32Q14(MAX_iDQ),
+	.min = S32Q14(-MAX_iDQ),
+	.Ui = 0,
+};
+
+PI_Controller PI_Ctrl_IQ = {
+	.kp = S32Q14(0.001f),
+	.ki = S32Q14(0.003f),
+	.max = S32Q14(MAX_iDQ),
+	.min = S32Q14(-MAX_iDQ),
+	.Ui = 0,
+};
+
+PI_Controller PI_Ctrl_Spd = {
+	.kp = S32Q14(0.001f),
+	.ki = S32Q14(0.003f),
+	.max = S32Q14(Default_Spd_Limit),
+	.min = S32Q14(0),
+	.Ui = 0,
+};
+
+

Applications/foc/core/foc_core.c

@@ -9,7 +9,7 @@
 #include "bsp/timer_count32.h"
 #include "libs/time_measure.h"
 #include "libs/logger.h"
-
+#include "foc/core/PI_Controller.h"
 pmsm_foc_t pmsm_foc = {0};
 
 extern void PMSM_FOC_Init(void);
@@ -17,6 +17,7 @@ extern ExtU *PMSM_FOC_GetInputs(void);
 extern ExtY *PMSM_FOC_GetOutputs(void);
 extern void PMSM_FOC_Step(void);
 extern P *PMSM_FOC_GetParams(void);
+PI_Controller pi;
 
 void PMSM_FOC_CoreInit(void) {
 	mc_hall_init();
@@ -30,7 +31,7 @@ void PMSM_FOC_CoreInit(void) {
 	pmsm_foc.FOC_P->id_fieldWeakMax = S16Q5(-50);
 	pmsm_foc.FOC_P->i_dqMax = S16Q5(150);
 	pmsm_foc.FOC_P->V_modulation = S16Q14(0.95f);
-	
+	pmsm_foc.FOC_Out->n_Sector = 1;
 	PMSM_FOC_iBusLimit(S16Q5(Default_iDC_Limit));
 	PMSM_FOC_SpeedLimit(S32Q4(Default_Spd_Limit));
 }
@@ -40,17 +41,17 @@ static __INLINE void PMSM_FOC_PWMCurrent_Update(void) {
 		pwm_update_duty(0, 0, 0);
 		pwm_update_sample(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period, 1);
 	}else {
-		current_samp_t *cs = get_phase_sample_point(pmsm_foc.FOC_Out->n_Sector);
+		current_samp_t *cs = get_phase_sample_point(pmsm_foc.FOC_Out->n_Sector-1);
 	
 		pwm_update_duty(pmsm_foc.FOC_Out->n_Duty[0], pmsm_foc.FOC_Out->n_Duty[1], pmsm_foc.FOC_Out->n_Duty[2]);
 	
-		pwm_update_sample(cs->time.Samp_p1, cs->time.Samp_p2, cs->sector);
+		//pwm_update_sample(cs->time.Samp_p1, cs->time.Samp_p2, cs->sector);
 	}
 }
 
 
 void PMSM_FOC_Schedule(void) {
-	pwm_clear_updata();
+
 	phase_current_sample(&pmsm_foc.FOC_In->adc_Phase[0], &pmsm_foc.FOC_In->adc_Phase[1], &pmsm_foc.FOC_In->adc_Phase[2]);
 	pmsm_foc.FOC_In->sys_ticks = hall_get_hwcount(pmsm_foc.FOC_In->hall_abc);
 
@@ -61,9 +62,12 @@ void PMSM_FOC_Schedule(void) {
 		pmsm_foc.FOC_In->spd_Target = S32Q4(eCtrl_get_FinalSpd());
 		pmsm_foc.FOC_In->idq_Target = S16Q5(eCtrl_get_RefTorque());
 	}
+	PI_Controller_run(&pi, 111);
 	
-	PMSM_FOC_Step();
+	//PMSM_FOC_Step();
 
+	pmsm_foc.FOC_Out->n_Duty[0] = pmsm_foc.FOC_Out->n_Duty[1] = pmsm_foc.FOC_Out->n_Duty[2] = FOC_PWM_Half_Period;
+	
 	PMSM_FOC_PWMCurrent_Update();
 }
 
@@ -93,11 +97,11 @@ bool PMSM_FOC_Is_Start(void) {
 	return pmsm_foc.FOC_In->b_motEna;
 }
 
-void PMSM_FOC_iBusLimit(_s16q5_t ibusLimit) {
+void PMSM_FOC_iBusLimit(s16q5_t ibusLimit) {
 	pmsm_foc.FOC_In->iDC_Limit = (ibusLimit);
 }
 
-void PMSM_FOC_SpeedLimit(_s32q4_t speedLimit) {
+void PMSM_FOC_SpeedLimit(s32q4_t speedLimit) {
 	pmsm_foc.FOC_In->spd_Limit = (speedLimit);
 }
 
@@ -105,7 +109,7 @@ float PMSM_FOC_GetSpeedLimit(void) {
 	return S32Q4toF(pmsm_foc.FOC_In->spd_Limit);
 }
 
-void PMSM_FOC_VbusVoltage(_s16q5_t vbusVol) {
+void PMSM_FOC_VbusVoltage(s16q5_t vbusVol) {
 	pmsm_foc.FOC_In->vDC = (vbusVol);
 }
 
@@ -113,7 +117,7 @@ void PMSM_FOC_SetCtrlMode(uint8_T mode) {
 	pmsm_foc.FOC_In->n_ctrlMod = mode;
 }
 
-void PMSM_FOC_SetOpenVdq(_s16q5_t vd, _s16q5_t vq) {
+void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq) {
 	pmsm_foc.FOC_In->vdq_Target[0] = (vd);
 	pmsm_foc.FOC_In->vdq_Target[1] = (vq);
 }

Applications/foc/core/foc_core.h

@@ -1,22 +1,55 @@
 #ifndef _FOC_CORE_H__
 #define _FOC_CORE_H__
-#include "foc_type.h"
+#include "math/fix_math.h"
+
+typedef struct {
+	u8 		n_poles;
+	s16q5_t s_maxIdq;
+	s16q5_t s_maxiDC;
+	s16     s_maxRPM;
+	s16q5_t s_iABC[3];
+	s16     s_motRPM;   //from hall or encoder
+	s16q5_t s_motAngle; //from hall or encoder
+	s16     s_targetRPM;
+	s16q5_t s_targetIdq;
+	s16q5_t s_targetVdq;
+	u8      n_ctlMode;
+	bool    b_motEnable;
+	s16q5_t s_manualAngle; //mainly used when calibrate hall
+}FOC_InP;
+
+typedef struct {
+	u16 n_Duty[3];
+	u8  n_Sector;
+	u8  n_RunMode;
+	u8  s_RealVdq;
+	u8  s_RealIdq;
+}FOC_OutP;
+
+typedef struct {
+	PI_Controller id_ctl;
+	PI_Controller iq_ctl;
+	PI_Controller spd_ctl;
+	PI_Controller fw_crl;
+	FOC_InP       in;
+	FOC_OutP      out;
+}PMSM_FOC_Ctrl;
 
 void PMSM_FOC_CoreInit(void);
 void PMSM_FOC_Schedule(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(s16q5_t ibusLimit);
+void PMSM_FOC_SpeedLimit(s32q4_t speedLimit);
 float PMSM_FOC_GetSpeedLimit(void);
-void PMSM_FOC_VbusVoltage(_s16q5_t vbusVol);
+void PMSM_FOC_VbusVoltage(s16q5_t vbusVol);
 void PMSM_FOC_SetCtrlMode(uint8_T mode);
-void PMSM_FOC_SetOpenVdq(_s16q5_t vd, _s16q5_t vq);
+void PMSM_FOC_SetOpenVdq(s16q5_t vd, s16q5_t vq);
 bool PMSM_FOC_EnableCruise(boolean_T enable);
 bool PMSM_FOC_Set_Speed(float rpm);
 bool PMSM_FOC_Set_Current(float current);
 bool PMSM_FOC_Set_CruiseSpeed(float rpm);
-void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, _s16q5_t open_vd);
+void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, s16q5_t open_vd);
 float PMSM_FOC_GetSpeed(void);
 void PMSM_FOC_SetErrCode(u8 code);
 u8 PMSM_FOC_GetErrCode(void);

Applications/foc/core/foc_type.h

@@ -105,6 +105,7 @@ typedef struct _foc {
 #define SECTOR_4  3u
 #define SECTOR_5  4u
 #define SECTOR_6  5u
+#define SECTOR_UKNOW 0xFF
 #else
 #define SECTOR_1  3u
 #define SECTOR_2  4u

Applications/foc/core/svpwm.c

@@ -0,0 +1,378 @@
+#include "foc/svpwm.h"
+#include "math/fast_math.h"
+
+
+static void __inline ModuleTime(u32 *T4, u32 *T6, u32 PWM_Period) {
+	u32 period = PWM_Period * 95 / 100; //95%�ĵ���
+	if (*T4 + *T6 > period){
+		float ration = ((float)period)/((float)*T4 + (float)*T6);
+		*T4 *= ration;
+		*T6 *= ration;
+	}
+}
+void SVPWM_ST(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out){
+	u32 PWM_Period = PWM_half_period * 2;
+	float wAlpha = SQRT3 * alpha_beta->alpha * 2.0f;
+	float wBeta = -alpha_beta->beta * 2.0f;
+	float X = wBeta * PWM_Period/vbus;
+	float Y = (wBeta + wAlpha)*PWM_Period/vbus/2.0f;
+	float Z = (wBeta - wAlpha)*PWM_Period/vbus/2.0f;	
+	s32 tA, tB, tC;
+	s32 low, midle, high;
+	if (Y < 0) {
+    	if (Z < 0) {
+        	*sector_out = 5;
+        	tA = PWM_Period/4 + (Y - Z)/4;
+        	tB = tA + Z/2;
+        	tC = tA - Y/2;
+      		low = tC;
+      		midle = tA;
+      		high = tB;			
+    	}else {
+        	if (X <= 0 ) {
+            	*sector_out = 4;
+            	tA = PWM_Period/4 + (X - Z)/4;
+            	tB = tA + Z/2;
+            	tC = tB - X/2;
+				low = tC;
+      			midle = tB;
+      			high = tA;
+        	}else {
+            	*sector_out = 3;
+            	tA = PWM_Period/4 + (Y - X)/4;
+            	tC = tA - Y/2;
+            	tB = tC + X/2;
+      			low = tB;
+      			midle = tC;
+      			high = tA;				
+        	}
+    	}
+	}else {
+    	if (Z >= 0) {
+        	*sector_out = 2;
+        	tA = PWM_Period/4 + (Y - Z)/4;
+        	tB= tA + Z/2;
+        	tC = tA - Y/2;
+      		low = tB;
+      		midle = tC;
+      		high = tA;			
+    	}else {
+        	if (X <= 0 ) {
+            	*sector_out = 6;
+            	tA = PWM_Period/4 + (Y - X)/4;
+            	tC = tA - Y/2;    
+            	tB = tC + X/2;
+	      		low = tA;
+      			midle = tC;
+      			high = tB;			
+        	} else {
+            	*sector_out = 1;
+            	tA = PWM_Period/4 + (X - Z)/4;
+            	tB = tA + Z/2;
+            	tC = tB - X/2;
+	      		low = tA;
+      			midle = tB;
+      			high = tC;
+        	}
+    	}
+	}
+	phase_out->A = ( uint16_t )tA;
+	phase_out->B = ( uint16_t )tB;
+	phase_out->C = ( uint16_t )tC;
+	phase_out->low = low;
+	phase_out->midle = midle;
+	phase_out->high = high;	
+}
+
+void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out) {
+	float alpha = alpha_beta->alpha * 2.0f / 3.0f;
+	float beta  = alpha_beta->beta  * 2.0f / 3.0f;
+	u8 sector = 0xFF;
+	u32 A_duty, B_duty, C_duty;
+	u32 low, midle, high;
+	u32 T1, T2;
+	float X, Y, Z;
+	float modu = (float)(PWM_half_period) / vbus;
+
+	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
+			T1 = -Z;
+			T2 = X;
+			break;		
+		case SECTOR_2: // 1
+			T1 = Z;
+			T2 = Y;			
+			break;
+		case SECTOR_3: // 5
+			T1 = X;
+			T2 = -Y;		
+			break;
+		case SECTOR_4: // 4
+			T1 = -X;
+			T2 = Z;		
+			break;
+		case SECTOR_5: // 6
+			T1 = -Y;
+			T2 = -Z;		
+			break;			
+		case SECTOR_6: // 2
+			T1 = Y;
+			T2 = -X;		
+			break;
+		default:
+			break;
+	}
+	ModuleTime(&T1, &T2, PWM_half_period);
+  /*	
+	u32 ta = (PWM_half_period - T1 - T2) / 2;
+	u32 tb = ta + T1 ;
+	u32 tc = tb + T2 ; */
+	switch(sector) {
+		case SECTOR_1: // 3
+			A_duty = (PWM_half_period - T1 - T2) / 2;
+			B_duty = A_duty + T1;
+			C_duty = B_duty + T2;
+
+			low = C_duty;
+			midle = B_duty;
+			high = A_duty;
+			break;		
+		case SECTOR_2: // 1
+			B_duty = (PWM_half_period - T1 - T2) / 2;
+			A_duty = B_duty + T1;
+			C_duty = A_duty + T2;
+
+			low = C_duty;
+			midle = A_duty;
+			high = B_duty;			
+			break;
+		case SECTOR_3: // 5
+			B_duty = (PWM_half_period - T1 - T2) / 2;
+			C_duty = B_duty + T1;
+			A_duty = C_duty + T2;
+
+			low = A_duty;
+			midle = C_duty;
+			high = B_duty;			
+			break;
+		case SECTOR_4: // 4
+			C_duty = (PWM_half_period - T1 - T2) / 2;
+			B_duty = C_duty + T1;
+			A_duty = B_duty + T2;
+
+			low = A_duty;
+			midle = B_duty;
+			high = C_duty;			
+			break;
+		case SECTOR_5: // 6
+			C_duty = (PWM_half_period - T1 - T2) / 2;
+			A_duty = C_duty + T1;
+			B_duty = A_duty + T2;
+
+			low = B_duty;
+			midle = A_duty;
+			high = C_duty;			
+			break;			
+		case SECTOR_6: // 2
+			A_duty = (PWM_half_period - T1 - T2) / 2;
+			C_duty = A_duty + T1;
+			B_duty = C_duty + T2;
+
+			low = B_duty;
+			midle = C_duty;
+			high = A_duty;			
+			break;
+		default:
+			break;
+	}
+	
+	phase_out->A = A_duty;
+	phase_out->B = B_duty;
+	phase_out->C = C_duty;
+	phase_out->low = low;
+	phase_out->midle = midle;
+	phase_out->high = high;
+	*sector_out = sector;
+
+//	printf("3sec %d, A:%d, B:%d, C:%d\n", sector, A_duty, B_duty, C_duty);
+}
+
+/* 7段式SVPWM
+ * 返回设置3相PWM的3个CCR寄存器的值
+ * 这里使用的是stm32的PWM mode1,在向上计数时，一旦TIMx_CNT<TIMx_CCR1时通道1为有效电平，否则为无效电平
+ * 在向下计数时，一旦TIMx_CNT>TIMx_CCR1时通道1为无效电平(OC1REF=0)，否则为有效 电平(OC1REF=1)。
+ * 整个时间的计算，前面X,Y,Z都是一样的，后面计算ABC三相的pwm CCR寄存器值的时候，需要注意，很多网络包括书本的资料都是用PWM2模式的
+   就是高电平的时间 pwm_period - ccr,我们用PWM1模式,所以最后abc的计算稍微有些不一样
+*/
+
+void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out) {
+	float alpha = alpha_beta->alpha * SQRT3_BY_2;
+	float beta  = alpha_beta->beta  * SQRT3_BY_2;
+	u32   PWM_Period = PWM_half_period * 2;
+	u8 sector = 0xFF;
+	u32 tA, tB, tC;
+	u32 low, midle, high;
+	float X, Y, Z;
+	float modu = (float)(PWM_Period) / vbus;
+
+	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 = TWO_BY_SQRT3 * beta * modu;
+	Y = (alpha + ONE_BY_SQRT3 * 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;
+			high = tC;
+			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;
+			high = tC;			
+			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;
+			high = tA;			
+			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;
+			high = tA;			
+			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;
+			high = tB;			
+			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;
+			high = tB;			
+			break;
+		}
+		default:
+			break;
+	}	
+	phase_out->A = tA;
+	phase_out->B = tB;
+	phase_out->C = tC;
+	phase_out->low = low;
+	phase_out->midle = midle;
+	phase_out->high = high;
+	*sector_out = sector;
+}
+
+
+

Applications/foc/core/svpwm.h

@@ -0,0 +1,10 @@
+#ifndef _SVPWM_H__
+#define _SVPWM_H__
+#include "foc/foc_type.h"
+
+void svpwm(alpha_beta_t *alpha_beta, float vbus, u32 PWW_half_period, phase_time_t *phase_out, u8 *sector_out);
+void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
+void SVPWM_ST(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
+void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
+#endif /* _SVPWM_H__ */
+

Applications/foc/motor/current.c

@@ -29,7 +29,7 @@ void phase_current_init(void) {
 	cs->adc_ic = 0;
 }
 
-void phase_current_calibrate(void){
+void phase_current_start_cali(void){
 	g_cs.adc_offset_a = 0;
 	g_cs.adc_offset_b = 0;
 	g_cs.adc_offset_c = 0;
@@ -38,21 +38,6 @@ void phase_current_calibrate(void){
 	g_cs.is_calibrating_offset = true;
 	g_cs.sector = SECTOR_5;
 	adc_current_sample_config(g_cs.sector);
-	pwm_start();
-	adc_start_convert();
-	while(g_cs.offset_sample_count != 0){};
-
-	adc_stop_convert();
-	pwm_stop();
-	task_udelay(100);
-	phase_current_init();	
-	g_cs.sector = SECTOR_1;
-	adc_current_sample_config(g_cs.sector);
-	pwm_start();
-	adc_start_convert();
-	while(g_cs.offset_sample_count != 0){};
-
-	g_cs.is_calibrating_offset = false;
 }
 
 
@@ -83,6 +68,16 @@ bool phase_current_offset(void) {
 				cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
 			}
 		}
+	}else {
+		if (cs->sector == SECTOR_5) {
+			cs->sector = SECTOR_1;
+			phase_current_init();
+			adc_current_sample_config(cs->sector);
+		}else {
+			cs->is_calibrating_offset = false;
+			sys_debug("offset %d, %d, %d\n", g_cs.adc_offset_a, g_cs.adc_offset_b, g_cs.adc_offset_c);
+		}
+
 	}
 	return true;
 }

Applications/foc/motor/current.h

@@ -36,6 +36,6 @@ void phase_current_sample(s16 *ia, s16 *ib, s16 *ic);
 bool phase_current_offset(void);
 current_samp_t * get_phase_sample_point(u8 sector);
 void phase_current_adc_triger(void);
-void phase_current_calibrate(void);
+void phase_current_start_cali(void);
 #endif /* _PHASE_CURRENT_H__ */
 

Applications/foc/motor/motor.c

@@ -11,12 +11,18 @@
 #include "bsp/bsp.h"
 #include "bsp/adc.h"
 #include "bsp/pwm.h"
+#include "foc/commands.h"
+#include "libs/logger.h"
 
 static bool _motor_started = false;
 static float _motor_throttle = 0;
 static u32 mc_main_task_handler(void *param);
 
 void mc_init(void) {
+	adc_init();
+	pwm_3phase_init();
+	samples_init();
+	foc_command_init();
 	PMSM_FOC_CoreInit();
 	shark_task_create(mc_main_task_handler, NULL);
 }
@@ -39,10 +45,10 @@ bool mc_start(u8 mode) {
 	}
 	pwm_turn_on_low_side();
 	task_udelay(500);
-	phase_current_calibrate();
-	PMSM_FOC_Start(mode);
+	phase_current_start_cali();
 	pwm_start();
 	adc_start_convert();
+	PMSM_FOC_Start(mode);
 	_motor_throttle = 0;
 	_motor_started = true;
 	return true;
@@ -73,7 +79,6 @@ void mc_hall_calibrate(s16 vd) {
 	}
 	pwm_turn_on_low_side();
 	task_udelay(500);
-	phase_current_calibrate();
 	PMSM_FOC_Start(OPEN_MODE);
 	pwm_start();
 	adc_start_convert();
@@ -175,7 +180,7 @@ measure_time_t g_meas_foc = {.exec_max_time = 15,};
 #define TIME_MEATURE_END() time_measure_end(&g_meas_foc)
 
 /*ADC 电流采集中断，调用FOC的核心处理函数*/
-void mc_phase_current_handler(void) {
+void ADC_IRQHandler(void) {
 	if (phase_current_offset()) {//check if is adc offset checked
 		return;
 	}

Applications/foc/samples.c

@@ -47,7 +47,7 @@ s16 get_vbus_sfix5(void){
 
 
 float get_throttle_float(void) {
-	return (float)((s32)(_throttle.filted_value * 100.0f)/100.0f);
+	return 0.0f;//(float)((s32)(_throttle.filted_value * 100.0f)/100.0f);
 }
 static u32 sample_task(void *param) {
 	sample_vbus();

Applications/math/fix_math.h

@@ -2,47 +2,52 @@
 #define _FIX_MATH_H__
 #include "bsp/bsp.h"
 #include "os/os_types.h"
-typedef signed short _s16q14_t;
-typedef signed short _s16q10_t;
-typedef signed short _s16q5_t;
-typedef signed short _s16q4_t;
-typedef signed int   _s32q4_t;
+#include "libs/utils.h"
+typedef signed short s16q14_t;
+typedef signed short s16q10_t;
+typedef signed short s16q5_t;
+typedef signed short s16q4_t;
+typedef signed int   s32q4_t;
+typedef signed int   s32q14_t;
+
 
 #define S16Q4(A) (signed short)((A) * 16.0F)
 #define S16Q5(A) (signed short)((A) * 32.0F)
 #define S16Q10(A) (signed short)((A) * 1024.0F)
 #define S16Q14(A) (signed short)((A) * 16384.0F)
 #define S32Q4(A) (signed int)((A) * 16.0F)
+#define S32Q14(A) (signed int)((A) * 16384.0F)
+#define S32Q14_MUL(a, b) (((a)*(b)) >>14)
 
 
-
-static __INLINE float S16Q4toF(_s16q4_t v) {
+static __INLINE float S16Q4toF(s16q4_t v) {
 	s16 num = (v >> 4) & 0xFFFF;
 	u16 tail = v & 0x000F;
 	float f = num + (float)tail / 16.0f;
 	return f;
 } 
 
-static __INLINE float S16Q5toF(_s16q5_t v) {
+static __INLINE float S16Q5toF(s16q5_t v) {
 	s16 num = (v >> 5) & 0xFFFF;
 	u16 tail = v & 0x001F;
 	float f = num + (float)tail / 32.0f;
 	return f;
 } 
 
-static __INLINE float S16Q10toF(_s16q5_t v) {
+static __INLINE float S16Q10toF(s16q5_t v) {
 	s16 num = (v >> 10) & 0xFFFF;
 	u16 tail = v & 0x03FF;
 	float f = num + (float)tail / 1024.0f;
 	return f;
 }
 
-static __INLINE float S32Q4toF(_s32q4_t v) {
+static __INLINE float S32Q4toF(s32q4_t v) {
 	s32 num = (v >> 4) & 0xFFFFFFFF;
 	u16 tail = v & 0x000F;
 	float f = num + (float)tail / 16.0f;
 	return f;
 } 
 
+#define MATH_sat(in, minOut, maxOut) (min((in), MAX((in), (minOut))))
 
 #endif /* _FIX_MATH_H__ */

Project/MC100.uvprojx

@@ -83,7 +83,7 @@
             <RunUserProg1>1</RunUserProg1>
             <RunUserProg2>1</RunUserProg2>
             <UserProg1Name>fromelf --bin --output ./Output/MC100.bin ./Objects/MC100.axf</UserProg1Name>
-            <UserProg2Name>SharkFwVersion mv  ./Output/MC100.bin</UserProg2Name>
+            <UserProg2Name>SharkFwVersion copy  ./Output/MC100.bin</UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
             <nStopA1X>0</nStopA1X>
@@ -275,7 +275,7 @@
               </OCR_RVCT3>
               <OCR_RVCT4>
                 <Type>1</Type>
-                <StartAddress>0x8000000</StartAddress>
+                <StartAddress>0x8002000</StartAddress>
                 <Size>0x40000</Size>
               </OCR_RVCT4>
               <OCR_RVCT5>
@@ -337,7 +337,7 @@
             <v6Rtti>0</v6Rtti>
             <VariousControls>
               <MiscControls>--gnu</MiscControls>
-              <Define>USE_STDPERIPH_DRIVER,GD32F30X_HD,BACK_TRACE</Define>
+              <Define>USE_STDPERIPH_DRIVER,GD32F30X_HD,BACK_TRACE,CONFIG_CAN_IAP</Define>
               <Undefine></Undefine>
               <IncludePath>..\Librarys\CMSIS\Include,..\Librarys\CMSIS\GD\GD32F30x\Include,..\Librarys\GD32F30x_Drivers\include,..\Applications;..\Simulink\PMSM_Controller_ert_rtw</IncludePath>
             </VariousControls>