加入三相电流不平衡算法，目前只能通过检测负序分量，比如相线螺丝松动等情况

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -13,6 +13,7 @@
 #include "foc/core/F_Calc.h"
 #include "foc/samples.h"
 #include "foc/limit.h"
+#include "foc/mc_error.h"
 #include "app/nv_storage.h"
 #include "bsp/bsp_driver.h"
 #include "libs/logger.h"
@@ -225,7 +226,7 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.in.s_dqAngle     = INVALID_ANGLE;
 	gFoc_Ctrl.in.b_fwEnable = nv_get_foc_params()->n_FwEnable;
 	gFoc_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxDCVol;//(CONFIG_RATED_DC_VOL);
-	
+	gFoc_Ctrl.in.s_angleLast = INVALID_ANGLE;
 	gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
 	gFoc_Ctrl.out.f_vdqRation = 0;
 
@@ -335,6 +336,82 @@ static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
 	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, iDC, 0.005f);
 }
 
+#define CONFIG_PEAK_CNT 3 //计算经过的电周期内的最大值(peak 峰值)
+#define CONFIG_PHASE_UNBALANCE_THROLD 10.0F
+#define CONFIG_PHASE_UNBALANCE_R      0.1F
+static float phase_unbalance_r = 0.0f;
+static float phase_a_max, phase_b_max, phase_c_max;
+static __INLINE void PMSM_FOC_Phase_Unbalance(void) {
+	static u32 _cycle_cnt = 0, _last_mod_cnt = 0;
+	static float a_max = 0, b_max = 0, c_max = 0;
+	static u32 _unbalance_cnt = 0;
+	float lowpass = gFoc_Ctrl.in.s_motVelDegreePers * FOC_CTRL_US / 2.0f;
+	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], lowpass);
+	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], lowpass);
+	gFoc_Ctrl.in.s_iABCFilter[2] = -(gFoc_Ctrl.in.s_iABCFilter[0] + gFoc_Ctrl.in.s_iABCFilter[1]);
+	if ((gFoc_Ctrl.in.s_angleLast == INVALID_ANGLE) || (gFoc_Ctrl.in.s_motVelDegreePers < 100)) {
+		gFoc_Ctrl.in.s_angleLast = gFoc_Ctrl.in.s_motAngle;
+		a_max = b_max = c_max = 0;
+		_unbalance_cnt = 0;
+		_cycle_cnt = 0;
+		_last_mod_cnt = 0;
+		phase_unbalance_r = 0;
+		return;
+	}
+	float delta_angle = gFoc_Ctrl.in.s_motAngle - gFoc_Ctrl.in.s_angleLast;
+	if (delta_angle > 200 || delta_angle < -200) { //one cycle
+		_cycle_cnt ++;
+	}
+	gFoc_Ctrl.in.s_angleLast = gFoc_Ctrl.in.s_motAngle;
+	u32 mod_cnt = _cycle_cnt % CONFIG_PEAK_CNT;
+	bool trigger = false;
+	if ((mod_cnt == 0) && (_last_mod_cnt != mod_cnt)) {
+		trigger = true;
+	}
+	_last_mod_cnt = mod_cnt;
+
+	a_max = MAX(a_max, gFoc_Ctrl.in.s_iABCFilter[0] * (1.0f / 0.707f));
+	b_max = MAX(b_max, gFoc_Ctrl.in.s_iABCFilter[1] * (1.0f / 0.707f));
+	c_max = MAX(c_max, gFoc_Ctrl.in.s_iABCFilter[2] * (1.0f / 0.707f));
+	if (trigger) { //经过CONFIG_PEAK_CNT个周期，已经得到peak值
+		float i_min = 1000.0f, i_max = 0;
+		if (a_max > i_max) {
+			i_max = a_max;
+		}
+		if (a_max < i_min) {
+			i_min = a_max;
+		}
+		if (b_max > i_max) {
+			i_max = b_max;
+		}
+		if (b_max < i_min) {
+			i_min = b_max;
+		}
+		if (c_max > i_max) {
+			i_max = c_max;
+		}
+		if (c_max < i_min) {
+			i_min = c_max;
+		}
+		float unbalance_r = (i_max - i_min - CONFIG_PHASE_UNBALANCE_THROLD)/(i_max + 1e-8f);
+		if (unbalance_r >= CONFIG_PHASE_UNBALANCE_R) {
+			if (_unbalance_cnt++ >= 500) {
+				if (mc_set_critical_error(FOC_CRIT_PHASE_UNBalance_Err)) {
+					mc_crit_err_add(FOC_CRIT_PHASE_UNBalance_Err, (s16)i_max, (s16)i_min);
+				}
+			}
+		}else {
+			_unbalance_cnt = 0;
+		}
+		phase_unbalance_r = unbalance_r;
+		phase_a_max = a_max;
+		phase_b_max = b_max;
+		phase_c_max = c_max;
+		a_max = b_max = c_max = 0;
+	}
+}
+
+
 //#define UPDATE_Lq_By_iq   /* Q轴电感 通过Iq电流补偿 */
 #define Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define Volvec_Delay_Comp_Start_Vel 500 // rpm
@@ -370,6 +447,9 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 	gFoc_Ctrl.in.s_motVelocity = enc_vel;
 	LowPass_Filter(gFoc_Ctrl.in.s_motVelocityFiltered, gFoc_Ctrl.in.s_motVelocity, 0.01f);
 	gFoc_Ctrl.in.s_motVelDegreePers = gFoc_Ctrl.in.s_motVelocityFiltered / 30.0f * PI * gFoc_Ctrl.params.n_poles;
+
+	PMSM_FOC_Phase_Unbalance();
+
 #ifdef CONFIG_DQ_STEP_RESPONSE
 	gFoc_Ctrl.in.s_motAngle = 0;
 #endif
@@ -425,8 +505,6 @@ static u32 PMSM_FOC_Debug_Task(void *p) {
 static __INLINE float id_feedforward(float eW) {
 #ifdef CONFIG_CURRENT_LOOP_DECOUPE
 	return -(gFoc_Ctrl.params.lq * gFoc_Ctrl.out.s_RealIdq.q * eW);
-	//gFoc_Ctrl.in.s_targetVdq.d += -(gFoc_Ctrl.params.lq * gFoc_Ctrl.out.s_RealIdq.q * eW);
-	//gFoc_Ctrl.in.s_targetVdq.d = fclamp(gFoc_Ctrl.in.s_targetVdq.d, gFoc_Ctrl.pi_id.min, gFoc_Ctrl.pi_id.max);
 #else
 	return 0;
 #endif
@@ -435,8 +513,6 @@ static __INLINE float id_feedforward(float eW) {
 static __INLINE float iq_feedforward(float eW) {
 #ifdef CONFIG_CURRENT_LOOP_DECOUPE
 	return (gFoc_Ctrl.params.ld * gFoc_Ctrl.out.s_RealIdq.d + gFoc_Ctrl.params.flux) * eW;
-	//gFoc_Ctrl.in.s_targetVdq.q += (gFoc_Ctrl.params.ld * gFoc_Ctrl.out.s_RealIdq.d + gFoc_Ctrl.params.flux) * eW;
-	//gFoc_Ctrl.in.s_targetVdq.q = fclamp(gFoc_Ctrl.in.s_targetVdq.q, gFoc_Ctrl.pi_iq.min, gFoc_Ctrl.pi_iq.max);
 #else
 	return 0;
 #endif
@@ -515,7 +591,7 @@ bool PMSM_FOC_Schedule(void) {
 void PMSM_FOC_LogDebug(void) {
 	sys_debug("DC curr %f --- %f, - %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_CalciDC2, gFoc_Ctrl.hwLim.s_iDCMax);
 	sys_debug("%s\n", gFoc_Ctrl.out.empty_load?"NoLoad Running":"Load Runing");
-	sys_debug("cruise vel: %d\n", (s16)gFoc_Ctrl.in.s_cruiseRPM);
+	sys_debug("unbalance: %f, %f, %f, %f\n", phase_unbalance_r, phase_a_max, phase_b_max, phase_c_max);
 }
 
 /*called in media task */

Applications/foc/core/PMSM_FOC_Core.h

@@ -67,7 +67,8 @@ typedef enum {
 	FOC_CRIT_IDC_OV,
 	FOC_CRIT_THRO_Err,
 	FOC_CRIT_ENC_AB_Err,
-	FOC_CRIT_Vol_HW_Err,
+	FOC_CRIT_Vol_HW_Err, //17
+	FOC_CRIT_PHASE_UNBalance_Err, /* 三相不平衡错误，比如相线螺丝松了 */
 	FOC_CRIT_Err_Max = 32,
 }FOC_CritiCal_Ebit_t;
 
@@ -89,6 +90,7 @@ typedef struct {
 	float   s_vABC[3];
 	float   s_motVelocity;   //from hall or encoder
 	float 	s_motAngle; //from hall or encoder
+	float   s_angleLast;
 	float   s_targetRPM;
 	float   s_cruiseRPM;
 	e_Ramp  cruiseRpmRamp;