Changeset 1466 for applications/pmsm/simulator_zdenek/ekf_example/ekf_mm.cpp

Timestamp:

08/02/12 22:43:00 (12 years ago)

Author:

smidl

Message:

opraven test Ch

Files:

• applications/pmsm/simulator_zdenek/ekf_example/ekf_mm.cpp (modified) (4 diffs)

applications/pmsm/simulator_zdenek/ekf_example/ekf_mm.cpp

@@ -1 +1 @@
 #include "ekf_mm.h"
 #include "qmath.h"
+#include "stdio.h"
 
 
@@ -12 +13 @@
         //               q15              + q15*q13
         (E->x_est)[1]=(((int32)(E->x_est[1])<<15)+(int32)(E->cG)*(E->x_est[0]))>>15;
+        E->x_pred[1]=E->x_est[1];
+        E->x_pred[0]=E->x_est[0];
         
         mmultACh(E->PSI,E->Chf,E->PSICh,2,2);
@@ -36 +39 @@
         E->y_est[1]=((int32)(E->cA)*(E->y_old[1])-(int32)tmp*t_cos+(int32)(E->cC)*(uy))>>15; // q13
         
-        
-        int16 difz[2];
-        difz[0]=(isx-(E->y_est[0])); // shift to q15!!
-        difz[1]=(isy-(E->y_est[1]));//<<2;
+        E->difz[0]=(isx-(E->y_est[0])); // shift to q15!!
+        E->difz[1]=(isy-(E->y_est[1]));//<<2;
         
         E->y_old[0] = isx;
@@ -45 +46 @@
         
         *detS = 32767; *rem=0;
-        carlson_fastC(difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem);
+        carlson_fastC(E->difz,E->x_est,E->Chf,E->C,E->dR,2,2, detS, rem);
 }