加入逆变器死区补偿，采用线性补偿方式，相电流滤波通过dq滤波，再变化到abc

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

Applications/bsp/gd32/board_mc105_v3.h

@@ -53,13 +53,16 @@
 #define HW_RISE_TIME_NS  150u
 #define HW_NOISE_TIME_NS 300u
 
-#define TDead NS_2_TCLK(HW_DEAD_TIME_NS + PWM_DEAD_TIME_NS)/* ����ʱ�� */ 
+#define CONFIG_HW_DeadTime NS_2_TCLK(HW_DEAD_TIME_NS + PWM_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) *2 * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC ����ʱ�� */
 #define TSampleMIN (TDead + TRise + TADC) //采样需要的总时间
 #define TSampleBefore (TDead + TRise) //采样开始前需要等待的时间
 
+#define CONFIG_START_LINE_DTC_CURRENT 5.0F /* 死区补偿开始电流，取决于电流噪声 */
+#define COMFIG_END_LINE_DTC_CURRENT   15.0F
+
 #define ADC_REFERENCE_VOLTAGE  (3.3F)
 #define ADC_FULL_MAX          (4095.0F)
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -39,6 +39,12 @@ static __INLINE void RevPark(DQ_t *dq, float angle, AB_t *alpha_beta) {
 	alpha_beta->b = dq->d * s + dq->q * c;
 }
 
+static __INLINE void RevClark(AB_t *alpha_beta, float *ABC){
+	ABC[0] = alpha_beta->a;
+	ABC[1] = -alpha_beta->a * 0.5f + alpha_beta->b * SQRT3_BY_2;
+	ABC[2] = -alpha_beta->a * 0.5f - alpha_beta->b * SQRT3_BY_2;
+}
+
 static __INLINE void Clark(float A, float B, float C, AB_t *alpha_beta){
 	alpha_beta->a = A;
 	alpha_beta->b = ONE_BY_SQRT3 * (B - C);
@@ -246,6 +252,7 @@ void PMSM_FOC_CoreInit(void) {
 
 /* 通过三相电流重构母线电流，和单电阻采样正好相反，原理一致 */
 static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
+#if 0
 	float deadtime = (float)(NS_2_TCLK(PWM_DEAD_TIME_NS + HW_DEAD_TIME_NS))/(float)FOC_PWM_Half_Period;
 	float duty_pu[3];
 	duty_pu[0] = (float)gFoc_Ctrl.out.n_Duty[0] / (float)FOC_PWM_Half_Period;
@@ -334,6 +341,7 @@ static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
 		}
 	}
 	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, iDC, 0.005f);
+#endif
 }
 
 #define CONFIG_PEAK_CNT 3 //计算经过的电周期内的最大值(peak 峰值)
@@ -411,6 +419,58 @@ static __INLINE void PMSM_FOC_Phase_Unbalance(void) {
 	}
 }
 
+/* 死区补偿 */
+static  __INLINE void PMSM_FOC_DeadTime_Compensate(s32 PWM_Half_Period) {
+#ifdef CONFIG_START_LINE_DTC_CURRENT
+	float deadTime = (float)CONFIG_HW_DeadTime/2.0f;
+	s32 dutyDTCA = 0;
+	s32 dutyDTCB = 0;
+	s32 dutyDTCC = 0;
+	float r, delta;
+	float iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[0]);
+	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
+		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
+		r = delta / (COMFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
+		if (r > 1.0f) {
+			r = 1.0f;
+		}
+		if (gFoc_Ctrl.in.s_iABC_DT[0] < 0) {
+			r = -r;
+		}
+		dutyDTCA = (s32)(r * deadTime);
+	}
+	iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[1]);
+	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
+		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
+		r = delta / (COMFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
+		if (r > 1.0f) {
+			r = 1.0f;
+		}
+		if (gFoc_Ctrl.in.s_iABC_DT[1] < 0) {
+			r = -r;
+		}
+		dutyDTCB = (s32)(r * deadTime);
+	}
+	iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[2]);
+	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
+		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
+		r = delta / (COMFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
+		if (r > 1.0f) {
+			r = 1.0f;
+		}
+		if (gFoc_Ctrl.in.s_iABC_DT[2] < 0) {
+			r = -r;
+		}
+		dutyDTCC = (s32)(r * deadTime);
+	}
+	s32 dutyA = (s32)gFoc_Ctrl.out.n_Duty[0] + dutyDTCA;
+	s32 dutyB = (s32)gFoc_Ctrl.out.n_Duty[1] + dutyDTCB;
+	s32 dutyC = (s32)gFoc_Ctrl.out.n_Duty[2] + dutyDTCC;
+	gFoc_Ctrl.out.n_Duty[0] = sclamp(dutyA, 0, PWM_Half_Period);
+	gFoc_Ctrl.out.n_Duty[1] = sclamp(dutyB, 0, PWM_Half_Period);
+	gFoc_Ctrl.out.n_Duty[2] = sclamp(dutyC, 0, PWM_Half_Period);
+#endif
+}
 
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
 #define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
@@ -461,9 +521,14 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
 
 	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
+	float lowpass = gFoc_Ctrl.in.s_motVelDegreePers * FOC_CTRL_US * 2.0f;
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, lowpass);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, lowpass);
+
+	/* 使用低通后的dq电流重新变换得到abc电流 */
+	RevPark(&gFoc_Ctrl.out.s_FilterIdq, gFoc_Ctrl.in.s_motAngle, &iAB);
+	RevClark(&iAB, gFoc_Ctrl.in.s_iABC_DT);
 
-	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.004f);
-	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.004f);
 #ifdef CONFIG_Volvec_Delay_Comp
 	if (gFoc_Ctrl.in.s_motVelocityFiltered >= CONFIG_Volvec_Delay_Comp_Start_Vel) {
 		float next_angle = gFoc_Ctrl.in.s_motAngle + gFoc_Ctrl.in.s_motVelDegreePers / PI * 180.0f * (FOC_CTRL_US * 0.8f);
@@ -580,6 +645,8 @@ bool PMSM_FOC_Schedule(void) {
 	
 	SVM_Duty_Fix(&gFoc_Ctrl.out.s_OutVAB, gFoc_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &gFoc_Ctrl.out);
 
+	PMSM_FOC_DeadTime_Compensate((s32)FOC_PWM_Half_Period);
+
 	phase_current_point(&gFoc_Ctrl.out);
 	
 	pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);

Applications/foc/core/PMSM_FOC_Core.h

@@ -86,6 +86,7 @@ typedef struct {
 typedef struct {
 	float 	s_iABC[3];
 	float 	s_iABCFilter[3];
+	float   s_iABC_DT[3]; //abc phase current for deadtime compesition
 	float   s_vABC[3];
 	float   s_motVelocity;   //from hall or encoder
 	float 	s_motAngle; //from hall or encoder