MC100/Applications/foc/phase_current.c

#include "bsp/adc.h"
#include "foc_type.h"

#define NB_OFFSET_SAMPLES 32

static float __inline adc_to_current(int adc){
	int i_adc = (int)adc;
	if (i_adc > INT16_MAX){
		i_adc = INT16_MAX;
	}else if (i_adc < -INT16_MAX) {
		i_adc = - INT16_MAX;
	}
	return (i_adc/65535.0f * 3.3f / 1.53f / 0.33f);
}

void phase_current_init(current_samp_t *cs) {
	cs->offset_sample_count = NB_OFFSET_SAMPLES;
}


void phase_current_offset(current_samp_t *cs) {
	u32 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->sector == SECTOR_5 && cs->offset_sample_count >= 0) {
			cs->adc_offset_a += phase_current1;
			cs->adc_offset_c += phase_current2;
			if (cs->offset_sample_count == 0) {
				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;
			adc_vbus_current_read(cs->sector, &phase_current1, &phase_current2);
			cs->adc_offset_ivbus += phase_current1;
			if (cs->offset_sample_count == 0) {
				cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
				cs->adc_offset_ivbus = cs->adc_offset_ivbus / NB_OFFSET_SAMPLES;
			}
		}
	}

}

void phase_current_sample(current_samp_t *cs){
		u32 phase_current1, phase_current2;
		adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
		if (cs->sector == SECTOR_1 || cs->sector == SECTOR_2) {
			/* Current on Phase C is not accessible */
			/* Ia = PhaseAOffset - ADC converted value) */
			cs->Ib = adc_to_current((int)phase_current1 - (int)cs->adc_offset_b);
			cs->Ia = adc_to_current((int)phase_current2 - (int)cs->adc_offset_a);
			cs->Ic = -(cs->Ia + cs->Ib);
			adc_vbus_current_read(cs->sector, &cs->adc_vbus_ib, &cs->adc_vbus_ia);
		}else if (cs->sector == SECTOR_3 || cs->sector == SECTOR_4) {
			/* Current on Phase A is not accessible 	*/
			/* Ib = PhaseBOffset - ADC converted value) */
			cs->Ic = adc_to_current((int)phase_current1 - (int)cs->adc_offset_c);
			cs->Ib = adc_to_current((int)phase_current2 - (int)cs->adc_offset_b);
			cs->Ia = -(cs->Ib + cs->Ic);
			adc_vbus_current_read(cs->sector, &cs->adc_vbus_ic, &cs->adc_vbus_ib);
		}else if (cs->sector == SECTOR_5 || cs->sector == SECTOR_6) {
			/* 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);
			adc_vbus_current_read(cs->sector, &cs->adc_vbus_ia, &cs->adc_vbus_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));
		}
#endif	

}


u32 get_phase_sample_point(current_samp_t *cs, phase_time_t *time, u8 sector){
	u32 low_side_low_duty = FOC_PWM_Half_Period - time->low;	
	cs->sector = sector;
	//duty > deadtime + max(Rise time, Noise time)
	if (low_side_low_duty > (TDead + MAX(TRise, TNoise))) {
		cs->sector = SECTOR_1;
		return FOC_PWM_Half_Period - 1;
	}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) {
			return time->low - TADC;
		}else {
			u32 sample_point = time->low + (TDead + MAX(TRise, TNoise));
			if (sample_point >= FOC_PWM_Half_Period) {
				sample_point = ( 2u * FOC_PWM_Half_Period ) - sample_point - (uint16_t) 1;		
			}
			return sample_point;
		}
	}

}

void phase_current_adc_triger(current_samp_t *cs){
	adc_phase_inserted_config(cs->sector);
}