加入母线电流的另一种计算方式

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

Applications/app/app.c

@@ -141,7 +141,7 @@ static u32 _app_report_task(void *p) {
 		sys_debug("> %f, %f\n", ladrc_observer_get()->Ealpha, ladrc_observer_get()->Ebeta);
 		encoder_log();
 		//sample_log();
-		///PMSM_FOC_LogDebug();
+		PMSM_FOC_LogDebug();
 		//eCtrl_debug_log();
 		//err_code_log();
 	}
@@ -150,11 +150,11 @@ static u32 _app_report_task(void *p) {
 int plot_type = 0;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_sensorless_angle();
-	float delta = smo_angle - PMSM_FOC_Get()->in.s_hallAngle;
+	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;
 	float s, c;
 	arm_sin_cos_f32(delta, &s, &c);
 	delta = fast_atan2(s, c)/PI*180.0f;
-	can_plot3(PMSM_FOC_Get()->in.s_hallAngle, smo_angle, delta);
+	can_plot3(PMSM_FOC_Get()->in.s_motAngle, smo_angle, delta);
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {

Applications/foc/core/PMSM_FOC_Core.c

@@ -241,6 +241,33 @@ void PMSM_FOC_CoreInit(void) {
 
 	gFoc_Ctrl.plot_type = Plot_None;
 }
+
+/* 通过三相电流重构母线电流，和单电阻采样正好相反，原理一致 */
+static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
+	float deadtime = (float)(NS_2_TCLK(PWM_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;
+	duty_pu[1] = (float)gFoc_Ctrl.out.n_Duty[1] / (float)FOC_PWM_Half_Period;
+	duty_pu[2] = (float)gFoc_Ctrl.out.n_Duty[2] / (float)FOC_PWM_Half_Period;
+	
+	float *iABC = gFoc_Ctrl.in.s_iABC;
+	float iDC;
+	if ((duty_pu[0] >= duty_pu[1]) && (duty_pu[1] >= duty_pu[2])) {
+		iDC = iABC[0] * MAX(duty_pu[0] - duty_pu[1] - deadtime, 0) + (iABC[0] + iABC[1]) * MAX(duty_pu[1] - duty_pu[2] - deadtime, 0);
+	}else if ((duty_pu[0] >= duty_pu[2]) && (duty_pu[2] >= duty_pu[1])) {
+		iDC = iABC[0] * MAX(duty_pu[0] - duty_pu[2] - deadtime, 0) + (iABC[0] + iABC[2]) * MAX(duty_pu[2] - duty_pu[1] - deadtime, 0);
+	}else if ((duty_pu[1] >= duty_pu[0]) && (duty_pu[0] >= duty_pu[2])) {
+		iDC = iABC[1] * MAX(duty_pu[1] - duty_pu[0] - deadtime, 0) + (iABC[1] + iABC[0]) * MAX(duty_pu[0] - duty_pu[2] - deadtime, 0);
+	}else if ((duty_pu[1] >= duty_pu[2]) && (duty_pu[2] >= duty_pu[0])) {
+		iDC = iABC[1] * MAX(duty_pu[1] - duty_pu[2] - deadtime, 0) + (iABC[1] + iABC[2]) * MAX(duty_pu[2] - duty_pu[0] - deadtime, 0);
+	}else if ((duty_pu[2] >= duty_pu[0]) && (duty_pu[0] >= duty_pu[1])) {
+		iDC = iABC[2] * MAX(duty_pu[2] - duty_pu[0] - deadtime, 0) + (iABC[2] + iABC[0]) * MAX(duty_pu[0] - duty_pu[1] - deadtime, 0);
+	}else { // duty_pu[2] >= duty_pu[1] && duty_pu[1] >= duty_pu[0]
+		iDC = iABC[2] * MAX(duty_pu[2] - duty_pu[1] - deadtime, 0) + (iABC[2] + iABC[1]) * MAX(duty_pu[1] - duty_pu[0] - deadtime, 0);
+	}
+	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, iDC, 0.01f);
+}
+
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
 //#define Volvec_Delay_Comp /* 电压矢量角度补偿 */
 static __INLINE float PMSM_FOC_Update_Input(void) {
@@ -250,6 +277,8 @@ static __INLINE float PMSM_FOC_Update_Input(void) {
 
 	phase_current_get(iabc);
 
+	PMSM_FOC_Calc_iDC_Fast();
+
 	Clark(iabc[0], iabc[1], iabc[2], &iAB);
 
 	foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a * 0.66667f, gFoc_Ctrl.out.s_OutVAB.b * 0.66667f, iAB.a, iAB.b);
@@ -265,16 +294,13 @@ static __INLINE float PMSM_FOC_Update_Input(void) {
 	
 	if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
 		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_manualAngle;
-		gFoc_Ctrl.in.s_hallAngle = enc_angle;
 	}else {
-		gFoc_Ctrl.in.s_hallAngle = enc_angle;
-		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_hallAngle;
+		gFoc_Ctrl.in.s_motAngle = enc_angle;
 	}
 	gFoc_Ctrl.in.s_motVelocity = enc_vel;
 	LowPass_Filter(gFoc_Ctrl.in.s_motRPMFilted, gFoc_Ctrl.in.s_motVelocity, 0.005f);
 	gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motVelocity / 30.0f * PI * gFoc_Ctrl.params.n_poles;
 #ifdef CONFIG_DQ_STEP_RESPONSE
-	gFoc_Ctrl.in.s_hallAngle = 0;
 	gFoc_Ctrl.in.s_motAngle = 0;
 #endif
 
@@ -407,7 +433,7 @@ void PMSM_FOC_Schedule(void) {
 }
 
 void PMSM_FOC_LogDebug(void) {
-	sys_debug("DC curr %f, Lq %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.params.lq);
+	sys_debug("DC curr %f --- %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_CalciDC2);
 }
 
 /*called in media task */

Applications/foc/core/PMSM_FOC_Core.h

@@ -87,7 +87,6 @@ typedef struct {
 	float   s_vABC[3];
 	float   s_motVelocity;   //from hall or encoder
 	float 	s_motAngle; //from hall or encoder
-	float 	s_hallAngle;//from hall or encoder
 	float   s_targetRPM;
 	float   s_cruiseRPM;
 	e_Ramp  cruiseRpmRamp;
@@ -159,6 +158,7 @@ typedef struct {
 	DQ_t  s_FilterIdq;
 	float s_FilteriDC;
 	float s_CalciDC;
+	float s_CalciDC2;
 	float s_RealCurrentFiltered;
 	float f_vdqRation;
 	float f_autohold_trq;