update three shunts current sample

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

Applications/app/app.c

@@ -25,16 +25,18 @@ static void _can_report_info(void) {
 	can_report_speed(0x45, foc_get_speed());
 	current_samp_t *s = foc_get_current_sample();
 	can_report_phase_current(0x45, F2I(s->Ia * 1000), F2I(s->Ib * 1000), F2I(s->Ic * 1000));
-	//sys_debug("phase current %f %f %f\n", s->Ia, s->Ib, s->Ic);
+	//sys_debug("phase current %f %f %f %f\n", s->Ia, s->Ib, s->Ic, s->max_Ia);
 	sys_debug("phase offset %d %d %d\n", s->adc_offset_a, s->adc_offset_b, s->adc_offset_c);
+	s->max_Ia = 0.0f;
 }
 
 extern void hall_debug_log(void);
 static void _app_low_task(void *args) {
+	sys_debug("reset src: 0x%x\n", gd32_get_reset_source());
 	while(1) {
 		wdog_reload();
 		_can_report_info();
-		sys_debug("foc exec time %d, intval %d, max %d, error %d\n", g_meas_foc.exec_time, g_meas_foc.intval_time, g_meas_foc.exec_max_error_time, g_meas_foc.intval_time_error);
+		///sys_debug("foc exec time %d, intval %d, max %d, error %d\n", g_meas_foc.exec_time, g_meas_foc.intval_time, g_meas_foc.exec_max_error_time, g_meas_foc.intval_time_error);
 		//sys_debug("hall exec time %d, intval %d\n", g_meas_hall.exec_time, g_meas_hall.intval_time);
 		//sys_debug("vbus voltage: %f\n", foc_get_vbus_voltage());
 		//hall_debug_log();

Applications/bsp/adc.c

@@ -90,12 +90,12 @@ static void _gpio_init(void) {
 
 
 void adc_start_insert_convert(void) {
-	int drop = 5;
+	int drop = 2;
     /* clear the ADC flag */
     adc_flag_clear(ADC0, ADC_FLAG_EOIC);
     adc_flag_clear(ADC1, ADC_FLAG_EOIC);
 
-	adc_enable_ext_trigger();
+	//adc_enable_ext_trigger();
 	while(drop-- > 0) {
 		while ((adc_flag_get(ADC0, ADC_FLAG_EOIC) == RESET) || (adc_flag_get(ADC1, ADC_FLAG_EOIC) == RESET));
 		adc_flag_clear(ADC0, ADC_FLAG_EOIC);

Applications/bsp/adc.h

@@ -30,6 +30,7 @@ inserted ADC 由timer0 ch3触发，
 //#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) 
+
 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 
@@ -75,11 +76,44 @@ static void __inline adc_phase_current_read(u8 sector, s32 *v1, s32 *v2) {
 #endif
 }
 
-
 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];
+#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);	
+#endif
 #endif	
 }
 
@@ -138,6 +172,7 @@ static __inline__ bool adc_eoic_interrupt(void)
 static __inline__ void adc_clear_irq_flags(void) {
 #if SHUNT_NUM==THREE_SHUNTS_SAMPLE
 	ADC_STAT(ADC0) &= ~((u32) ADC_INT_FLAG_EOIC);
+	ADC_STAT(ADC1) &= ~((u32) ADC_INT_FLAG_EOIC);
 #else
 	ADC_STAT(ADC0) &= ~((u32) ADC_INT_FLAG_EOIC);
 	ADC_STAT(ADC1) &= ~((u32) ADC_INT_FLAG_EOIC);

Applications/bsp/bsp.h

@@ -17,14 +17,14 @@
 #define ADC_REFERENCE_VOLTAGE  3300.0f
 
 
-#define SYSTEM_CLOCK (120000000L) //system clk 120M Hz
+#define SYSTEM_CLOCK (120000000u) //system clk 120M Hz
 #define TIM_CLOCK (SYSTEM_CLOCK) /*SystemClock_Config��TIM1��clk��sys PLL �������̶�2����PLLƵ��*/
-#define TIM_CLOCK_MHz (120)
-#define ADC_CLOCK (30000000L)
-#define ADC_CLOCK_MHz (30)
-#define NS_PER_TCLK (8) /* (1/120000000 * 1000000000) */
-#define NS_2_TCLK(ns) ((ns/NS_PER_TCLK) + 1) //ns תΪpwmʹ�õ��Ǹ�TIM��clk count
-#define FOC_PWM_FS (15 * 1000)
+#define TIM_CLOCK_MHz (120u)
+#define ADC_CLOCK (30000000u)
+#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_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 
@@ -32,18 +32,18 @@
 #define ADC_SAMPLING_CYCLES 7.5f
 
 
-#define HW_DEAD_TIME_NS  1200
-#define HW_RISE_TIME_NS  500
-#define HW_NOISE_TIME_NS 500
-#define TDead NS_2_TCLK(HW_DEAD_TIME_NS/2)/* ����ʱ�� */ 
+#define HW_DEAD_TIME_NS  1200u
+#define HW_RISE_TIME_NS  500u
+#define HW_NOISE_TIME_NS 500u
+#define TDead NS_2_TCLK(HW_DEAD_TIME_NS)/* ����ʱ�� */ 
 #define TRise NS_2_TCLK(HW_RISE_TIME_NS)/* MOS ����ʱ��*/
 #define TNoise NS_2_TCLK(HW_NOISE_TIME_NS)/* MOS��������Ŀ�������ʱ�� */
-#define TADC   ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES) * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1)/* ADC ����ʱ�� */
+#define TADC  ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES) * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC ����ʱ�� */
 #define TSampleMIN (TADC + TDead*2 + TRise) //采样需要的总时间
 #define TSampleBefore (TDead + TRise) //采样开始前需要等待的时间
 
 
-#define START_RAMP_DURATION (1000)//ms
+#define START_RAMP_DURATION (2000)//ms
 #define SPEED_SAMPLE_INVAL (100) //ת�Ѳɼ��ļ��,ms
 #define SPEED_RAMP_DURATION SPEED_SAMPLE_INVAL //�����ٶȵ�б��ʱ�䣬�ٶ�ƽ���������½�
 
@@ -66,7 +66,7 @@
 
 #define SHUNT_NUM THREE_SHUNTS_SAMPLE
 
-//#define ENABLE_AUX_TIMER 1
+#define ENABLE_AUX_TIMER 1
 
 void bsp_init(void);
 void wdog_reload(void);

Applications/bsp/can.c

@@ -201,7 +201,7 @@ void shark_can0_deinit(void){
 void shark_can0_init(void){
 	tx_queue = queue_create(64, sizeof(can_trasnmit_message_struct));
 	rx_queue = queue_create(32, sizeof(can_trasnmit_message_struct));
-	co_task_create(_can_tx_task, NULL, 256);
+	co_task_create(_can_tx_task, NULL, 512);
 	co_task_create(_can_rx_task, NULL, 512);
 	shark_can0_txrx_pin_config();
 	shark_can0_config();

Applications/bsp/mc_irqs.c

@@ -85,7 +85,7 @@ void ADC0_1_IRQHandler(void)
 {
 	adc_disable_ext_trigger();
 	mc_phase_current_irq();
-	adc_enable_ext_trigger();
+	//adc_enable_ext_trigger();
     /* clear the ADC flag */
 	adc_clear_irq_flags();	
 }

Applications/bsp/pwm.c

@@ -303,7 +303,7 @@ static void _gpio_brakein_irq_enable(void){
 
 void pwm_start(void){
 #if SHUNT_NUM==THREE_SHUNTS_SAMPLE
-	pwm_update_duty(0, 0, 0);
+	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-1, FOC_PWM_Half_Period+1);
 #else
 	pwm_update_duty_dma(0,0,0,0,0,0);
@@ -312,8 +312,9 @@ void pwm_start(void){
 #endif	
 	/* 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 )
-	{}
+
+	timer_event_software_generate(pwm_timer, TIMER_EVENT_SRC_UPG);
+	while ( timer_flag_get(pwm_timer, TIMER_FLAG_UP) == RESET ){}
 	/* Clear Update Flag */
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
 
@@ -322,7 +323,7 @@ void pwm_start(void){
 	timer_primary_output_config(adc_timer,ENABLE);
 #endif
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
-	//timer_interrupt_enable(pwm_timer, TIMER_INT_UP);
+	timer_interrupt_enable(pwm_timer, TIMER_INT_UP);
 }
 
 void pwm_stop(void){
@@ -334,8 +335,7 @@ void pwm_stop(void){
 	//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 )
-	{}
+	//while ( timer_flag_get(pwm_timer, TIMER_FLAG_UP) == RESET ){}
 	/* Clear Update Flag */
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
 }
@@ -360,6 +360,7 @@ void pwm_turn_on_low_side(void)
 	pwm_update_2smaples(FOC_PWM_Half_Period-(2 * TADC + TRise), FOC_PWM_Half_Period-TADC);
 #endif
 	timer_flag_clear(pwm_timer,TIMER_FLAG_UP);
+	timer_event_software_generate(pwm_timer, TIMER_EVENT_SRC_UPG);
   	while (timer_flag_get(pwm_timer,TIMER_FLAG_UP) == RESET );
   	/* Main PWM Output Enable */
   	timer_primary_output_config(pwm_timer, ENABLE);

Applications/bsp/pwm.h

@@ -23,11 +23,18 @@
 		ch1_update_duty(dutyB);\
 		ch2_update_duty(dutyC);\
 	}while(0)
+#ifdef ENABLE_AUX_TIMER
 #define pwm_update_2smaples(samp1, samp2) \
 	do { \
 		TIMER_CH3CV(pwm_timer) = (uint32_t)samp1; \
 		TIMER_CH0CV(adc_timer) = (uint32_t)samp2; \
 	}while(0)
+#else
+#define pwm_update_2smaples(samp1, samp2) \
+	do { \
+		TIMER_CH3CV(pwm_timer) = (uint32_t)samp1; \
+	}while(0)
+#endif
 #define update_sample_time(samp1, samp2) do{adc_timer_dma_buf[2] = samp1; adc_timer_dma_buf[0] = samp2;}while(0)
 #define update_distort_chxcv(duty) do {pwm_timer_dma_buf[1] = duty;}while(0)
 

Applications/foc/foc_api.c

@@ -65,7 +65,8 @@ float speed_to_current(u16 rpm) {
 	return MAX_CURRENT * rpm / MAX_SPEED_RPM;
 }
 
-void foc_clear(void) {
+
+void foc_stop(void) {
 	pwm_stop();
 	adc_stop_insert_convert();
 	g_foc.mosfec_gate = false;
@@ -73,6 +74,11 @@ void foc_clear(void) {
 	hall_sensor_clear();
 }
 
+void foc_clear(void) {
+	foc_defulat_value();
+	hall_sensor_clear();
+}
+
 u32 foc_get_speed(void) {
 	float speed = hall_sensor_avg_speed()/(g_foc.motor_param.poles);
 	return abs(speed);
@@ -139,7 +145,7 @@ void foc_current_calibrate(void){
 	phase_current_init(&g_foc.current_samp);
 	g_foc.current_samp.is_calibrating_offset = true;
 	g_foc.current_samp.sector = SECTOR_5;
-
+	adc_current_sample_config(g_foc.current_samp.sector);
 	foc_pwm_start(true);
 
 	cpu_udelay(50 * 1000);
@@ -151,6 +157,7 @@ void foc_current_calibrate(void){
 	cpu_udelay(100);
 	phase_current_init(&g_foc.current_samp);	
 	g_foc.current_samp.sector = SECTOR_1;
+	adc_current_sample_config(g_foc.current_samp.sector);
 	foc_pwm_start(true);
 	while(g_foc.current_samp.offset_sample_count != 0){};
 #endif

Applications/foc/foc_api.h

@@ -4,6 +4,7 @@
 
 void foc_init(void);
 void foc_clear(void);
+void foc_stop(void);
 void foc_set_controller_mode(control_mode_t mode);
 void set_dq_voltage(float d_v, float q_v);
 void foc_pwm_start(bool start);

Applications/foc/foc_core.c

@@ -156,6 +156,7 @@ void do_motor_foc(motor_foc_t *foc){
 	foc_update_theta(foc);
 	/* 采集相电流 */
 	phase_current_sample(c_sample);
+		
 	/* ABC三相坐标到alpha-beta坐标 */
 	do_clark(c_sample->Ia, c_sample->Ib, c_sample->Ic, &sample_ab);
 	/* alpha-beta坐标系到D-Q旋转坐标系 */
@@ -163,7 +164,7 @@ void do_motor_foc(motor_foc_t *foc){
 	/* 电流环，输出电压给SVPWM */
 	foc_calc_voltage(foc, &sample_dq, &v_dq);
 	/* 确保电压在6个扇区的内切圆中 */
-	circle_limitation(&v_dq, foc->vbus, 0.95f);
+	circle_limitation(&v_dq, foc->vbus, 0.96f);
 	/* d-q坐标系到alpha-beta坐标系，输出给svpwm */
 	Rev_Park(&v_dq, foc->motor_stat.theta, &pwm_ab);
 	/* SVPWM，获取三相逆变器的开关时间，用的是pwm1模式，如果是pwm2模式，这个函数需要修改 */
@@ -173,6 +174,7 @@ void do_motor_foc(motor_foc_t *foc){
 	/* 死区补偿 */
 	deadtime_compensation(c_sample, &c_sample->time);
 	/* 更新 TIM1的CCR0-2，生成互补pwm, 相电流更新采样点 */
+
 #if SHUNT_NUM==THREE_SHUNTS_SAMPLE	
 	pwm_update_duty(c_sample->time.A, c_sample->time.B, c_sample->time.C);
 #else

Applications/foc/foc_fsm.c

@@ -51,6 +51,7 @@ foc_fault_t foc_fsm_next_state(foc_state_t state) {
 void foc_fsm(motor_foc_t *foc) {
 	switch (foc->state) {
 		case IDLE:
+			foc_clear();
 			foc->mode = FOC_MODE_OPEN_LOOP;
 			break;
 		case START:
@@ -96,8 +97,7 @@ void foc_fsm(motor_foc_t *foc) {
 			}*/
 			break;
 		case ANY_STOP:
-			ramp_clear(&foc->current_ramp);
-			foc_clear();
+			foc_stop();
 			foc_fsm_next_state(IDLE);
 			break;
 		default:

Applications/foc/foc_type.h

@@ -52,6 +52,7 @@ typedef struct _phase_time {
 	u32 low;
 	u32 midle;
 	u32 high;
+	u8  three_shunts_flags;
 }phase_time_t; //����pwn��duty cnt
 
 typedef enum {
@@ -89,6 +90,9 @@ typedef struct current_sample {
 	float old_Ia;
 	float old_Ib;
 	float old_Ic;
+	float max_Ia;
+	float max_Ib;
+	float max_Ic;
 	u8    sector;
 	phase_time_t time;	
 	volatile int   offset_sample_count;

Applications/foc/hall_sensor.c

@@ -208,7 +208,7 @@ int hall_sensor_calibrate(float voltage){
 	for (int i = 0;i < 5;i++) {
 		for (int j = 0;j < 360;j++) {
 			hall_sensor_set_theta(true, j);
-			co_task_delay(5);
+			co_task_delay(50);
 			wdog_reload();
 			int hall = get_hall_stat(7);
 			float s, c;
@@ -225,7 +225,7 @@ int hall_sensor_calibrate(float voltage){
 	for (int i = 0;i < 5;i++) {
 		for (int j = 360;j >= 0;j--) {
 			hall_sensor_set_theta(true, j);
-			co_task_delay(5);
+			co_task_delay(50);
 			wdog_reload();
 			int hall = get_hall_stat(7);
 			float s, c;
@@ -350,8 +350,7 @@ static s32 _hall_position(u8 state_now, u8 state_prev) {
 
 void hall_sensor_handler(void) {
 	if (_sensor_hander.is_override_angle) {
-		log_chan_value(1, _sensor_hander.override_el_angle);
-		log_chan_value(2, (int)get_hall_stat(HALL_READ_TIMES));
+		sys_debug("%d:%d\n", (int)get_hall_stat(HALL_READ_TIMES), (int)_sensor_hander.override_el_angle);
 		return;
 	}
 	time_measure_start(&g_meas_hall);

Applications/foc/phase_current.c

@@ -8,9 +8,9 @@
 
 #define Rvbus  0.0005f
 #define Gvbus (13.1f) //母线电流的运放 
-#define Rds_Defualt  0.0025f//欧
+#define Rds_Defualt  0.005f//欧
 #define Gmos  (1.7f)//mos 电流的运放
-#define Sample_R Rvbus
+#define Sample_R Rds_Defualt
 #define Lower_Pass_p 0.2f
 
 #define VBUS_VOL(adc) (((float)(adc)) * 3.3f / 4096.0f / Gvbus)
@@ -19,7 +19,10 @@
 #define current_i(v, r) ((v)/(r))
 
 void phase_current_init(current_samp_t *cs) {
-	cs->offset_sample_count = NB_OFFSET_SAMPLES;
+	cs->offset_sample_count = NB_OFFSET_SAMPLES + 1;
+	cs->Ia = 0.0f;
+	cs->Ib = 0.0f;
+	cs->Ic = 0.0f;
 }
 
 
@@ -27,51 +30,70 @@ void phase_current_init(current_samp_t *cs) {
 void phase_current_offset(current_samp_t *cs) {
 	s32 phase_current1, phase_current2;
 	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
+	if (cs->offset_sample_count == (NB_OFFSET_SAMPLES + 1)) {
+		cs->offset_sample_count --;
+		return;
+	}
 	if (cs->offset_sample_count > 0) {
 		cs->offset_sample_count--;
 		if (cs->sector == SECTOR_5 && cs->offset_sample_count >= 0) {
-			cs->adc_offset_a += phase_current1;
-			cs->adc_offset_c += phase_current2;
+			cs->adc_offset_b += phase_current1;
+			cs->adc_offset_a += 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_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
 			}
 		}
 		if (cs->sector == SECTOR_1 && cs->offset_sample_count >= 0) {
-			cs->adc_offset_b += phase_current1;
+			cs->adc_offset_c += phase_current2;
 			if (cs->offset_sample_count == 0) {
-				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
+				cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
 			}
 		}
 	}
 }
 
+
+#define LowPass_filter 1.0f
 void phase_current_sample(current_samp_t *cs){
 	s32 phase_current1, phase_current2;
+	float Ia, Ib, Ic;
+	phase_time_t *time = &cs->time;
 	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
+	if (time->three_shunts_flags == 1) {
+		time->three_shunts_flags = 0;
+		return; //use old current;
+	}
 	if (cs->sector == SECTOR_4 || cs->sector == SECTOR_5) {
 		/* Current on Phase C is not accessible */
 		/* Ia = PhaseAOffset - ADC converted value) */
-		cs->Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), Rds_Defualt);
-		cs->Ia = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_a), Rds_Defualt);
+		Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), Rds_Defualt);
+		LowPass_Filter(cs->Ib, Ib, LowPass_filter);
+		Ia = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_a), Rds_Defualt);
+		LowPass_Filter(cs->Ia, Ia, LowPass_filter);
 		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 = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), Rds_Defualt);
-		cs->Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Rds_Defualt);
+		Ib = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_b), Rds_Defualt);
+		LowPass_Filter(cs->Ib, Ib, LowPass_filter);
+		Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Rds_Defualt);
+		LowPass_Filter(cs->Ic, Ic, LowPass_filter);
 		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 = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_a), Rds_Defualt);
-		cs->Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Rds_Defualt);
+		Ia = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_a), Rds_Defualt);
+		LowPass_Filter(cs->Ia, Ia, LowPass_filter);
+		Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Rds_Defualt);
+		LowPass_Filter(cs->Ic, Ic, LowPass_filter);
 		cs->Ib = -(cs->Ia + cs->Ic);
 	}
+	
 	{
 		static int count = 0;
-		if (count++ % 3 == 0) {
-			log_chan_value(1, (int)(cs->Ia * 1000));
+		if (count++ % 2 == 0) {
+			//log_chan_value(1, (int)(cs->Ia * 1000));
 		}
 	}
 
@@ -81,21 +103,23 @@ void phase_current_sample(current_samp_t *cs){
 void get_phase_sample_point(current_samp_t *cs, u8 sector){
 	phase_time_t *time = &cs->time;
 	u32 low_side_low_duty = FOC_PWM_Half_Period - time->low;
-#if 0	
 	u32 low_side_mid_duty = FOC_PWM_Half_Period - time->midle;
 	cs->sector = sector;
 	time->Samp_p1 = FOC_PWM_Half_Period + 1;
-	time->Samp_p2 = FOC_PWM_Half_Period + 1;	
+	time->Samp_p2 = FOC_PWM_Half_Period + 1;
 	/*底边开mos的时间是2倍的 low_side_low_duty(一个周期)*/
 	if (low_side_low_duty * 2 >= TSampleMIN) { //可以采样
 		if (low_side_low_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
 			time->Samp_p1 = FOC_PWM_Half_Period - 1;
+			cs->sector = SECTOR_1;
 		}else {
 			u32 Samp_p = time->low + TSampleBefore;
 			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
 				time->Samp_p2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
+				//log_chan_value(2, time->Samp_p2);
 			}else {
 				time->Samp_p1 = Samp_p;
+				//log_chan_value(4, time->Samp_p1);
 			}
 		}
 	}else if (low_side_mid_duty * 2 >= TSampleMIN){
@@ -104,34 +128,17 @@ void get_phase_sample_point(current_samp_t *cs, u8 sector){
 		}else {
 			u32 Samp_p = time->midle + TSampleBefore;
 			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样，需要配置PWM0模式
-				time->Samp_p2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;		
+				time->Samp_p2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
+				//log_chan_value(3, time->Samp_p2);
 			}else {
 				time->Samp_p1 = Samp_p;
+				//log_chan_value(5, time->Samp_p1);
 			}
-		}		
-	}else {
-		//not reached there
-		sys_debug("ERROR!!!!!\n");
-	}
-#else	
-	if (low_side_low_duty > (TDead + MAX(TRise, TNoise))) {
-		cs->sector = SECTOR_1;
-		time->Samp_p1 =  time->low + MAX(TRise, TNoise);
-	}else {
-		u32 low_side_mid_duty = FOC_PWM_Half_Period - time->midle;
-		u32 delta_duty = low_side_mid_duty - low_side_low_duty;
-		if (delta_duty > low_side_low_duty * 2) {
-			time->Samp_p1 =  time->low - TADC;
-		}else {
-			u32 sample_point = time->low + (TDead + MAX(TRise, TNoise));
-			if (sample_point >= FOC_PWM_Half_Period) {
-				//这里需要修改触发方式，GD不支持adc设置上升或下降沿触发，考虑切换pwm模式？？？
-				sample_point = ( 2u * FOC_PWM_Half_Period ) - sample_point - (uint16_t) 1;		
-			}
-			time->Samp_p1 =  sample_point;
 		}
+	}else {
+		time->three_shunts_flags = 1; //means do'nt use the sample current
+		time->Samp_p1 = FOC_PWM_Half_Period - 1;//dumy trigger
 	}
-#endif	
 }
 #else
 
@@ -632,7 +639,6 @@ void get_phase_sample_point(current_samp_t *cs, u8 sector){
 
 #endif
 void phase_current_adc_triger(current_samp_t *cs){
-	
-	//adc_enable_ext_trigger();
+	adc_enable_ext_trigger();
 }