|
|
@@ -1,5 +1,6 @@
|
|
|
+#include <math.h>
|
|
|
#include "bsp/adc.h"
|
|
|
-#include "foc_type.h"
|
|
|
+#include "phase_current.h"
|
|
|
#include "libs/utils.h"
|
|
|
#include "libs/logger.h"
|
|
|
|
|
|
@@ -7,9 +8,9 @@
|
|
|
|
|
|
#define Rvbus 0.0005f
|
|
|
#define Gvbus (13.1f) //母线电流的运放
|
|
|
-#define Rds_Defualt 0.005f//欧
|
|
|
+#define Rds_Defualt 0.0025f//欧
|
|
|
#define Gmos (1.7f)//mos 电流的运放
|
|
|
-#define Sample_R Rds_Defualt
|
|
|
+#define Sample_R Rvbus
|
|
|
#define Lower_Pass_p 0.2f
|
|
|
|
|
|
#define VBUS_VOL(adc) (((float)(adc)) * 3.3f / 4096.0f / Gvbus)
|
|
|
@@ -22,8 +23,8 @@ void phase_current_init(current_samp_t *cs) {
|
|
|
}
|
|
|
|
|
|
|
|
|
+#if SHUNT_NUM==THREE_SHUNTS_SAMPLE
|
|
|
void phase_current_offset(current_samp_t *cs) {
|
|
|
-#if 1
|
|
|
s32 phase_current1, phase_current2;
|
|
|
adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
|
|
|
if (cs->offset_sample_count > 0) {
|
|
|
@@ -43,65 +44,593 @@ void phase_current_offset(current_samp_t *cs) {
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-#else
|
|
|
- cs->adc_offset_a = adc_sample_regular_channel(U_PHASE_I_CHAN, 16);
|
|
|
- cs->adc_offset_a = adc_sample_regular_channel(U_PHASE_I_CHAN, 64);
|
|
|
-
|
|
|
- cs->adc_offset_b = adc_sample_regular_channel(V_PHASE_I_CHAN, 64);
|
|
|
- cs->adc_offset_c = adc_sample_regular_channel(W_PHASE_I_CHAN, 64);
|
|
|
-#endif
|
|
|
}
|
|
|
|
|
|
void phase_current_sample(current_samp_t *cs){
|
|
|
s32 phase_current1, phase_current2;
|
|
|
adc_phase_current_read(cs->sector, &phase_current1, &phase_current2);
|
|
|
- if (cs->sector == SECTOR_1 || cs->sector == SECTOR_2) {
|
|
|
+ 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), Sample_R);
|
|
|
- cs->Ia = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_a), Sample_R);
|
|
|
+ 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);
|
|
|
cs->Ic = -(cs->Ia + cs->Ib);
|
|
|
- }else if (cs->sector == SECTOR_3 || cs->sector == SECTOR_4) {
|
|
|
+ }else if (cs->sector == SECTOR_1 || cs->sector == SECTOR_6) {
|
|
|
/* Current on Phase A is not accessible */
|
|
|
/* Ib = PhaseBOffset - ADC converted value) */
|
|
|
- cs->Ic = current_i(MOSds_VOL(phase_current1 - cs->adc_offset_c), Sample_R);
|
|
|
- cs->Ib = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_b), Sample_R);
|
|
|
+ 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);
|
|
|
cs->Ia = -(cs->Ib + cs->Ic);
|
|
|
- }else if (cs->sector == SECTOR_5 || cs->sector == SECTOR_6) {
|
|
|
+ }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), Sample_R);
|
|
|
- cs->Ic = current_i(MOSds_VOL(phase_current2 - cs->adc_offset_c), Sample_R);
|
|
|
+ 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);
|
|
|
cs->Ib = -(cs->Ia + cs->Ic);
|
|
|
}
|
|
|
- //log_chan_value(1, (int)(cs->Ia * 1000));
|
|
|
+ {
|
|
|
+ static int count = 0;
|
|
|
+ if (count++ % 3 == 0) {
|
|
|
+ log_chan_value(1, (int)(cs->Ia * 1000));
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
}
|
|
|
|
|
|
|
|
|
-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;
|
|
|
+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;
|
|
|
- //duty > deadtime + max(Rise time, Noise time)
|
|
|
+ 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;
|
|
|
+ }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;
|
|
|
+ }else {
|
|
|
+ time->Samp_p1 = Samp_p;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }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;
|
|
|
+ }else {
|
|
|
+ time->Samp_p1 = Samp_p;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }else {
|
|
|
+ //not reached there
|
|
|
+ sys_debug("ERROR!!!!!\n");
|
|
|
+ }
|
|
|
+#else
|
|
|
if (low_side_low_duty > (TDead + MAX(TRise, TNoise))) {
|
|
|
cs->sector = SECTOR_1;
|
|
|
- return FOC_PWM_Half_Period - 1;
|
|
|
+ time->Samp_p1 = time->low + MAX(TRise, TNoise);
|
|
|
}else {
|
|
|
- log_chan_value(2, 111);
|
|
|
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;
|
|
|
+ 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;
|
|
|
}
|
|
|
- return sample_point;
|
|
|
+ time->Samp_p1 = sample_point;
|
|
|
}
|
|
|
}
|
|
|
+#endif
|
|
|
+}
|
|
|
+#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();
|
huhui