MC100/Applications/foc/phase_current.c

#include <math.h>
#include "bsp/adc.h"
#include "phase_current.h"
#include "libs/utils.h"
#include "libs/logger.h"

#define NB_OFFSET_SAMPLES 32

#define Rvbus  0.0005f
#define Gvbus (13.1f) //母线电流的运放 
#define Rds_Defualt  0.005f//欧
#define Gmos  (1.7f)//mos 电流的运放
#define Sample_R Rds_Defualt
#define Lower_Pass_p 0.2f

#define VBUS_VOL(adc) (((float)(adc)) * 3.3f / 4096.0f / Gvbus)
#define MOSds_VOL(adc) (((float)(adc)) * 3.3f / 4096.0f / Gmos)

#define current_i(v, r) ((v)/(r))

void phase_current_init(current_samp_t *cs) {
	cs->offset_sample_count = NB_OFFSET_SAMPLES + 1;
	cs->Ia = 0.0f;
	cs->Ib = 0.0f;
	cs->Ic = 0.0f;
}


#if SHUNT_NUM==THREE_SHUNTS_SAMPLE
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_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;
			}
		}
		if (cs->sector == SECTOR_1 && 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;
			}
		}
	}
}


#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) */
		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) */
		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) */
		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++ % 2 == 0) {
			//log_chan_value(1, (int)(cs->Ia * 1000));
		}
	}

}


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;
	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;
	/*底边开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){
		if (low_side_mid_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
			time->Samp_p1 = FOC_PWM_Half_Period - 1;
		}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;
				//log_chan_value(3, time->Samp_p2);
			}else {
				time->Samp_p1 = Samp_p;
				//log_chan_value(5, time->Samp_p1);
			}
		}
	}else {
		time->three_shunts_flags = 1; //means do'nt use the sample current
		time->Samp_p1 = FOC_PWM_Half_Period - 1;//dumy trigger
	}
}
#else

#define TBEFOR (TDead + MAX(TRise, TNoise))
#define TMIN (TDead + MAX(TRise, TNoise) + TADC)

static __inline u8 _get_sample_boundary(current_samp_t *cs, phase_time_t *time) {
#if 0
	s32 delta_duty0 = (s32)time->midle - (s32)time->high;
	s32 delta_duty1 = (s32)time->low - (s32)time->midle;

	if (delta_duty0 <= TMIN && delta_duty1 <= TMIN) {
		return BOUNDARY_3;
	}else if (delta_duty0 <= TMIN && delta_duty1 > TMIN) {
		return BOUNDARY_2;
	}else if (delta_duty0 > TMIN && delta_duty1 <= TMIN) {
		return BOUNDARY_1;
	}else {
		return REGULAR;
	}
#else
	return REGULAR;
#endif
}

static __inline void _get_boundary1_samp(current_samp_t *cs, phase_time_t *time) {
	s32 delta_duty1 = (s32)time->low - (s32)time->midle;
	s32 delta_time_inc = (TMIN - delta_duty1);
	s32 delta_time_dec = min(delta_time_inc, delta_duty1);
	s32 sample_p;
	switch(cs->sector) {
		case SECTOR_1: //AB big and delta small
			time->Samp_p1 = time->B - TADC;
			sample_p = time->A;
			time->A = sample_p + delta_time_inc;
			time->A_next = sample_p - delta_time_dec;
			time->Samp_p2 = time->A - TADC;
			time->sampe_phase_1 = SAMP_NIC;
			time->sampe_phase_2 = SAMP_IA;
			break;
		case SECTOR_2://BA big and delta small
			time->Samp_p1 = time->A - TADC;
			sample_p = time->B;
			time->B = sample_p + delta_time_inc;
			time->B_next = sample_p - delta_time_dec;
			time->Samp_p2 = time->B - TADC;
			time->sampe_phase_1 = SAMP_NIC;
			time->sampe_phase_2 = SAMP_IB;		
			break;
		case SECTOR_3://BC big and delta small
			time->Samp_p1 = time->C - TADC;
			sample_p = time->B;
			time->B = sample_p + delta_time_inc;
			time->B_next = sample_p - delta_time_dec;
			time->Samp_p2 = time->B - TADC;
			time->sampe_phase_1 = SAMP_NIA;
			time->sampe_phase_2 = SAMP_IB;				
			break;
		case SECTOR_4://CB big and delta small
			time->Samp_p1 = time->B - TADC;
			sample_p = time->C;
			time->C = sample_p + delta_time_inc;
			time->C_next = sample_p - delta_time_dec;
			time->Samp_p2 = time->C - TADC;
			time->sampe_phase_1 = SAMP_NIA;
			time->sampe_phase_2 = SAMP_IC;			
			break;
		case SECTOR_5://CA big and delta small
			time->Samp_p1 = time->A - TADC;
			sample_p = time->C;
			time->C = sample_p + delta_time_inc;
			time->C_next = sample_p - delta_time_dec;
			time->Samp_p2 = time->C - TADC;
			time->sampe_phase_1 = SAMP_NIB;
			time->sampe_phase_2 = SAMP_IC;			
			break;
		case SECTOR_6://AC big and delta small
			time->Samp_p1 = time->C - TADC;
			sample_p = time->A;
			time->A = sample_p + delta_time_inc;
			time->A_next = sample_p - delta_time_dec;
			time->Samp_p2 = time->A - TADC;
			time->sampe_phase_1 = SAMP_NIB;
			time->sampe_phase_2 = SAMP_IA;				
			break;
		default:
			break;
	}
}

static __inline void _get_boundary2_samp(current_samp_t *cs, phase_time_t *time) {
	s32 delta_duty0 = (s32)time->midle - (s32)time->high;
	s32 delta_time_dec = (TMIN - delta_duty0);
	s32 delta_time_inc = min(delta_time_dec, delta_duty0);
	s32 sample_p;
	switch(cs->sector) {
		case SECTOR_1: //BC samll and delta small
			time->Samp_p2 = time->A - TADC;
			sample_p = time->C;
			time->C = sample_p - delta_time_dec;
			time->C_next = sample_p + delta_time_inc;
			time->Samp_p1 = time->B - TADC;
			time->sampe_phase_2 = SAMP_IA;
			time->sampe_phase_1 = SAMP_NIC;		
			break;
		case SECTOR_2://AC samll and delta small
			time->Samp_p2 = time->B - TADC;
			sample_p = time->C;
			time->C = sample_p - delta_time_dec;
			time->C_next = sample_p + delta_time_inc;
			time->Samp_p1 = time->A - TADC;
			time->sampe_phase_2 = SAMP_IB;
			time->sampe_phase_1 = SAMP_NIC;			
			break;
		case SECTOR_3://CA samll and delta small
			time->Samp_p2 = time->B - TADC;
			sample_p = time->A;
			time->A = sample_p - delta_time_dec;
			time->A_next = sample_p + delta_time_inc;
			time->Samp_p1 = time->C - TADC;
			time->sampe_phase_2 = SAMP_IB;
			time->sampe_phase_1 = SAMP_NIA;				
			break;
		case SECTOR_4://BA samll and delta small
			time->Samp_p2 = time->C - TADC;
			sample_p = time->A;
			time->A = sample_p - delta_time_dec;
			time->A_next = sample_p + delta_time_inc;
			time->Samp_p1 = time->B - TADC;
			time->sampe_phase_2 = SAMP_IC;
			time->sampe_phase_1 = SAMP_NIA;			
			break;
		case SECTOR_5://AB samll and delta small
			time->Samp_p2 = time->C - TADC;
			sample_p = time->B;
			time->B = sample_p - delta_time_dec;
			time->B_next = sample_p + delta_time_inc;
			time->Samp_p1 = time->A - TADC;
			time->sampe_phase_2 = SAMP_IC;
			time->sampe_phase_1 = SAMP_NIB;			
			break;
		case SECTOR_6://CB samll and delta small
			time->Samp_p2 = time->A - TADC;
			sample_p = time->B;
			time->B = sample_p - delta_time_dec;
			time->B_next = sample_p + delta_time_inc;
			time->Samp_p1 = time->C - TADC;
			time->sampe_phase_2 = SAMP_IA;
			time->sampe_phase_1 = SAMP_NIB;			
			break;
		default:
			break;
	}

}


static __inline void _get_boundary3_samp(current_samp_t *cs, phase_time_t *time) {
#if 1
	s32 sample_p;
	if ((time->boundary3_flags & 1) == 0) {
		time->boundary3_flags |= 1;
		sample_p = time->A;
		time->A = sample_p + TMIN;
		time->A_next = sample_p - TMIN;
		time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
		time->Samp_p2 = time->A - TADC;
		time->sampe_phase_1 = SAMP_OLDB;
		time->sampe_phase_2 = SAMP_IA;			
	}else {
		time->boundary3_flags &= ~1;
		sample_p = time->B;
		time->B = sample_p + TMIN;
		time->B_next = sample_p - TMIN;
		time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
		time->Samp_p2 = time->B - TADC;
		time->sampe_phase_1 = SAMP_OLDA;
		time->sampe_phase_2 = SAMP_IB;			
	}
#else
	s32 delta_duty0 = (s32)time->midle - (s32)time->high;
	s32 delta_duty1 = (s32)time->low - (s32)time->midle;

	s32 delta_time_inc1 = (TMIN - delta_duty1);
	s32 delta_time_dec1 = min(delta_time_inc1, delta_duty1);
	
	s32 delta_time_dec2 = (TMIN - delta_duty0);
	s32 delta_time_inc2 = min(delta_time_dec2, delta_duty0);

	s32 sample_p;
	switch(cs->sector) {
		case SECTOR_1: //deltaBC > deltaAB
			if (delta_duty0 > delta_duty1) {
				sample_p = time->C;
				time->C = sample_p - delta_time_dec2;
				time->C_next = sample_p + delta_time_inc2;
				time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->B - TADC;
				time->sampe_phase_1 = SAMP_OLDB;
				time->sampe_phase_2 = SAMP_NIC;					
			}else {
				sample_p = time->A;
				time->A = sample_p + delta_time_inc1;
				time->A_next = sample_p - delta_time_dec1;	
				time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->A - TADC;
				time->sampe_phase_1 = SAMP_OLDC;
				time->sampe_phase_2 = SAMP_IA;						
			}
			break;
		case SECTOR_2: //deltaAC > deltaBA
			if (delta_duty0 > delta_duty1) {
				sample_p = time->C;
				time->C = sample_p - delta_time_dec2;
				time->C_next = sample_p + delta_time_inc2;
				time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->A - TADC;
				time->sampe_phase_1 = SAMP_OLDA;
				time->sampe_phase_2 = SAMP_NIC;					
			}else {
				sample_p = time->B;
				time->B = sample_p + delta_time_inc1;
				time->B_next = sample_p - delta_time_dec1;	
				time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->B - TADC;
				time->sampe_phase_1 = SAMP_OLDB;
				time->sampe_phase_2 = SAMP_IB;						
			}	
			break;
		case SECTOR_3: //deltaCA > deltaBC
			if (delta_duty0 > delta_duty1) {
				sample_p = time->A;
				time->A = sample_p - delta_time_dec2;
				time->A_next = sample_p + delta_time_inc2;
				time->Samp_p1 = time->C - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->C - TADC;
				time->sampe_phase_1 = SAMP_OLDC;
				time->sampe_phase_2 = SAMP_NIA;					
			}else {
				sample_p = time->B;
				time->B = sample_p + delta_time_inc1;
				time->B_next = sample_p - delta_time_dec1;	
				time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->B - TADC;
				time->sampe_phase_1 = SAMP_OLDA;
				time->sampe_phase_2 = SAMP_IB;						
			}		
			break;
		case SECTOR_4: //CBA, //deltaBA > deltaCB
			if (delta_duty0 > delta_duty1) {
				sample_p = time->A;
				time->A = sample_p - delta_time_dec2;
				time->A_next = sample_p + delta_time_inc2;
				time->Samp_p1 = time->B - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->B - TADC;
				time->sampe_phase_1 = SAMP_OLDB;
				time->sampe_phase_2 = SAMP_NIA;					
			}else {
				sample_p = time->C;
				time->C = sample_p + delta_time_inc1;
				time->C_next = sample_p - delta_time_dec1;	
				time->Samp_p1 = time->C - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->C - TADC;
				time->sampe_phase_1 = SAMP_OLDC;
				time->sampe_phase_2 = SAMP_IC;						
			}	
			break;
		case SECTOR_5: //CAB, //deltaAB > deltaCA
			if (delta_duty0 > delta_duty1) {
				sample_p = time->B;
				time->B = sample_p - delta_time_dec2;
				time->B_next = sample_p + delta_time_inc2;
				time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->A - TADC;
				time->sampe_phase_1 = SAMP_OLDA;
				time->sampe_phase_2 = SAMP_NIB;					
			}else {
				sample_p = time->C;
				time->C = sample_p + delta_time_inc1;
				time->C_next = sample_p - delta_time_dec1;	
				time->Samp_p1 = time->C - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->C - TADC;
				time->sampe_phase_1 = SAMP_OLDB;
				time->sampe_phase_2 = SAMP_IC;						
			}		
			break;
		case SECTOR_6: //ACB, //deltaCB > deltaAC
			if (delta_duty0 > delta_duty1) {
				sample_p = time->B;
				time->B = sample_p - delta_time_dec2;
				time->B_next = sample_p + delta_time_inc2;
				time->Samp_p1 = time->C - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->C - TADC;
				time->sampe_phase_1 = SAMP_OLDC;
				time->sampe_phase_2 = SAMP_NIB;					
			}else {
				sample_p = time->A;
				time->A = sample_p + delta_time_inc1;
				time->A_next = sample_p - delta_time_dec1;	
				time->Samp_p1 = time->A - (2 * TADC + TRise);//dumy trigger
				time->Samp_p2 = time->A - TADC;
				time->sampe_phase_1 = SAMP_OLDA;
				time->sampe_phase_2 = SAMP_IA;						
			}		
			break;
		default:
			break;
	}
#endif
}

static __inline void _get_regular_samp(current_samp_t *cs, phase_time_t *time) {
	time->Samp_p1 = time->midle - TADC;
	time->Samp_p2 = time->low - TADC;
	switch(cs->sector) {
		case SECTOR_1: //ABC
			time->sampe_phase_1 = SAMP_NIC;
			time->sampe_phase_2 = SAMP_IA;	
			break;
		case SECTOR_2: //BAC
			time->sampe_phase_1 = SAMP_NIC;
			time->sampe_phase_2 = SAMP_IB;		
			break;
		case SECTOR_3: //BCA
			time->sampe_phase_1 = SAMP_NIA;
			time->sampe_phase_2 = SAMP_IB;		
			break;
		case SECTOR_4: //CBA
			time->sampe_phase_1 = SAMP_NIA;
			time->sampe_phase_2 = SAMP_IC;		
			break;
		case SECTOR_5: //CAB
			time->sampe_phase_1 = SAMP_NIB;
			time->sampe_phase_2 = SAMP_IC;		
			break;
		case SECTOR_6: //ACB
			time->sampe_phase_1 = SAMP_NIB;
			time->sampe_phase_2 = SAMP_IA;		
			break;
		default:
			break;
	}

}

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 > 0) {
		cs->offset_sample_count--;
		if (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_a = cs->adc_offset_a / NB_OFFSET_SAMPLES;
				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
			}
		}
	}
}

void phase_current_sample(current_samp_t *cs){
	s32 phase_current1, phase_current2;
	u8  b_curr_a = 0;
	u8  b_curr_b = 0;
	u8  b_curr_c = 0;
	phase_time_t *time = &cs->time;
	adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
	phase_current1 -= cs->adc_offset_a;
	phase_current2 -= cs->adc_offset_b;
	float current = current_i(VBUS_VOL(abs(phase_current1)), Sample_R);
	switch (time->sampe_phase_1) {
		case SAMP_IA:
			cs->Ia = current;
			b_curr_a = 1;
			break;
		case SAMP_IB:
			cs->Ib = current;
			b_curr_b = 1;
			break;
		case SAMP_IC:
			cs->Ic = current;
			b_curr_c = 1;
			break;
		case SAMP_NIA:
			cs->Ia = -current;
			b_curr_a = 1;
			break;
		case SAMP_NIB:
			cs->Ib = -current;
			b_curr_b = 1;
			break;
		case SAMP_NIC:
			cs->Ic = -current;
			b_curr_c = 1;
			break;		
		case SAMP_OLDA:
			cs->Ia = cs->old_Ia;
			b_curr_a = 1;
			break;
		case SAMP_OLDB:
			cs->Ib = cs->old_Ib;
			b_curr_b = 1;
			break;
		case SAMP_OLDC:
			cs->Ic = cs->old_Ic;
			b_curr_c = 1;
			break;
		default:
			break;
	}

	current = current_i(VBUS_VOL(abs(phase_current2)), Sample_R);

	switch (time->sampe_phase_2) {
		case SAMP_IA:
			cs->Ia = current;
			b_curr_a = 1;
			break;
		case SAMP_IB:
			cs->Ib = current;
			b_curr_b = 1;
			break;
		case SAMP_IC:
			cs->Ic = current;
			b_curr_c = 1;
			break;
		case SAMP_NIA:
			cs->Ia = -current;
			b_curr_a = 1;
			break;
		case SAMP_NIB:
			cs->Ib = -current;
			b_curr_b = 1;
			break;
		case SAMP_NIC:
			cs->Ic = -current;
			b_curr_c = 1;
			break;		
		case SAMP_OLDA:
			cs->Ia = cs->old_Ia;
			b_curr_a = 1;
			break;
		case SAMP_OLDB:
			cs->Ib = cs->old_Ib;
			b_curr_b = 1;
			break;
		case SAMP_OLDC:
			cs->Ic = cs->old_Ic;
			b_curr_c = 1;
			break;
		default:
			break;
	}
	if (b_curr_a == 0) {
		cs->Ia = -(cs->Ib + cs->Ic);
	}
	if (b_curr_b == 0) {
		cs->Ib = -(cs->Ia + cs->Ic);
	}
	if (b_curr_c == 0) {		
		cs->Ic = -(cs->Ia + cs->Ib);
	}
	cs->old_Ia = cs->Ia;
	cs->old_Ib = cs->Ib;
	cs->old_Ic = cs->Ic;
	{
		static int count = 0;
		if (count++ % 3 == 0) {
			log_chan_value(1, (int)(cs->Ia * 1000));
		}
	}
}


void get_phase_sample_point(current_samp_t *cs, u8 sector){
	phase_time_t *time = &cs->time;
	if (cs->is_calibrating_offset) {
		time->Samp_p1 = FOC_PWM_Half_Period - 2 * TMIN;
		time->Samp_p2 = FOC_PWM_Half_Period - 1;
		return;
	}
	cs->sector = sector;
	time->A_next = time->A;
	time->B_next = time->B;
	time->C_next = time->C;
	u8 boundary = _get_sample_boundary(cs, time);
	if (boundary == BOUNDARY_1) {
		_get_boundary1_samp(cs, time);
	}else if (boundary == BOUNDARY_2) {
		_get_boundary2_samp(cs, time);
	}else if (boundary == BOUNDARY_3) {
		_get_boundary3_samp(cs, time);
	}else { //REGULAR, 直接可以采样
		_get_regular_samp(cs, time);
	}
}

#endif
void phase_current_adc_triger(current_samp_t *cs){
	adc_enable_ext_trigger();
}