1. hall 速度传感器使用平均来计算rpm

2. 新的SVPWM，保持和仿真的算法一致

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

Application/App/app.c

@@ -8,7 +8,7 @@ void app_init(void){
 void start_stop_handler(void) {
 	FOCState s = FOC_STM_State();
 	if (s == IDLE) {
-		Foc_Set_StartRamp(8.0f, 100);
+		Foc_Set_StartRamp(10.0f, 200);
 		foc_start_motor();
 	}else {
 		foc_stop_motor();

Application/FOC/foc.c

@@ -28,6 +28,8 @@ void foc_init(void) {
 	ntc_sensor_init();
 	
 	task_start(foc_measure_task, 0);
+
+	//hall_sensor_calibrate(2.0f, mFOC.hall_table);
 }
 
 static void foc_defulat_value(void){
@@ -152,6 +154,8 @@ void foc_overide_set_vdq(float d, float q){
 static u32 foc_measure_task(void){
 	vbus_sample_voltage();
 	ntc_sensor_sample();
+	mFOC.vbus = vbus_get_voltage();
+	wdog_reload();
 	return 1;
 }
 

Application/FOC/foc_task.c

@@ -8,6 +8,7 @@
 #include "hall_sensor.h"
 #include "svpwm.h"
 
+#if 1
 static void __inline foc_update_theta(motor_foc_t *foc) {
 	float angle = 0.0f;
 	if (foc->override.is_theta) {
@@ -15,9 +16,26 @@ static void __inline foc_update_theta(motor_foc_t *foc) {
 	}else {
 		angle = hall_sensor_get_theta();
 	}
+	foc->motor_s.angle = angle;
 	foc->motor_s.theta = degree_2_pi(angle);
 }
 
+#else
+static void __inline foc_update_theta(motor_foc_t *foc) {
+	static float angle = 0.0f;
+	static bool first_s = false;
+	if (!first_s) {
+		first_s = true;
+		angle = hall_sensor_get_theta();
+	}else {
+		angle += 1;
+	}
+	fast_norm_angle(&angle);
+	foc->motor_s.angle = angle;
+	foc->motor_s.theta = degree_2_pi(angle);
+}
+#endif
+
 /* 输出dq电压给反park,最后给svpwm */
 static void __inline Foc_Dq_PI_Contrl(motor_foc_t *foc, dq_t *sampled, dq_t *ref_out) {
 	if (foc->mode == FOC_MODE_PI_DQ || foc->mode == FOC_MODE_PI_FULL) {
@@ -34,8 +52,9 @@ static void __inline Foc_Dq_PI_Contrl(motor_foc_t *foc, dq_t *sampled, dq_t *ref
 }
 
 static void __inline DeadTime_Compensation(current_samp_t *c_sample, phase_time_t *time) {
+#if 0	
     /* Dead time compensation */
-    if ( c_sample->ia > 0)
+    if ( c_sample->Ia > 0)
     {
       time->A += TDead;
     }
@@ -44,7 +63,7 @@ static void __inline DeadTime_Compensation(current_samp_t *c_sample, phase_time_
       time->A -= TDead;
     }
 
-    if ( c_sample->ib > 0 )
+    if ( c_sample->Ib > 0 )
     {
       time->B += TDead;
     }
@@ -53,7 +72,7 @@ static void __inline DeadTime_Compensation(current_samp_t *c_sample, phase_time_
       time->B -= TDead;
     }
 
-    if ( c_sample->ic > 0 )
+    if ( c_sample->Ic > 0 )
     {
       time->C += TDead;
     }
@@ -61,7 +80,18 @@ static void __inline DeadTime_Compensation(current_samp_t *c_sample, phase_time_
     {
       time->C -= TDead;
     }
-	
+#endif
+}
+
+static void __inline Debug_Log(motor_foc_t *foc){
+#if 1
+	static int count;
+	if (count++ % 5== 0) {
+		//printf("$%d %d %d %d;", foc->motor_s.angle, (int)(foc->current_samp.Ia * 1000.0f), (int)(foc->current_samp.Ib * 1000.0f),
+		//	(int)(foc->current_samp.Ic * 1000.0f));
+		printf("$%d;", (int)hall_sensor_get_speed());
+	}
+#endif	
 }
 
 /* FOC 主控制逻辑 */
@@ -77,7 +107,7 @@ void FOC_Fast_Task(motor_foc_t *foc){
 	/* 采集3相电流 */
 	phase_current_sample(c_sample);
 	/* ABC坐标转alpha-beta坐标 */
-	Clark(c_sample->ia, c_sample->ib, c_sample->ic, &sample_ab);
+	Clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
 	/* alpha-beta坐标转旋转坐标系D-Q */
 	Park(&sample_ab, foc->motor_s.theta, &sample_dq);
 	/* 处理D,Q电流环，速度环低频运行，不在此处处理*/
@@ -85,13 +115,15 @@ void FOC_Fast_Task(motor_foc_t *foc){
 	/* d-q坐标转 alpha-beta坐标，输入给pwm */
 	Rev_Park(&v_dq, foc->motor_s.theta, &pwm_ab);
 	/* 生成 pwm，模拟正弦波,此处vbus需要动态采集 */
-	svpwm(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
+	SVPWM_7(&pwm_ab, foc->vbus, FOC_PWM_Half_Period, &phase_time, &foc->sector);
 	/* 通过扇区和pwm duty 选择合适的3相电流采样点 */
 	sample_point = get_phase_sample_point(c_sample, &phase_time, foc->sector);
 	/* 死区补偿 */
 	DeadTime_Compensation(c_sample, &phase_time);
 	/* 更新duty和采样点到硬件TIM1中 */
 	PWM_UpdateDuty(phase_time.A, phase_time.B, phase_time.C, sample_point);
+
+	Debug_Log(foc);
 }
 
 //输出dq电流，给电流环

Application/FOC/foc_type.h

@@ -23,7 +23,8 @@ typedef struct _motor_p {
 }motor_param_t;
 
 typedef struct _motor_s {
-	float theta;//ת×Óµç½Ç¶È
+	float theta;//ת×Óµç½Ç¶È, PI
+	int angle;//ת×Óµç½Ç¶È, ¶È
 	float rpm; //תËÙ
 }motor_stat_t;
 
@@ -59,9 +60,9 @@ typedef struct current_sample {
 	u32   adc_offset_a;
 	u32   adc_offset_b;
 	u32   adc_offset_c;
-	float ia;
-	float ib;
-	float ic;
+	float Ia;
+	float Ib;
+	float Ic;
 	u8    sector;
 	u32   adc_inject_flags;
 	volatile int   offset_sample_count;

Application/FOC/hall_sensor.c

@@ -5,7 +5,7 @@
 #include "hall_sensor.h"
 #include "foc/foc.h"
 
-#define HALL_READ_TIMES 7
+#define HALL_READ_TIMES 3
 /* 
 100
 101
@@ -16,7 +16,10 @@
 4,5,1,3,2,6,4
 */
 static u16 _hall_table[] = {0xFFFF, 292/*1*/, 54/*2*/, 1/*3*/, 180/*4*/, 229/*5*/, 115/*6*/, 0xFFFF};
+//static u16 _hall_table[] = {0xFFFF, 292/*1*/, 65/*2*/, 7/*3*/, 185/*4*/, 250/*5*/, 116/*6*/, 0xFFFF};
+
 static hall_t _hall;
+static hall_sample_t h_samples;
 
 #define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
 #define tick_2_s(tick) ((float)tick / (float)SYSTEM_CLOCK)
@@ -29,6 +32,31 @@ static u32 __inline delta_ticks(u32 prev) {
 	return (0xFFFFFFFFU - prev + now) + 1;
 }
 
+static void _hall_put_sample(float angle, u32 ticks) {
+	hall_sample_t *s = &h_samples;
+	s->index += 1;
+	if (s->index >= SAMPLE_MAX_COUNT) {
+		s->full = true;
+		s->index = 0;
+	}
+	s->angle[s->index] = angle;
+	s->ticks[s->index] = ticks;
+}
+
+static float __inline _hall_avg_speed(void){
+	hall_sample_t *s = &h_samples;
+	float t_angle = 0.0f;
+	u32   t_ticks = 0;
+	for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
+		t_angle += s->angle[i];
+		t_ticks += s->ticks[i];
+	}
+	if (t_ticks == 0.0f) {
+		return 0.0f;
+	}
+	return (t_angle / tick_2_s(t_ticks));
+}
+
 void hall_sensor_init(void) {
 	memset(&_hall, 0, sizeof(_hall));
 	read_hall(_hall.state, _hall.theta);
@@ -39,9 +67,14 @@ float hall_sensor_get_theta(void){
 		read_hall(_hall.state, _hall.theta);
 		return _hall.theta;
 	}
-	float est_theta = tick_2_s(delta_ticks(_hall.ticks)) * _hall.degree_per_s + _hall.theta;
-	fast_norm_angle(&est_theta);
-	return est_theta;
+	_hall.est_theta = tick_2_s(delta_ticks(_hall.ticks)) * _hall.degree_per_s + _hall.theta;
+	if (_hall.est_theta > _hall.theta + 60.0f) {
+		_hall.est_theta = _hall.theta + 60.0f;
+	}
+	
+	float angle = _hall.est_theta;
+	fast_norm_angle(&angle);
+	return angle;
 }
 
 float hall_sensor_get_speed(void) {
@@ -50,14 +83,14 @@ float hall_sensor_get_speed(void) {
 
 
 int hall_sensor_calibrate(float current, u16 *hall_table){
-	foc_pwm_start(true);
-
 	foc_overide_set_theta(0.0f);
 	foc_overide_theta(true);
 
 	foc_overide_set_vdq(0.0f, 0.0f);
 	foc_overide_vdq(true);
-		
+	
+	foc_pwm_start(true);
+	HAL_ADC1_InJ_StartConvert();
 	for (int i = 0;i < 1000;i++) {
 		foc_overide_set_vdq((float)i * current / 1000.0f, 0.0f);
 		task_udelay(1000);
@@ -199,12 +232,18 @@ void hall_sensor_handler(void) {
 	if (delta_time == 0.0f) { //may be errors ???
 		return;
 	}
-	_hall.degree_per_s = delta_theta / delta_time;
-	//printf("speed :%.4f - %.4f - %.4f - %d\n", _hall.degree_per_s, delta_theta, delta_time, state_now);
+	delta_theta = 60.0f;
+	_hall_put_sample(delta_theta, delta_ticks(_hall.ticks));
+	if (!h_samples.full) {
+		_hall.degree_per_s = delta_theta / delta_time;
+	}else {
+		_hall.degree_per_s = _hall_avg_speed();
+	}
 	_hall.ticks = task_ticks_abs();
-	_hall.theta = theta_now;
+	_hall.theta += delta_theta;
 	_hall.state = state_now;
 	_hall.e_rpm = _hall.degree_per_s / 360.0f * 60.0f;
+	//printf("speed :%.4f - %.4f - %.4f - %d\n", _hall.degree_per_s, delta_theta, delta_time, (int)_hall.e_rpm);
 }
 
 

Application/FOC/hall_sensor.h

@@ -19,6 +19,7 @@
 typedef struct {
 	bool  alignmnet;
 	float theta;
+	float est_theta;
 	float e_rpm;        //每分钟转速，电转速，不是机械转速
 	u8    state;
 	u32   ticks;
@@ -27,6 +28,15 @@ typedef struct {
 	float degree_per_s; //每秒度， ext: 10度/s
 }hall_t;
 
+#define SAMPLE_MAX_COUNT 12
+
+typedef struct {
+	float angle[SAMPLE_MAX_COUNT];
+	u32   ticks[SAMPLE_MAX_COUNT];
+	u32   index;
+	bool  full;
+}hall_sample_t;
+
 void hall_sensor_init(void);
 float hall_sensor_get_theta(void); //return degree
 float hall_sensor_get_speed(void); //return rpm

Application/FOC/phase_current.c

@@ -47,26 +47,28 @@ void phase_current_sample(current_samp_t *cs){
 	if (cs->sector == SECTOR_4 || cs->sector == SECTOR_5) {
 		/* Current on Phase C is not accessible */
       	/* Ia = PhaseAOffset - ADC converted value) */
-		cs->ia = -adc_to_current((int)phase_current1 - (int)cs->adc_offset_a);
-		cs->ib = -adc_to_current((int)phase_current2 - (int)cs->adc_offset_b);
-		cs->ic = -(cs->ia + cs->ib);
+		cs->Ia = -adc_to_current((int)phase_current1 - (int)cs->adc_offset_a);
+		cs->Ib = -adc_to_current((int)phase_current2 - (int)cs->adc_offset_b);
+		cs->Ic = -(cs->Ia + cs->Ib);
 	}else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_6) {
 		/* Current on Phase A is not accessible 	*/
 		/* Ib = PhaseBOffset - ADC converted value) */
-		cs->ib = -adc_to_current((int)phase_current1 - (int)cs->adc_offset_b);
-		cs->ic = -adc_to_current((int)phase_current2 - (int)cs->adc_offset_c);
-		cs->ia = -(cs->ib + cs->ic);
+		cs->Ib = -adc_to_current((int)phase_current1 - (int)cs->adc_offset_b);
+		cs->Ic = -adc_to_current((int)phase_current2 - (int)cs->adc_offset_c);
+		cs->Ia = -(cs->Ib + cs->Ic);
 	}else if (cs->sector == SECTOR_2 || cs->sector == SECTOR_3) {
 		/* Current on Phase B is not accessible 	*/
 		/* Ia = PhaseAOffset - ADC converted value) */
-		cs->ia = -adc_to_current((int)phase_current1 - (int)cs->adc_offset_a);
-		cs->ic = -adc_to_current((int)phase_current2 - (int)cs->adc_offset_c);
-		cs->ib = -(cs->ia + cs->ic);
+		cs->Ia = -adc_to_current((int)phase_current1 - (int)cs->adc_offset_a);
+		cs->Ic = -adc_to_current((int)phase_current2 - (int)cs->adc_offset_c);
+		cs->Ib = -(cs->Ia + cs->Ic);
 	}
+#if 0
 	static int tet_p = 0;
 	if (tet_p++ % 5 == 0) {
-	printf("$%d %d %d;", (int)(cs->ia * 1000), (int)(cs->ib*1000), (int)(cs->ic*1000));
-		}
+		printf("$%d %d %d;", (int)(cs->Ia * 1000), (int)(cs->Ib*1000), (int)(cs->Ic*1000));
+	}
+#endif	
 }
 
 

Application/FOC/svpwm.c

@@ -2,9 +2,9 @@
 #include "math/fast_math.h"
 
 void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM_half_period, phase_time_t *phase_out, u8 *sector_out){
-	float alpha = alpha_beta->alpha / (2.0f/2.0f * vbus);
-	float beta  = alpha_beta->beta / (2.0f/2.0f * vbus);
-	uint32_t sector;
+	float alpha = alpha_beta->alpha / (3.0f/2.0f * vbus);
+	float beta  = alpha_beta->beta / (3.0f/2.0f * vbus);
+	uint32_t sector = 0xFF;
 
 	if (beta >= 0.0f) {
 		if (alpha >= 0.0f) {
@@ -146,5 +146,354 @@ void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM_half_period, phase
 	phase_out->midle = midle;
 	phase_out->high = high;
 	*sector_out = sector;
+#if 0
+	static int tet_p = 0;
+	if (tet_p++ % 5 == 0) {
+		printf("$%d %d %d;", tA, tB, tC);
+	}
+#endif		
+}
+
+#if 0
+static u8 __inline Calc_N(alpha_beta_t *alpha_beta){
+	float sqr_alpha = alpha_beta->alpha * SQRT3_BY_2;
+	float half_beta = 0.5f * alpha_beta->beta;
+	return (((sqr_alpha - half_beta > 0.0f) << 1) + (alpha_beta->beta > 0.0f)) + ((-(sqr_alpha + half_beta) > 0.0f) << 2);
+}
+
+static void __inline Calc_XYZ(alpha_beta_t *alpha_beta, float vbus, uint32_t PWM_Period, float *XYZ_Out) {
+	float modu = (1.0f / vbus) * (float)PWM_Period;
+	float sqr_beta = SQRT3_BY_2 * alpha_beta->beta;
+	float alpha = 1.5f * alpha_beta->alpha;
+
+	XYZ_Out[0] = SQRT3 * alpha_beta->beta * modu;
+	XYZ_Out[1] = (sqr_beta + alpha) * modu;
+	XYZ_Out[2] = (sqr_beta - alpha) * modu;
+}
+#endif
+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;
+	}
+}
+
+/* 7段式SVPWM 
+ * 返回设置3相PWM的3个CCR寄存器的值
+ * 这里使用的是stm32的PWM mode1,在向上计数时，一旦TIMx_CNT<TIMx_CCR1时通道1为有效电平，否则为无效电平
+ * 在向下计数时，一旦TIMx_CNT>TIMx_CCR1时通道1为无效电平(OC1REF=0)，否则为有效 电平(OC1REF=1)。
+*/
+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 = A_duty;
+			midle = B_duty;
+			high = C_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 = B_duty;
+			midle = A_duty;
+			high = C_duty;			
+			break;
+		case SECTOR_3: // 5
+			A_duty = tc;
+			B_duty = ta;
+			C_duty = tb;
+
+			low = B_duty;
+			midle = C_duty;
+			high = A_duty;			
+			break;
+		case SECTOR_4: // 4
+			A_duty = tc;
+			B_duty = tb;
+			C_duty = ta;
+
+			low = C_duty;
+			midle = B_duty;
+			high = A_duty;			
+			break;
+		case SECTOR_5: // 6
+			A_duty = tb;
+			B_duty = tc;
+			C_duty = ta;
+
+			low = C_duty;
+			midle = A_duty;
+			high = B_duty;			
+			break;			
+		case SECTOR_6: // 2
+			A_duty = ta;
+			B_duty = tc;
+			C_duty = tb;
+
+			low = A_duty;
+			midle = C_duty;
+			high = B_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;
+#if 0
+	static int tet_p = 0;
+	if (tet_p++ % 5 == 0) {
+		printf("$%d %d %d;", A_duty, B_duty, C_duty);
+	}
+#endif		
+//	printf("3sec %d, A:%d, B:%d, C:%d\n", sector, A_duty, B_duty, C_duty);
 }
 
+#if 0
+
+void XYZ_step(void)
+{
+  real_T rtb_Product1;
+  real_T rtb_Product2;
+  real_T rtb_Product3;
+
+  /* Product: '<S1>/Product' incorporates:
+   *  Inport: '<Root>/Ts'
+   *  Inport: '<Root>/Udc'
+   *  Math: '<S1>/Math Function'
+   *
+   * About '<S1>/Math Function':
+   *  Operator: reciprocal
+   */
+  rtb_Product1 = 1.0 / rtU.Udc * rtU.Ts;
+
+  /* Gain: '<S1>/Gain2' incorporates:
+   *  Inport: '<Root>/Ubeta'
+   */
+  rtb_Product2 = 0.8660254037844386 * rtU.Ubeta;
+
+  /* Gain: '<S1>/Gain' incorporates:
+   *  Inport: '<Root>/Ualpha'
+   */
+  rtb_Product3 = 1.5 * rtU.Ualpha;
+
+  /* Outport: '<Root>/XYZ' incorporates:
+   *  Gain: '<S1>/Gain1'
+   *  Inport: '<Root>/Ubeta'
+   *  Product: '<S1>/Product1'
+   *  Product: '<S1>/Product2'
+   *  Product: '<S1>/Product3'
+   *  Sum: '<S1>/Add'
+   *  Sum: '<S1>/Add1'
+   */
+  rtY.XYZ_d[2] = (rtb_Product2 - rtb_Product3) * rtb_Product1;
+  rtY.XYZ_d[1] = (rtb_Product2 + rtb_Product3) * rtb_Product1;
+  rtY.XYZ_d[0] = 1.7320508075688772 * rtU.Ubeta * rtb_Product1;
+}
+
+
+
+
+/* Model step function */
+void T1T2_step(void)
+{
+  real_T rtb_Subtract;
+  real_T rtb_T1;
+  real_T rtb_T2;
+
+  /* MultiPortSwitch: '<S1>/Multiport Switch' incorporates:
+   *  Gain: '<S1>/Gain'
+   *  Gain: '<S1>/Gain1'
+   *  Gain: '<S1>/Gain2'
+   *  Inport: '<Root>/N'
+   *  Inport: '<Root>/XYZ'
+   */
+  switch ((int32_T)rtU.N) {
+   case 1:
+    rtb_T1 = rtU.XYZ[2];
+
+    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
+     *  Inport: '<Root>/XYZ'
+     */
+    rtb_T2 = rtU.XYZ[1];
+    break;
+
+   case 2:
+    rtb_T1 = rtU.XYZ[1];
+
+    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
+     *  Gain: '<S1>/Gain'
+     *  Inport: '<Root>/XYZ'
+     */
+    rtb_T2 = -rtU.XYZ[0];
+    break;
+
+   case 3:
+    rtb_T1 = -rtU.XYZ[2];
+
+    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
+     *  Gain: '<S1>/Gain2'
+     *  Inport: '<Root>/XYZ'
+     */
+    rtb_T2 = rtU.XYZ[0];
+    break;
+
+   case 4:
+    rtb_T1 = -rtU.XYZ[0];
+
+    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
+     *  Gain: '<S1>/Gain'
+     *  Inport: '<Root>/XYZ'
+     */
+    rtb_T2 = rtU.XYZ[2];
+    break;
+
+   case 5:
+    rtb_T1 = rtU.XYZ[0];
+
+    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
+     *  Gain: '<S1>/Gain1'
+     *  Inport: '<Root>/XYZ'
+     */
+    rtb_T2 = -rtU.XYZ[1];
+    break;
+
+   default:
+    rtb_T1 = -rtU.XYZ[1];
+
+    /* MultiPortSwitch: '<S1>/Multiport Switch1' incorporates:
+     *  Gain: '<S1>/Gain1'
+     *  Gain: '<S1>/Gain2'
+     *  Inport: '<Root>/XYZ'
+     */
+    rtb_T2 = -rtU.XYZ[2];
+    break;
+  }
+
+  /* End of MultiPortSwitch: '<S1>/Multiport Switch' */
+
+  /* Sum: '<S1>/Subtract' */
+  rtb_Subtract = rtb_T1 + rtb_T2;
+
+  /* Switch: '<S1>/Switch' incorporates:
+   *  Inport: '<Root>/Tpwm'
+   *  Sum: '<S1>/Subtract1'
+   *  Switch: '<S1>/Switch1'
+   */
+  if (rtU.Tpwm - rtb_Subtract > 0.0) {
+    /* Outport: '<Root>/T1 ' */
+    rtY.T1 = rtb_T1;
+
+    /* Outport: '<Root>/T2' */
+    rtY.T2 = rtb_T2;
+  } else {
+    /* Outport: '<Root>/T1 ' incorporates:
+     *  Product: '<S1>/Divide'
+     *  Product: '<S1>/Product'
+     */
+    rtY.T1 = rtb_T1 * rtU.Tpwm / rtb_Subtract;
+
+    /* Outport: '<Root>/T2' incorporates:
+     *  Product: '<S1>/Divide1'
+     *  Product: '<S1>/Product1'
+     */
+    rtY.T2 = 1.0 / rtb_Subtract * (rtb_T2 * rtU.Tpwm);
+  }
+
+  /* End of Switch: '<S1>/Switch' */
+}
+
+#endif
+

Application/FOC/svpwm.h

@@ -2,7 +2,8 @@
 #define _SVPWM_H__
 #include "foc/foc_type.h"
 
-void svpwm(alpha_beta_t *alpha_beta, float vbus, uint32_t PWW_half_period, phase_time_t *phase_out, u8 *sector_out);
+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);
 
 
 #endif /* _SVPWM_H__ */

Application/Hal/adc.c

@@ -1,3 +1,4 @@
+#include "libs/task.h"
 #include "hal/adc.h"
 
 static ADC_HandleTypeDef hadc1;
@@ -176,7 +177,13 @@ u16 HAL_ADC1_ReadValue(u32 channel)
 
     LL_ADC_REG_StartConversion(hadc1.Instance);
   	/* Wait until end of regular conversion */
-  	while ( LL_ADC_IsActiveFlag_EOC(hadc1.Instance) == 0u ) {}
+	u64 StartTime = get_mseconds();
+  	while ( LL_ADC_IsActiveFlag_EOC(hadc1.Instance) == 0u ) {
+		if (get_mseconds() - StartTime >= 1) {
+			LL_ADC_REG_StartConversion(hadc1.Instance);
+			StartTime = get_mseconds();
+		}
+	}
   	retVal = LL_ADC_REG_ReadConversionData12(hadc1.Instance);
 	return retVal;
 }

Application/Hal/hal.c

@@ -169,7 +169,7 @@ void HAL_NVIC_Init(void){
 	HAL_NVIC_SetPriority(EXTI3_IRQn, 8, 0);
 	HAL_NVIC_SetPriority(EXTI15_10_IRQn, 8, 0);
 	/* USART2_IRQn interrupt configuration */
-	HAL_NVIC_SetPriority(USART2_IRQn, 8, 0);
+	HAL_NVIC_SetPriority(USART2_IRQn, 9, 0);
 }
 
 void HAL_EXIT_Enable(void) {

Application/Hal/pwm.c

@@ -21,7 +21,7 @@ void HAL_PWM_Init(int fs) {
 	htim1.Instance = TIM1;
 	htim1.Init.Prescaler = 0;
 	htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
-	htim1.Init.Period = (TIM_CLOCK / fs /2);
+	htim1.Init.Period = (TIM_CLOCK / fs / 2);
 	htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
 	htim1.Init.RepetitionCounter = 0;
 	htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
@@ -110,7 +110,7 @@ void PWM_TimerEnable(void){
 	/* Prepare timer for synchronization */
 	LL_TIM_GenerateEvent_UPDATE( TIMx );
 
-	LL_TIM_SetCounter( TIMx, 0);
+	LL_TIM_SetCounter( TIMx, FOC_PWM_Half_Period-1);
 
 	LL_TIM_ClearFlag_BRK( TIMx );
 

Application/Math/fast_math.h

@@ -5,6 +5,7 @@
 #define ONE_BY_SQRT3			(0.57735026919f) // 1/sqrt(3)
 #define TWO_BY_SQRT3			(2.0f * 0.57735026919f)
 #define SQRT3_BY_2				(0.86602540378f)
+#define SQRT3                   (1.73205080757f)
 #define M_PI (3.14159265f)
 void fast_sincos(float angle, float *sin, float *cos);
 void fast_norm_angle(float *angle);

Project/Motor_PMSM.uvoptx

@@ -163,32 +163,32 @@
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>m_foc,0x0A</ItemText>
+          <ItemText>hall_iterations,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>hall_iterations,0x0A</ItemText>
+          <ItemText>_vbus,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>_vbus,0x0A</ItemText>
+          <ItemText>_ntc,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>_ntc,0x0A</ItemText>
+          <ItemText>timer_head</ItemText>
         </Ww>
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
-          <ItemText>timer_head</ItemText>
+          <ItemText>mFOC.current_samp</ItemText>
         </Ww>
         <Ww>
           <count>6</count>
           <WinNumber>1</WinNumber>
-          <ItemText>mFOC.current_samp</ItemText>
+          <ItemText>mFOC.hall_table,0x0A</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>

Project/Motor_PMSM.uvprojx

@@ -52,7 +52,7 @@
           <OutputName>Motor_PMSM</OutputName>
           <CreateExecutable>1</CreateExecutable>
           <CreateLib>0</CreateLib>
-          <CreateHexFile>0</CreateHexFile>
+          <CreateHexFile>1</CreateHexFile>
           <DebugInformation>1</DebugInformation>
           <BrowseInformation>1</BrowseInformation>
           <ListingPath>.\Listings\</ListingPath>