重新计算相电流的采样方式

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

Applications/bsp/adc.h

@@ -38,10 +38,16 @@ inserted ADC 由timer0 ch3触发，
 #define ADC_TRIGGER_NONE  ADC0_1_2_EXTTRIG_INSERTED_NONE
 #define ADC_TRIGGER_VBUS ADC0_1_EXTTRIG_INSERTED_T1_CH0
 
+
+#define PHASE_AB 0
+#define PHASE_AC 1
+#define PHASE_BC 2
+
 //#define ADC_RANK_CHANNEL(c1, c2, l) ((c1)<<ISQ2_OFFSET | (c2)<<ISO3_OFFSET | (l)<<IL_OFFSET)
 #define ADC_RANK_CHANNEL(c)  ((c)<<ISO3_OFFSET | (0)<<IL_OFFSET) 
 #define ADC_CALI_RANK_CHANEL(c)  ((c)<<ISO3_OFFSET | (0)<<IL_OFFSET) 
 
+#if 0
 static u32 adc0_rank_channels[6] = {
 	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),//1, B, BC
 	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),//2, A, AC 
@@ -89,45 +95,51 @@ static void __inline adc_phase_current_read(u8 sector, s32 *v1, s32 *v2) {
 
 static void __inline adc_current_sample_config(u8 sector) {
 #if SHUNT_NUM==THREE_SHUNTS_SAMPLE
-#if 1
 	ADC_ISQ(ADC0) = adc0_rank_channels[sector];
 	ADC_ISQ(ADC1) = adc1_rank_channels[sector];
+#endif	
+}
 #else
-	u32 chan1, chan2;
-	switch (sector) {
-		case 0:
-			chan1 = V_PHASE_I_CHAN;
-			chan2 = W_PHASE_I_CHAN;
-			break;
-		case 1:
-			chan1 = U_PHASE_I_CHAN;
-			chan2 = W_PHASE_I_CHAN;
-			break;
-		case 2:
-			chan1 = W_PHASE_I_CHAN;
-			chan2 = U_PHASE_I_CHAN;
-			break;
-		case 3:
-			chan1 = V_PHASE_I_CHAN;
-			chan2 = U_PHASE_I_CHAN;
-			break;
-		case 4:
-			chan1 = U_PHASE_I_CHAN;
-			chan2 = V_PHASE_I_CHAN;
-			break;
-		case 5:
-			chan1 = W_PHASE_I_CHAN;
-			chan2 = V_PHASE_I_CHAN;
-			break;
-		default:
-			return;
-	}
-	adc_inserted_channel_config(ADC0, 0, chan1, ADC_SAMPLE_TIME);
-	adc_inserted_channel_config(ADC1, 0, chan2, ADC_SAMPLE_TIME);	
+static u32 adc0_rank_channels[3] = {
+	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),//0, A, AB
+	ADC_RANK_CHANNEL(U_PHASE_I_CHAN),//1, A, AC
+	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),//2, B, BC
+};
+static u32 adc1_rank_channels[3] = {
+	ADC_RANK_CHANNEL(V_PHASE_I_CHAN),//0, B
+	ADC_RANK_CHANNEL(W_PHASE_I_CHAN),//1, C
+	ADC_RANK_CHANNEL(W_PHASE_I_CHAN),//2, C
+};
+
+
+static u32 volatile * adc_phase_reg1[3] = {
+	&ADC_IDATA0(ADC0),//0, A
+	&ADC_IDATA0(ADC0),//1, A
+	&ADC_IDATA0(ADC0),//2, B
+};
+static u32 volatile * adc_phase_reg2[3] = {
+	&ADC_IDATA0(ADC1),//0, B
+	&ADC_IDATA0(ADC1),//1, C
+	&ADC_IDATA0(ADC1),//2, C
+};
+
+static void __inline adc_phase_current_read(u8 phases, s32 *v1, s32 *v2) {
+#if SHUNT_NUM==THREE_SHUNTS_SAMPLE	
+	*v1 = (s32)(*adc_phase_reg1[phases]) ;
+	*v2 = (s32)(*adc_phase_reg2[phases]) ;
+#else
+	*v1 =  (ADC_IDATA0(ADC0) & 0xFFF);
+	*v2 =  (ADC_IDATA0(ADC1) & 0xFFF);
 #endif
-#endif	
 }
 
+static void __inline adc_current_sample_config(u8 phases) {
+#if SHUNT_NUM==THREE_SHUNTS_SAMPLE
+	ADC_ISQ(ADC0) = adc0_rank_channels[phases];
+	ADC_ISQ(ADC1) = adc1_rank_channels[phases];
+#endif
+}
+#endif
 
 static void __inline adc_disable_ext_trigger(void) {   
 	ADC_CTL1(ADC0) &= ~ADC_CTL1_ETEIC;

Applications/bsp/bsp.h

@@ -34,9 +34,9 @@
 #define ADC_SAMPLING_CYCLES 7.5f
 
 #ifdef GD32_FOC_DEMO
-#define TIMER_DT_NS      400U
+#define MOSDEV_DLY      550U
 #define MOSDRV_DT_NS     400U
-#define HW_DEAD_TIME_NS  (TIMER_DT_NS+MOSDRV_DT_NS)
+#define HW_DEAD_TIME_NS  (MOSDEV_DLY+MOSDRV_DT_NS)
 #define HW_RISE_TIME_NS  500u
 #define HW_NOISE_TIME_NS 500u
 #else
@@ -78,6 +78,7 @@
 #define SHUNT_NUM THREE_SHUNTS_SAMPLE
 
 //#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)

Applications/bsp/pwm.c

@@ -317,7 +317,7 @@ static void _gpio_brakein_irq_enable(void){
 
 void pwm_start(void){
 	pwm_update_duty(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2);
-	pwm_update_2smaples(FOC_PWM_Half_Period-10, FOC_PWM_Half_Period + 1);
+	pwm_update_2smaples(FOC_PWM_Half_Period-1, FOC_PWM_Half_Period + 1);
 	/* wait for a new PWM period to flush last HF task */
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
 
@@ -371,6 +371,7 @@ void pwm_turn_on_low_side(void)
 }
 
 void pwm_update_sample(u32 samp1, u32 samp2, u8 sector) {
+#if 0
 	pwm_update_2smaples(samp1, samp2);
 #ifdef ENABLE_AUX_TIMER
 	if (samp1 < FOC_PWM_Half_Period) {
@@ -378,6 +379,15 @@ void pwm_update_sample(u32 samp1, u32 samp2, u8 sector) {
 	}else {
 		adc_update_ext_trigger(ADC_TRIGGER_PHASE2);
 	}
-#endif	
+#endif
+#else
+	if (samp1 < FOC_PWM_Half_Period) {
+		TIMER_CH3CV(pwm_timer) = samp1;
+		pwm_change_t3_mode(TIMER_OC_MODE_PWM1);
+	}else {
+		TIMER_CH3CV(pwm_timer) = samp2;
+		pwm_change_t3_mode(TIMER_OC_MODE_PWM0);
+	}
+#endif
 	adc_current_sample_config(sector);
 }

Applications/bsp/pwm.h

@@ -64,6 +64,13 @@ extern u16 timer_update_buffer[6];
 		timer_flag_clear(pwm_timer, TIMER_FLAG_UP); \
 	}while(0)
 
+#define pwm_change_t3_mode(m) \
+	do { \
+		if (((TIMER_CHCTL1(pwm_timer) >> 12) & 0x7) != m) { \
+			TIMER_CHCTL1(pwm_timer) &= (~(uint32_t)TIMER_CHCTL1_CH3COMCTL); \
+        	TIMER_CHCTL1(pwm_timer) |= (uint32_t)((uint32_t)(m) << 8U); \
+		} \
+	}while(0)
 void pwm_3phase_init(void);
 void pwm_start(void);
 void pwm_stop(void);

Applications/foc/core/PMSM_FOC_Core.c

@@ -181,15 +181,17 @@ void PMSM_FOC_Schedule(void) {
 
 	SVM_Duty_Fix(&vAB, _gFOC_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &_gFOC_Ctrl.out);
 
-	if (_g_ctl_count++ % 5 == 0) {
-		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
-		//plot_3data16(test_motangle>>5, _gFOC_Ctrl.in.s_motAngle>>5, get_hall_stat(9)*60);
-		plot_3data16(_gFOC_Ctrl.in.s_iABC[0], get_hall_stat(9)*60, _gFOC_Ctrl.in.s_motAngle>>5);
-	}
 	phase_current_point(&_gFOC_Ctrl.out);
 
 	pwm_update_duty(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
-	pwm_update_sample(_gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2, _gFOC_Ctrl.out.n_Sector);
+	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_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
+		//plot_3data16(test_motangle>>5, _gFOC_Ctrl.in.s_motAngle>>5, get_hall_stat(9)*60);
+		plot_3data16(_gFOC_Ctrl.in.s_iABC[0], _gFOC_Ctrl.in.s_iABC[1], _gFOC_Ctrl.in.s_iABC[2]);
+		//plot_3data16(_gFOC_Ctrl.out.n_Sample1, _gFOC_Ctrl.out.n_Sample2, _gFOC_Ctrl.out.n_CPhases * 10 + 3000);
+	}	
 }
 
 void PMSM_FOC_LogDebug(void) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -39,6 +39,7 @@ typedef struct {
 	u16   n_lowDuty;
 	u16   n_midDuty;
 	u8    n_Sector;
+	u8    n_CPhases;
 	u16   n_Sample1;
 	u16   n_Sample2;
 	u8    n_RunMode;

Applications/foc/motor/current.c

@@ -42,8 +42,8 @@ void phase_current_start_cali(void){
 
 	phase_current_init();
 	g_cs.is_calibrating_offset = true;
-	g_cs.sector = SECTOR_5;
-	adc_current_sample_config(g_cs.sector);
+	g_cs.c_phases = PHASE_AB;
+	adc_current_sample_config(g_cs.c_phases);
 }
 
 void phase_current_wait_cali(void) {
@@ -54,36 +54,36 @@ void phase_current_wait_cali(void) {
 
 bool phase_current_offset(void) {
 	current_samp_t *cs = &g_cs;
-	if (!g_cs.is_calibrating_offset) {
+	if (!cs->is_calibrating_offset) {
 		return false;
 	}
 	s32 phase_current1 = 0 , phase_current2 = 0;
-	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
+	adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);
 	if (cs->offset_sample_count == (NB_OFFSET_SAMPLES + 1)) {
 		cs->offset_sample_count --;
 		return true;
 	}
 	if (cs->offset_sample_count > 0) {
 		cs->offset_sample_count--;
-		if (cs->sector == SECTOR_5 && cs->offset_sample_count >= 0) {
-			cs->adc_offset_b += phase_current1;
-			cs->adc_offset_a += phase_current2;
+		if (cs->c_phases == PHASE_AB && cs->offset_sample_count >= 0) {
+			cs->adc_offset_a += phase_current1;
+			cs->adc_offset_b += phase_current2;
 			if (cs->offset_sample_count == 0) {
-				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
 				cs->adc_offset_a = cs->adc_offset_a / NB_OFFSET_SAMPLES;
+				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
 			}
 		}
-		if (cs->sector == SECTOR_1 && cs->offset_sample_count >= 0) {
+		if (cs->c_phases == PHASE_BC && cs->offset_sample_count >= 0) {
 			cs->adc_offset_c += phase_current2;
 			if (cs->offset_sample_count == 0) {
 				cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
 			}
 		}
 	}else {
-		if (cs->sector == SECTOR_5) {
-			cs->sector = SECTOR_1;
+		if (cs->c_phases == PHASE_AB) {
+			cs->c_phases = PHASE_BC;
 			phase_current_init();
-			adc_current_sample_config(cs->sector);
+			adc_current_sample_config(cs->c_phases);
 		}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);
@@ -93,26 +93,26 @@ bool phase_current_offset(void) {
 	return true;
 }
 
-#define LowPass_filter 1.0f
+#define LowPass_filter S16Q14(1)
 void phase_current_get(s16 *iABC){
 	current_samp_t *cs = &g_cs;
 	s32 phase_current1, phase_current2;
 
-	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
+	adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);
 
-	if (cs->sector == SECTOR_4 || cs->sector == SECTOR_5) {
+	if (cs->c_phases == PHASE_AB) {
 		/* Current on Phase C is not accessible */
 		/* Ia = PhaseAOffset - ADC converted value) */
-		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
-		cs->adc_ia = (phase_current2 - cs->adc_offset_a);
+		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
+		cs->adc_ib = (phase_current2 - cs->adc_offset_b);
 		cs->adc_ic = -(cs->adc_ia + cs->adc_ib);
-	}else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_6) {
+	}else if (cs->c_phases == PHASE_BC) {
 		/* Current on Phase A is not accessible 	*/
 		/* Ib = PhaseBOffset - ADC converted value) */
 		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
 		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
 		cs->adc_ia = -(cs->adc_ib + cs->adc_ic);
-	}else if (cs->sector == SECTOR_2 || cs->sector == SECTOR_3) {
+	}else if (cs->c_phases == PHASE_AC) {
 		/* Current on Phase B is not accessible 	*/
 		/* Ia = PhaseAOffset - ADC converted value) */
 		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
@@ -124,7 +124,7 @@ void phase_current_get(s16 *iABC){
 	iABC[2] = S16_mul(cs->adc_ic, ADC_TO_CURR_ceof, 9);
 }
 
-
+#if 0
 void phase_current_point(void *p){
 	FOC_OutP *out = p;
 	current_samp_t *cs = &g_cs;
@@ -133,34 +133,94 @@ void phase_current_point(void *p){
 	cs->sector = out->n_Sector;
 	out->n_Sample1 = FOC_PWM_Half_Period + 1;
 	out->n_Sample2 = FOC_PWM_Half_Period + 1;
+	s16 potest = 7;
 	/*底边开mos的时间是2倍的 low_side_low_duty(一个周期)*/
 	if (low_side_low_duty * 2 >= TSampleMIN) { //可以采样
 		if (low_side_low_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
 			out->n_Sample1 = FOC_PWM_Half_Period - 1;
 			cs->sector = SECTOR_1;
+			potest = 1;
 		}else {
 			u32 Samp_p = out->n_lowDuty + TSampleBefore;
 			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
 				out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
+				potest = 2;
 			}else {
 				out->n_Sample1 = Samp_p;
+				potest = 3;
 			}
 		}
 	}else if (low_side_mid_duty * 2 >= TSampleMIN){
 		if (low_side_mid_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
 			out->n_Sample1 = FOC_PWM_Half_Period - 1;
+			potest = 4;
 		}else {
 			u32 Samp_p = out->n_midDuty + TSampleBefore;
 			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
 				out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
+				potest = 5;
 			}else {
 				out->n_Sample1 = Samp_p;
+				potest = 6;
 			}
 		}
 	}
+	plot_1data16(potest);
 	out->n_Sector = cs->sector;
 }
+#else
+static __inline__ s16 get_mid_duty(u32 max_hi_side, FOC_OutP *out) {
+	u32 min_low_side = FOC_PWM_Half_Period - max_hi_side;
+	if (min_low_side >= MAX(TSampleBefore, TADC)) { //可以采样
+		if ((min_low_side > TSampleBefore) && (min_low_side > TADC)) {
+			out->n_Sample1 = FOC_PWM_Half_Period - 1;
+			return 0;
+		}else {
+			u32 Samp_p = max_hi_side + TSampleBefore;
+			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
+				out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
+				return 1;
+			}else {
+				out->n_Sample1 = Samp_p;
+				return 2;
+			}
+		}
+	}
+	return -1;
+}
+void phase_current_point(void *p){
+	FOC_OutP *out = p;
+	current_samp_t *cs = &g_cs;
+	u32 max_hi_side = MAX(out->n_Duty[0], MAX(out->n_Duty[1], out->n_Duty[2]));
+
+	out->n_Sample1 = FOC_PWM_Half_Period + 1;
+	out->n_Sample2 = FOC_PWM_Half_Period + 1;
+
+	if (get_mid_duty(max_hi_side, out) == -1) {
+		u32 mid_hi_side;
+		out->n_Sample1 = FOC_PWM_Half_Period + 1;
+		out->n_Sample2 = FOC_PWM_Half_Period + 1;		
+		if (out->n_Duty[0] == max_hi_side) {
+			//sample B, C
+			mid_hi_side = MAX(out->n_Duty[1], out->n_Duty[2]);
+			cs->c_phases = PHASE_BC;
+		}else if (out->n_Duty[1] == max_hi_side) {
+			//sample A, C
+			mid_hi_side = MAX(out->n_Duty[0], out->n_Duty[2]);
+			cs->c_phases = PHASE_AC;
+		}else {
+			//sample A, B
+			mid_hi_side = MAX(out->n_Duty[0], out->n_Duty[1]);
+			cs->c_phases = PHASE_AB;
+		}
+		get_mid_duty(mid_hi_side, out);
+	}else {
+		cs->c_phases = PHASE_AB;
+	}
+	out->n_CPhases = cs->c_phases;
+}
 
+#endif
 
 void phase_current_adc_triger(void){
 	adc_enable_ext_trigger();

Applications/foc/motor/current.h

@@ -39,7 +39,7 @@ typedef struct current_sample {
 	s16   adc_ia;
 	s16   adc_ib;
 	s16   adc_ic;
-	u8    sector;
+	u8    c_phases;
 	phase_time_t time;	
 	volatile int   offset_sample_count;
 	volatile bool  is_calibrating_offset;

Applications/foc/samples.c

@@ -62,7 +62,7 @@ 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);
+	LowPass_Filter_Fix(_vbus.filted_value, _vbus.value, _vbus.lowpass, 14);
 	sapmple_delta = task_ticks_rel(ticks);
 }
 
@@ -70,7 +70,7 @@ 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);
+	LowPass_Filter_Fix(_throttle.filted_value, _throttle.value, _throttle.lowpass, 14);
 #endif
 }
 

Applications/math/fix_math.h

@@ -37,7 +37,7 @@ typedef int32_t   s32q19_t;
 
 #define S16_mul(a, b, q) (((s32)(a)*(b)) >> q)
 
-#define LowPass_Filter_Fix(value, sample, filter_constant)	(value = ((s32)value * (S16Q14(1) - filter_constant) + (s32)sample * filter_constant) >> 14)
+#define LowPass_Filter_Fix(value, sample, filter_constant, shift)	(value = ((s32)value * (S16Q14(1) - filter_constant) + (s32)sample * filter_constant) >> (shift))
 
 /*
 static __INLINE float S16Q4toF(s16q4_t v) {

Project/GD32_DEMO.uvoptx

@@ -199,6 +199,11 @@
           <WinNumber>1</WinNumber>
           <ItemText>g_config,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>10</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>g_cs,0x0A</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>