Changeset 1465 for applications

Timestamp:

08/02/12 22:42:13 (12 years ago)

Author:

smidl

Message:

oprava pretekani A*Ch

Location:

applications/pmsm/simulator_zdenek/ekf_example

Files:

• matrix_vs.cpp (modified) (6 diffs)
• matrix_vs.h (modified) (1 diff)

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -527 +527 @@
         }
         
+        //printf("Gin: %d,%d,%d,%d\n", Ch[0],Ch[1],Ch[2],Ch[3] );
+        
         for (i=dimx-1; i>=0; i--)
         {
@@ -539 +541 @@
                                 //                              rho=qsqrt(tmp_long);                   // verze pro DSP
                                 rho=(int16)(sqrt((double)tmp_long));     // verze pro PC
-                                s=(((int32)*A_ij)<<14)/rho;
+                                s=(((int32)*A_ij)<<14)/rho;    // qCh + q14 /qch => s in q14
                                 c=(((int32)*Ch_ii)<<14)/rho;
                                 
@@ -547 +549 @@
                                 for (k=0;k<=i; k++)
                                 {
-                                        tau=((int32)c**A_kj-(int32)s**Ch_ki)>>14;
-                                        tmp_long=(int32)s**A_kj+(int32)c**Ch_ki;
-                                        if (tmp_long>(1<<29)) //q14 + q14
-                                                *Ch_ki = (1<<15)-1;
+                                        tau=((int32)c**A_kj-(int32)s**Ch_ki)>>14; // tau in qCh
+                                        tmp_long=(int32)s**A_kj+(int32)c**Ch_ki;  // q14+qCh
+                                        if ((tmp_long>(1<<(14+14)))){ //q14 + q15
+                                                *Ch_ki = (1<<14)-1; // rezerva pro nasobeni A*Ch
+                                        }
                                         else 
-                                                *Ch_ki=tmp_long>>14;
+                                                *Ch_ki=tmp_long>>14; // qCh
                                         *A_kj=tau;
                                         
@@ -571 +574 @@
                                 tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij;
                                 //                      rho=qsqrt(tmp_long);                     // verze pro DSP
-                                if (tmp_long>(1<<30)-1)
+                                if (tmp_long>(1<<30)-1){
                                         rho=(1<<15)-1;
+                                }
                                 else
                                         rho=(int16)(sqrt((double)tmp_long));       // verze pro PC
@@ -586 +590 @@
                                         tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>14;
                                         tmp_long =((int32)s**Ch_kj+(int32)c**Ch_ki);
-                                        if (tmp_long>(1<<29))
-                                                *Ch_ki = (1<<15)-1;
+                                        if ((tmp_long>(1<<(14+14)))){ //q14 + q15
+                                                *Ch_ki = (1<<14)-1; // rezerva pronasobeni A*Ch
+                                        }
                                         else 
                                                 *Ch_ki=tmp_long>>14;
@@ -637 +642 @@
                                 tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma;
                                 
-                                if (tmp_long>32767)
+                                if (tmp_long>32767){
                                         tmp_long=32767;
+                                }
                                 if (tmp_long<-32768)
                                         tmp_long=-32768;

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.h

@@ -14 +14 @@
 #define qCU 15
 #define qD 13
-#define qCh 14
+#define qCh 13
 
 #define int16 short
 #define int32 int
+#define int64 long
 
 /* Matrix multiply Full matrix by upper diagonal matrix with unit diagonal; */