smo 拼写错误

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

Applications/app/app.c

@@ -101,8 +101,8 @@ static u32 _app_report_task(void *p) {
 		sys_debug("Fast: %d - %d, err: %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);
 		sys_debug("acc vol %d, mos2 %d\n", get_acc_vol(), get_mos_temp2());
 		sys_debug("throttle %f\n", get_throttle_float());
-		sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
-		//encoder_log();
+		//sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
+		encoder_log();
 		//err_code_log();
 	}
 	return 200;
@@ -113,7 +113,7 @@ static u32 _app_plot_task(void * args) {
 	//can_report_plot_values(0x45);
 	//can_plot3(PMSM_FOC_Get()->out.n_Duty[0], PMSM_FOC_Get()->out.n_Duty[1], PMSM_FOC_Get()->out.n_Duty[2]);
 	//can_plot2(PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation, PMSM_FOC_GetSpeed());
-	can_plot2(PMSM_FOC_Get()->in.s_targetTorque, PMSM_FOC_Get_Real_Torque()*SIGN(PMSM_FOC_Get()->out.s_FilterIdq.q));
+	can_plot2(PMSM_FOC_Get()->in.s_motAngle, PMSM_FOC_Get()->in.s_smoAngle + 360.0f);
 #if 0
 	if (!_mc_start) {
 		_mc_start = true;
@@ -126,7 +126,7 @@ static u32 _app_plot_task(void * args) {
 	}
 	PMSM_FOC_Set_Angle(_angle);
 #endif
-	return 50;
+	return 20;
 }
 static u32 _app_low_task(void *args) {
 	wdog_reload();

Applications/foc/core/PMSM_FOC_Core.c

@@ -288,7 +288,7 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
 
 #ifdef CONFIG_SMO_OBSERVER
-	smo_observer_update(gFoc_Ctrl.out.s_OutVAB.a, gFoc_Ctrl.out.s_OutVAB.b, iAB.a, iAB.b);
+	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
 }
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -82,6 +82,7 @@ typedef struct {
 	float   s_motRPM;   //from hall or encoder
 	float 	s_motAngle; //from hall or encoder
 	float 	s_hallAngle;//from hall or encoder
+	float   s_smoAngle;
 	float   s_targetRPM;
 	float   s_cruiseRPM;
 	e_Ramp  cruiseRpmRamp;

Applications/foc/core/smo_observer.c

@@ -15,6 +15,8 @@ void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta)
 	smo.pll_max_rad_pers = CONFIG_MAX_MOT_RPM/30.0f * M_PI;
 	smo.lpf_wc = lpf_wc;
 	smo.lpf_ceof = (lpf_wc*2*M_PI/(float)FOC_PWM_FS);
+	smo.Ksmo = Ksmo;
+	smo.Ksta = Ksta;
 	smo.motor_r = nv_get_motor_params()->r;
 	smo.motor_ld = nv_get_motor_params()->ld;
 	smo.motor_lq = nv_get_motor_params()->lq;
@@ -37,7 +39,7 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 
 	float err_iAlpha = smo.Ialpha_hat - iAlpha;
 	
-	smo.est_eAlpha = fclamp(err_iAlpha, -smo.Ksta, smo.Ksta * smo.Ksmo);
+	smo.est_eAlpha = fclamp(err_iAlpha, -smo.Ksta, smo.Ksta) * smo.Ksmo;
 	smo.est_eAlpha_Filted = do_lpf(smo.est_eAlpha_Filted, smo.est_eAlpha, smo.lpf_ceof);
 
 	/* est beta back emf */
@@ -46,7 +48,7 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 	
 	float err_iBeta = smo.IBeta_hat - iBeta;
 	
-	smo.est_eBeta = fclamp(err_iBeta, -smo.Ksta, smo.Ksta * smo.Ksmo);
+	smo.est_eBeta = fclamp(err_iBeta, -smo.Ksta, smo.Ksta) * smo.Ksmo;
 	smo.est_eBeta_Filted = do_lpf(smo.est_eBeta_Filted, smo.est_eBeta, smo.lpf_ceof);
 
 }
@@ -68,10 +70,9 @@ static void smo_pll(void) {
 	}
 	smo.est_rad_pers_filted = do_lpf(smo.est_rad_pers_filted, smo.est_rad_pers, smo.lpf_ceof); //对速度低通滤波
 	smo.est_angle += smo.ts * smo.est_rad_pers; //角速度积分
-
+	angle_clamp(smo.est_angle);
 	/* 对低通进行角度补偿 */
-	smo.est_angle_out = smo.est_angle + fast_arctan2(smo.est_rad_pers_filted, smo.bandwith*2*M_PI);
-	
+	smo.est_angle_out = smo.est_angle + fast_arctan2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
 	angle_clamp(smo.est_angle_out);
 }
 

Applications/foc/foc_config.h

@@ -61,7 +61,7 @@
 	#define CONFIG_SMO_PLL_BANDWITH 2000.0f
 	#define CONFIG_SMO_LFP_WC       100.0F
 	#define CONFIG_SMO_GAIN_K            4.0F
-	#define CONFIG_SMO_SIGMOID_MAX   80.0F
+	#define CONFIG_SMO_SIGMOID_MAX   40.0F
 #endif
 #endif /* _FOC_CONFIG_H__ */
 

Project/MC_yuanqu.uvoptx

@@ -1047,7 +1047,7 @@
 
   <Group>
     <GroupName>StartUp</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>