smo加入电流电压滞后一个控制器周期的相位补偿

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

Applications/app/app.c

@@ -127,8 +127,8 @@ static u32 _app_report_task(void *p) {
 	can_report_phase_current(0x45);
 	if (++loop % 10 == 0) {
 		//sys_debug("modulation %f, %f\n", PMSM_FOC_Get()->out.f_vdqRation, PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation);
-		//sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
-		//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("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
+		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("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
 		sys_debug("acc vol %d\n", get_acc_vol());
 		sys_debug("throttle %f\n", get_throttle_float());
@@ -155,7 +155,6 @@ static void plot_smo_angle(void) {
 	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);
-
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {

Applications/foc/core/smo_observer.c

@@ -112,7 +112,8 @@ static void smo_arctan(void) {
 		smo.pll.out = smo.est_rad_pers;
 	}
 	smo.est_rad_pers_filted = smo.est_rad_pers;
-	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc);
+	/*低通滤波有相位滞后，需要补偿，同时电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
+	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc) + smo.est_rad_pers_filted * smo.ts;
 	angle_clamp(smo.est_angle_out);
 	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
 	if (smo.est_rpm > CONFIG_MAX_MOT_RPM) {
@@ -146,7 +147,9 @@ static void smo_pll(void) {
 	}else if (smo.est_rpm < 0) {
 		smo.est_rpm = 0;
 	}
-	smo.est_angle_out = smo.est_angle;
+	/* 电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
+	smo.est_angle_out = smo.est_angle + smo.est_rad_pers_filted * smo.ts;
+	angle_clamp(smo.est_angle_out);
 }
 #endif