func: caliberate mos Rds through vbus current

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

Applications/bsp/adc.c

@@ -23,6 +23,8 @@ void adc_init(void){
 
 	_adc1_init();
 
+	adc_config_trigger(ADC_TRIGGER_PHASE);
+
 	nvic_irq_enable(ADC0_1_IRQn, 0, 0);
 }
 
@@ -90,7 +92,7 @@ static void _adc0_insert_chan_init(void) {
     adc_inserted_channel_config(adc_dev, 0, VBUS_I_CHAN, ADC_SAMPLETIME_7POINT5);
 	adc_inserted_channel_config(adc_dev, 1, VBUS_I_CHAN, ADC_SAMPLETIME_7POINT5);
     /* ADC trigger config */
-    adc_external_trigger_source_config(adc_dev, ADC_INSERTED_CHANNEL, ADC0_1_EXTTRIG_INSERTED_T1_CH0); 
+//    adc_external_trigger_source_config(adc_dev, ADC_INSERTED_CHANNEL, ADC0_1_EXTTRIG_INSERTED_T1_CH0); 
 }
 
 /* ADC1 insert chan sample phase I(use two chan, selected by foc) */
@@ -106,7 +108,7 @@ static void _adc1_insert_chan_init(void) {
 	adc1_update_insert_sample_time(V_PHASE_I_CHAN, ADC_SAMPLETIME_7POINT5);
 	adc1_update_insert_sample_time(W_PHASE_I_CHAN, ADC_SAMPLETIME_7POINT5);
     /* ADC trigger config */
-    adc_external_trigger_source_config(adc_dev, ADC_INSERTED_CHANNEL, ADC0_1_EXTTRIG_INSERTED_T1_CH0); 
+//    adc_external_trigger_source_config(adc_dev, ADC_INSERTED_CHANNEL, ADC0_1_EXTTRIG_INSERTED_T1_CH0); 
 }
 
 

Applications/bsp/adc.h

@@ -21,14 +21,18 @@ inserted ADC 由timer0 ch3触发，
 #define ISO3_OFFSET 15
 #define IL_OFFSET   20
 
+#define ADC_RANK_CHANNEL(c1, c2) ((c1)<<ISQ2_OFFSET | (c2)<<ISO3_OFFSET | 1<<IL_OFFSET)
+
 static u32 adc_rank_channels[6] = {
-	V_PHASE_I_CHAN<<ISQ2_OFFSET | U_PHASE_I_CHAN<<ISO3_OFFSET | 1<<IL_OFFSET,
-	U_PHASE_I_CHAN<<ISQ2_OFFSET | V_PHASE_I_CHAN<<ISO3_OFFSET | 1<<IL_OFFSET,
-	W_PHASE_I_CHAN<<ISQ2_OFFSET | V_PHASE_I_CHAN<<ISO3_OFFSET | 1<<IL_OFFSET,
-	V_PHASE_I_CHAN<<ISQ2_OFFSET | W_PHASE_I_CHAN<<ISO3_OFFSET | 1<<IL_OFFSET,
-	U_PHASE_I_CHAN<<ISQ2_OFFSET | W_PHASE_I_CHAN<<ISO3_OFFSET | 1<<IL_OFFSET,
-	W_PHASE_I_CHAN<<ISQ2_OFFSET | U_PHASE_I_CHAN<<ISO3_OFFSET | 1<<IL_OFFSET,
+	ADC_RANK_CHANNEL(V_PHASE_I_CHAN, U_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(U_PHASE_I_CHAN, V_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(W_PHASE_I_CHAN, V_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(V_PHASE_I_CHAN, W_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(U_PHASE_I_CHAN, W_PHASE_I_CHAN),
+	ADC_RANK_CHANNEL(W_PHASE_I_CHAN, U_PHASE_I_CHAN),
 };
+
+
 #define PHASE_I_ADC ADC1
 static u32 volatile * adc_phase_reg1[6] = {
 	&ADC_IDATA2(PHASE_I_ADC),
@@ -65,13 +69,21 @@ static u32 volatile * adc_vbus_reg2[6] = {
 };
 
 void __inline adc_phase_current_read(u8 sector, u32 *v1, u32 *v2) {
-	*v1 = *adc_phase_reg1[sector];
-	*v2 = *adc_phase_reg2[sector];
+	*v1 = (*adc_phase_reg1[sector]) << 4;
+	*v2 = (*adc_phase_reg2[sector]) << 4;
+}
+
+u32 __inline adc_phase_read_1(u8 sector) {
+	return (*adc_phase_reg1[sector]) << 4;
 }
 
 void __inline adc_vbus_current_read(u8 sector, u32 *v1, u32 *v2) {
-	*v1 = *adc_vbus_reg1[sector];
-	*v2 = *adc_vbus_reg2[sector];
+	*v1 = (*adc_vbus_reg1[sector]) << 4;
+	*v2 = (*adc_vbus_reg2[sector]) << 4;
+}
+
+u32 __inline adc_vbus_read_1(u8 sector) {
+	return (*adc_vbus_reg1[sector]) << 4;
 }
 
 
@@ -79,6 +91,23 @@ void __inline adc_phase_inserted_config(u8 sector) {
 	ADC_ISQ(ADC1) = adc_rank_channels[sector];
 }
 
+#define ADC_TRIGGER_PHASE ADC0_1_EXTTRIG_INSERTED_T0_CH3
+#define ADC_TRIGGER_VBUS ADC0_1_EXTTRIG_INSERTED_T1_CH0
+
+
+void __inline adc_config_trigger(u32 trigger) {
+	ADC_CTL1(ADC0) &= ~((uint32_t)ADC_CTL1_ETSIC);
+	ADC_CTL1(ADC0) |= (uint32_t)trigger;
+	ADC_CTL1(ADC1) &= ~((uint32_t)ADC_CTL1_ETSIC);
+	ADC_CTL1(ADC1) |= (uint32_t)trigger;
+}
+
+bool __inline adc_is_trigged_vbus(void) {
+	if (ADC_CTL1(ADC0) & ADC_TRIGGER_VBUS == ADC_TRIGGER_VBUS) {
+		return true;
+	}
+	return false;
+}
 
 /* insert len fixed to 2(IL=1), ISQ2 >> ISQ3*/
 static __inline__ void adc1_update_insert_sample_rank(u8 channel1, u8 channel2) {

Applications/bsp/mc_irqs.c

@@ -81,11 +81,15 @@ void SysTick_Handler(void) {
 
 void ADC0_1_IRQHandler(void)
 {
-	if (adc_interrupt_flag_get(ADC0, ADC_INT_FLAG_EOIC) == SET) {
+	if (adc_interrupt_flag_get(ADC0, ADC_INT_FLAG_EOIC) == SET) {//phase I samples
     	/* clear the ADC flag */
     	adc_interrupt_flag_clear(ADC0, ADC_INT_FLAG_EOIC);
 		mc_phase_current_irq();
 	}
+	if (adc_flag_get(ADC1, ADC_FLAG_EOIC) == SET) {//vbus I sampes
+    	/* clear the ADC flag */
+    	adc_flag_clear(ADC1, ADC_FLAG_EOIC);
+	}	
 }
 
 

Applications/foc/foc_core.c

@@ -1,5 +1,6 @@
 #include "libs/task.h"
 #include "bsp/pwm.h"
+#include "bsp/adc.h"
 #include "foc_core.h"
 #include "foc_api.h"
 #include "foc_stm.h"
@@ -226,6 +227,9 @@ void mc_phase_current_irq(void) {
 	if (g_foc.current_samp.is_calibrating_offset) {
 		phase_current_offset(&g_foc.current_samp);
 		return;
+	}else if (adc_is_trigged_vbus()) {
+		phase_Rds_calibrate(&g_foc.current_samp);
+		return;
 	}
 	FOC_Fast_Task(&g_foc);
 }

Applications/foc/foc_type.h

@@ -64,6 +64,13 @@ typedef enum {
 	FOC_MODE_CLOSE_LOOP,         //电流速度双闭环模式
 }control_mode_t;
 
+typedef enum {
+	INVERT_NO,
+	INVERT_A,
+	INVERT_B,
+	INVERT_C
+}phase_invert_t;
+
 typedef struct current_sample {
 	u32   adc_offset_a;
 	u32   adc_offset_b;
@@ -72,11 +79,11 @@ typedef struct current_sample {
 	float Ia;
 	float Ib;
 	float Ic;
-	u32   adc_vbus_ia;
-	u32   adc_vbus_ib;
-	u32   adc_vbus_ic;
+	float   Rds_a; //各相MOS的DS极内阻，在线测量. 单位欧
+	float   Rds_b;
+	float   Rds_c;
+	phase_invert_t vbus_i_invert;
 	u8    sector;
-	u32   adc_inject_flags;
 	volatile int   offset_sample_count;
 	bool  is_calibrating_offset;
 }current_samp_t;

Applications/foc/phase_current.c

@@ -3,18 +3,28 @@
 
 #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);
+#define Rvbus  0.0005f
+#define Gvbus (13.1f) //母线电流的运放 
+#define Rds_Defualt  0.00263f//欧
+#define Gmos  (1.7f)//mos 电流的运放
+
+#define Lower_Pass_p 0.2f
+
+#define VBUS_VOL(adc) ((adc) * 3.3f / 65535.0f / Gvbus)
+#define MOSds_VOL(adc) ((adc) * 3.3f / 65535.0f / Gmos)
+
+#define current_i(v, r) ((v)/(r))
+
+/* Ids 通过母线采样电阻测量 */
+static __inline__ void _calc_mos_rds(u32 Vds, u32 Ids, float *dest) {
+	*dest = (*dest) * (1.0f - Lower_Pass_p) + Vds/Ids * Lower_Pass_p;
 }
 
 void phase_current_init(current_samp_t *cs) {
 	cs->offset_sample_count = NB_OFFSET_SAMPLES;
+	cs->Rds_a = Rds_Defualt;
+	cs->Rds_b = Rds_Defualt;
+	cs->Rds_c = Rds_Defualt;
 }
 
 
@@ -47,28 +57,26 @@ void phase_current_offset(current_samp_t *cs) {
 
 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->Ib = current_i(MOSds_VOL((int)phase_current1 - (int)cs->adc_offset_b), cs->Rds_b);
+			cs->Ia = current_i(MOSds_VOL((int)phase_current2 - (int)cs->adc_offset_a), cs->Rds_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);
+			cs->Ic = current_i(MOSds_VOL((int)phase_current1 - (int)cs->adc_offset_c), cs->Rds_c);
+			cs->Ib = current_i(MOSds_VOL((int)phase_current2 - (int)cs->adc_offset_b), cs->Rds_b);
+			cs->Ia = -(cs->Ib + cs->Ic);		
 		}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->Ia = current_i(MOSds_VOL((int)phase_current1 - (int)cs->adc_offset_a), cs->Rds_a);
+			cs->Ic = current_i(MOSds_VOL((int)phase_current2 - (int)cs->adc_offset_c), cs->Rds_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;
@@ -79,6 +87,24 @@ void phase_current_sample(current_samp_t *cs){
 
 }
 
+/* 校准mos的 drain-source 内阻 */
+void phase_Rds_calibrate(current_samp_t *cs) {	
+	if (cs->vbus_i_invert != INVERT_NO) {
+		u32 adcm = adc_phase_read_1(cs->sector);
+		u32 adcv = adc_vbus_read_1(cs->sector);
+		float vbus_i = VBUS_VOL(adcv)/Rvbus;
+		if (cs->vbus_i_invert == INVERT_A) {
+			_calc_mos_rds(adcm, vbus_i, &cs->Rds_a);
+		}else if (cs->vbus_i_invert == INVERT_B) {
+			_calc_mos_rds(adcm, vbus_i, &cs->Rds_b);
+		}else if (cs->vbus_i_invert == INVERT_C) {
+			_calc_mos_rds(adcm, vbus_i, &cs->Rds_c);
+		}
+		cs->vbus_i_invert = INVERT_NO;		
+	}
+
+	adc_config_trigger(ADC_TRIGGER_PHASE);
+}
 
 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;	
@@ -100,7 +126,10 @@ u32 get_phase_sample_point(current_samp_t *cs, phase_time_t *time, u8 sector){
 			return sample_point;
 		}
 	}
+}
 
+u32 get_vbus_sample_point(current_samp_t *cs, phase_time_t *time){
+	return 0;
 }
 
 void phase_current_adc_triger(current_samp_t *cs){

Applications/foc/phase_current.h

@@ -4,7 +4,9 @@
 void phase_current_init(current_samp_t *cs);
 void phase_current_sample(current_samp_t *cs);
 void phase_current_offset(current_samp_t *cs);
+void phase_Rds_calibrate(current_samp_t *cs);
 u32 get_phase_sample_point(current_samp_t *cs, phase_time_t *time, u8 sector);
+u32 get_vbus_sample_point(current_samp_t *cs, phase_time_t *time);
 void phase_current_adc_triger(current_samp_t *cs);
 #endif /* _PHASE_CURRENT_H__ */