相电流加入自适应滤波，截止频率为当前电频率的4倍

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

Applications/app/app.c

@@ -102,6 +102,7 @@ static u32 _app_report_task(void *p) {
 		sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
 		sys_debug("acc vol %d, mos2 %d\n", get_acc_vol(), get_mos_temp2());
 		sys_debug("throttle %f\n", get_throttle_float());
+		sys_debug("curr lfp %f\n", PMSM_FOC_Get()->params.n_PhaseFilterCeof);
 		//sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
 		encoder_log();
 		//err_code_log();

Applications/bsp/board_mc100_v1.h

@@ -26,9 +26,9 @@
 #define CONFIG_UNDER_VOL_DC_CURR 15.0F
 #define CONFIG_MAX_FW_D_CURR     100.0F //d轴最大的退磁电流
 
-#define CONFIG_CURR_LP_CUT_FREQ (2500.0F)
+#define CONFIG_CURR_LP_WC (600.0F)
 
-#define CONFIG_CURR_LP_PARAM (CONFIG_CURR_LP_CUT_FREQ*2*3.14F/(float)FOC_PWM_FS)
+#define CONFIG_CURR_LP_CEOF (CONFIG_CURR_LP_WC*2*3.14F/(float)FOC_PWM_FS)
 
 #define CONFIG_96V_MODE_VOL (55.0F)
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -215,7 +215,7 @@ void PMSM_FOC_CoreInit(void) {
 	}
 	
 	gFoc_Ctrl.params.n_modulation = CONFIG_SVM_MODULATION;//SVM_Modulation;
-	gFoc_Ctrl.params.n_PhaseFilterCeof = (CONFIG_CURR_LP_PARAM>1.0f?1.0f:CONFIG_CURR_LP_PARAM);
+	gFoc_Ctrl.params.n_PhaseFilterCeof = CONFIG_CURR_LP_CEOF;
 	gFoc_Ctrl.params.n_poles = nv_get_motor_params()->poles;//MOTOR_POLES;
 	gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
 	gFoc_Ctrl.in.b_fwEnable = nv_get_foc_params()->n_FwEnable;
@@ -239,17 +239,13 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.plot_type = Plot_Phase_curr;
 }
 
-//#define PHASE_LFP_FIR
-//#define PHASE_LFP
+//#define CONFIG_USER_PHASE_LFP
 static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	AB_t iAB;
-#ifdef PHASE_LFP	
-	float *iabc = gFoc_Ctrl.in.s_iABCComp;
-#elif defined PHASE_LFP_FIR
-	float *iabc = gFoc_Ctrl.in.s_iABCFilter;
-#else
-	float *iabc = gFoc_Ctrl.in.s_iABC;
+#ifdef CONFIG_USER_PHASE_LFP
+	float *iabc_filted = gFoc_Ctrl.in.s_iABCComp;
 #endif
+	float *iabc = gFoc_Ctrl.in.s_iABC;
 
 	if (!gFoc_Ctrl.in.b_MTPA_calibrate && (gFoc_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
 		gFoc_Ctrl.in.s_motAngle = gFoc_Ctrl.in.s_manualAngle;
@@ -267,24 +263,39 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	//sample current
 	phase_current_get(gFoc_Ctrl.in.s_iABC);
 	get_phase_vols(gFoc_Ctrl.in.s_vABC);
-
-#ifdef PHASE_LFP
-	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], gFoc_Ctrl.params.n_PhaseFilterCeof);
-	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], gFoc_Ctrl.params.n_PhaseFilterCeof);
-	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[2], gFoc_Ctrl.in.s_iABC[2], gFoc_Ctrl.params.n_PhaseFilterCeof);
-	float comp = lp_compestion(gFoc_Ctrl.in.s_motRPM/60.0f*4.0f, CONFIG_CURR_LP_CUT_FREQ);
-	gFoc_Ctrl.in.s_iABCComp[0] = gFoc_Ctrl.in.s_iABCFilter[0] * comp;
-	gFoc_Ctrl.in.s_iABCComp[1] = gFoc_Ctrl.in.s_iABCFilter[1] * comp;
-	gFoc_Ctrl.in.s_iABCComp[2] = gFoc_Ctrl.in.s_iABCFilter[2] * comp;
-#elif defined PHASE_LFP_FIR
-	gFoc_Ctrl.in.s_iABCFilter[1] = Fir_Filter(&phase1, gFoc_Ctrl.in.s_iABC[1]);
-	gFoc_Ctrl.in.s_iABCFilter[2] = Fir_Filter(&phase2, gFoc_Ctrl.in.s_iABC[2]);
-	gFoc_Ctrl.in.s_iABCFilter[0] = -(gFoc_Ctrl.in.s_iABCFilter[1] + gFoc_Ctrl.in.s_iABCFilter[2]);
-#endif
+	
 	Clark(iabc[0], iabc[1], iabc[2], &iAB);
 #ifdef CONFIG_SMO_OBSERVER
 	gFoc_Ctrl.in.s_smoAngle = smo_observer_update(gFoc_Ctrl.out.s_OutVAB.a, gFoc_Ctrl.out.s_OutVAB.b, iAB.a, iAB.b);
 #endif
+
+#ifdef CONFIG_USER_PHASE_LFP
+	float e_freq = gFoc_Ctrl.in.s_motRPM / 60.0f * gFoc_Ctrl.params.n_poles;
+	float lpf_wc;
+	float lpf_comp = 1.0f;
+	if (e_freq <= 150.0f) {
+		lpf_wc = 600.0f;
+		if (e_freq >= 100) {
+			lpf_comp = 1.02f;
+		}else if (e_freq >= 50) {
+			lpf_comp = 1.01;
+		}
+	}else {
+		lpf_wc = e_freq * 4.0f;
+		lpf_comp = 1.03f;
+	}
+	gFoc_Ctrl.params.n_PhaseFilterCeof = (lpf_wc*2*M_PI*FOC_CTRL_US);
+	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], gFoc_Ctrl.params.n_PhaseFilterCeof);
+	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], gFoc_Ctrl.params.n_PhaseFilterCeof);
+	gFoc_Ctrl.in.s_iABCFilter[2] = - (gFoc_Ctrl.in.s_iABCFilter[0] + gFoc_Ctrl.in.s_iABCFilter[1]);
+	
+	gFoc_Ctrl.in.s_iABCComp[0] = gFoc_Ctrl.in.s_iABCFilter[0] * lpf_comp;
+	gFoc_Ctrl.in.s_iABCComp[1] = gFoc_Ctrl.in.s_iABCFilter[1] * lpf_comp;
+	gFoc_Ctrl.in.s_iABCComp[2] = gFoc_Ctrl.in.s_iABCFilter[2] * lpf_comp;
+
+	Clark(iabc_filted[0], iabc_filted[1], iabc_filted[2], &iAB);
+#endif
+
 	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);

Applications/math/fast_math.c

@@ -275,31 +275,3 @@ void TD_Init(TD_t *td, float wc, float DT) {
 	td->time = wc *wc;
 }
 
-#if 0
-float lp_compestion(float lp, float w, float sin, float cos) {
-	float H = 1.0f/sqrtf(1.0f + SQ(w/(float)CONFIG_CURR_LP_CUT_FREQ));
-	float delta_angle =  -fast_arctan2(w/(float)CONFIG_CURR_LP_CUT_FREQ)/(2*PI) * 360.0f;
-	float sin1, cos1;
-	SinCos_Lut(delta_angle, &sin1, &cos1);
-
-	float value = (sin/(sin*cos1 + cos*sin1)) * (lp / H);
-	return value;
-}
-#endif
-#if 1
-
-#else
-float lp_compestion(float lp, float w, float wc, float angle) {
-	static float A = 0.0f;
-	float H = 1.0f/sqrtf(1.0f + SQ(w/wc));
-	float delta_angle = -fast_arctan2(w, wc)/(2*PI) * 360.0f;
-	float sin1, cos1;
-	SinCos_Lut(delta_angle + angle, &sin1, &cos1);
-	if (sin1 == 0.0f) {
-		return A;
-	}
-	A = lp/(H * sin1);
-	return A;
-}
-
-#endif