解决滑膜观测器模型错误

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

Applications/foc/core/smo_observer.c

@@ -26,9 +26,9 @@ 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.ldq_diff_dm = (smo.motor_ld-smo.motor_lq)/smo.motor_lq*smo.motor_ld;
+	smo.ldq_diff_dm = (smo.motor_ld-smo.motor_lq);
 	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);
@@ -61,7 +61,7 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 	smo.est_eAlpha_Filted = do_lpf(smo.est_eAlpha_Filted, smo.est_eAlpha, smo.lpf_ceof);
 
 	/* est beta back emf */
-	float calc_beta = (uBeta - smo.IBeta_hat*smo.motor_r - smo.est_eBeta + est_ab * smo.Ialpha_hat) * smo.ld_inv;
+	float calc_beta = (uBeta - smo.IBeta_hat*smo.motor_r - smo.est_eBeta + est_ab * smo.Ialpha_hat) * smo.lq_inv;
 	smo.IBeta_hat += calc_beta * smo.ts; //积分
 	
 	float err_iBeta = smo.IBeta_hat - iBeta;

Applications/foc/core/smo_observer.h

@@ -35,8 +35,9 @@ typedef struct {
 	float motor_lq;
 	float motor_poles;
 
-	float ldq_diff_dm; //(lq-ld)/lq*ld
+	float ldq_diff_dm; //(lq-ld)
 	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);