update smo, 计算公司错误

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

Applications/foc/core/smo_observer.c

@@ -26,8 +26,6 @@ void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta)
 	smo.motor_lq = nv_get_motor_params()->lq;
 	smo.motor_poles = nv_get_motor_params()->poles;
 	smo.dir_ccw = true;
-	smo.ld_inv = 1.0f / smo.motor_ld;
-	smo.lq_inv = 1.0f / smo.motor_lq;
 	smo.pll.DT = smo.ts;
 	smo.pll.kp = pll_bandwith * 2;
 	smo.pll.ki = 0.25f * SQ(smo.pll.kp);
@@ -58,11 +56,11 @@ static __INLINE float line_func(float err, float max, float slide) {
 }
 
 static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
-	float est_ab = smo.est_rad_pers_filted;
+	float est_ab = smo.est_rad_pers_filted * (smo.motor_ld - smo.motor_lq);
 
 	/* est alpha back emf */
 	float Ialpha_hat = smo.Ialpha_hat;
-	float calc_alpha = (uAlpha - smo.Ialpha_hat*smo.motor_r - smo.est_eAlpha ) * smo.lq_inv - est_ab * smo.IBeta_hat;
+	float calc_alpha = (uAlpha - smo.Ialpha_hat*smo.motor_r - smo.est_eAlpha - est_ab * smo.IBeta_hat) / smo.motor_ld ;
 	smo.Ialpha_hat += calc_alpha * smo.ts; //积分
 
 	float err_iAlpha = smo.Ialpha_hat - iAlpha;
@@ -74,7 +72,7 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 	smo.est_eAlpha_Filted = smo.est_eAlpha;
 #endif
 	/* est beta back emf */
-	float calc_beta = (uBeta - smo.IBeta_hat*smo.motor_r - smo.est_eBeta) * smo.lq_inv  + est_ab * Ialpha_hat;
+	float calc_beta = (uBeta - smo.IBeta_hat*smo.motor_r - smo.est_eBeta + est_ab * Ialpha_hat) / smo.motor_ld;
 	smo.IBeta_hat += calc_beta * smo.ts; //积分
 	
 	float err_iBeta = smo.IBeta_hat - iBeta;

Applications/foc/core/smo_observer.h

@@ -34,9 +34,6 @@ typedef struct {
 	float motor_ld;
 	float motor_lq;
 	float motor_poles;
-
-	float ld_inv; //1/ld
-	float lq_inv;
 }smo_observer_t;
 
 void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta);