Changeset 1326 for applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

Timestamp:

04/12/11 22:30:24 (13 years ago)

Author:

smidl

Message:

fixed point implemnetation of 2d EKF

Files:

• applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (modified) (12 diffs)

applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp

@@ -206 +206 @@
                                 *U_ij -=  ((int32)(*a_j)*(*b_i))/lambda;         // pozadovane optimalni reseni
                                  *b_i  +=  ((int32)beta*(*b_j))>>15;
-                                 printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta);
+                                 //printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta);
                         }
                 }
@@ -451 +451 @@
                 tau=Ch[i*dimx+j];
                                 tmp_long=((int32)beta*Ch[i*dimx+j] -(int32)sigma*w[i])/gamma;
-                                if (tmp_long>32767) {
+                /*              if (tmp_long>32767) {
                                         Ch[i*dimx+j]=32767;
                                 } else {
@@ -457 +457 @@
                                                 Ch[i*dimx+j]=-32767;
                                         } else {
-                                                Ch[i*dimx+j]=tmp_long;
-                                        }
+                */                              Ch[i*dimx+j]=tmp_long;
+                        /*              }
                                 }
-                                
+                        */      
                 w[i]+=((int32)tau*sigma)>>15;
             }
@@ -537 +537 @@
                         }
                         m1pom-=(j+1); // shift back to first element
-                        
-                        *respom++=tmp_sum>>15;
-                        
+        
+                /*      if (tmp_sum>(1<<29)-1) 
+                                *respom++=(1<<14);
+                        else
+                */      
+                if (tmp_sum>0)
+                        *respom++=(tmp_sum+(1<<14)-1)>>15;
+                else 
+                        *respom++=(tmp_sum-(1<<14)+1)>>15;
+                
                         tmp_sum=0;
                 }
@@ -552 +559 @@
         int16 rho,s,c,tau;
         int32  tmp_long;
+        
+        //c,s in q14!!
         
         int16 A[25];//beware
@@ -573 +582 @@
                 for (j=0; j<dimx; j++) 
                 {
-                        //                      tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+(long)A[i*dimx+j]*A[i*dimx+j];
-                        
-                        tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*A_ij**A_ij;
-                        
-                        if (tmp_long>0)
+                        if (*A_ij!=0)
                         {
+                                tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*A_ij**A_ij;
                                 //                              rho=qsqrt(tmp_long);                   // verze pro DSP
                                 rho=(int16)(sqrt((double)tmp_long));     // verze pro PC
-                                s=((int32)*A_ij<<15)/rho;
-                                c=((int32)*Ch_ii<<15)/rho;
+                                s=(((int32)*A_ij)<<14)/rho;
+                                c=(((int32)*Ch_ii)<<14)/rho;
                                 
                                 Ch_ki=Ch+i;
@@ -589 +595 @@
                                 for (k=0;k<=i; k++)
                                 {
-                                        tau=((int32)c**A_kj-(int32)s**Ch_ki)>>15;
-                                        *Ch_ki=((int32)s**A_kj+(int32)c**Ch_ki)>>15;
+                                        tau=((int32)c**A_kj-(int32)s**Ch_ki)>>14;
+                                        tmp_long=(int32)s**A_kj+(int32)c**Ch_ki;
+                                        if (tmp_long>(1<<28)) //q14 + q15
+                                                *Ch_ki = (1<<14)-1;
+                                        else 
+                                                *Ch_ki=tmp_long>>14;
                                         *A_kj=tau;
                                         
@@ -604 +614 @@
                 for (j=0; j<i; j++)
                 {
-                        tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij;
-                        
-                        if (tmp_long>0)
+                        
+                        if (*Ch_ij>0)
                         {
+                                tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij;
                                 //                      rho=qsqrt(tmp_long);                     // verze pro DSP
-                                rho=(int16)(sqrt((double)tmp_long));       // verze pro PC
-                                s=((int32)*Ch_ij<<15)/rho;
-                                c=((int32)*Ch_ii<<15)/rho;
+                                if (tmp_long>(1<<30)-1)
+                                        rho=(1<<15)-1;
+                                else
+                                        rho=(int16)(sqrt((double)tmp_long));       // verze pro PC
+                                                
+                                s=(((int32)*Ch_ij)<<14)/rho;
+                                c=(((int32)*Ch_ii)<<14)/rho;
                                 
                                 Ch_kj = Ch + j;
@@ -618 +632 @@
                                 for (k=0; k<=i; k++)
                                 {
-                                        tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>15;
-                                        *Ch_ki =((int32)s**Ch_kj+(int32)c**Ch_ki)>>15;
+                                        tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>14;
+                                        tmp_long =((int32)s**Ch_kj+(int32)c**Ch_ki);
+                                        if (tmp_long>(1<<28))
+                                                *Ch_ki = (1<<14)-1;
+                                        else 
+                                                *Ch_ki=tmp_long>>14;
                                         *Ch_kj=tau;
                                         
@@ -720 +738 @@
                         
                         //sigma=Ch[iy*dimx+j];
-                        beta=alpha;
+                        beta=alpha; // in q15
                         //            alpha+=((long)sigma*sigma)>>15;
-                        alpha=(((int32)alpha<<15)+(int32)sigma*sigma)>>15;                              // vyssi presnost
+                        tmp_long=((int32)alpha<<15)+(((int32)sigma*sigma)<<(30-2*qCh));
+                        if (tmp_long>0)
+                                alpha=(tmp_long+(1<<14)-1)>>15;                         // vyssi presnost
+                        else
+                                alpha=(tmp_long-(1<<14)+1)>>15;                         // vyssi presnost
+                                        
                         //            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
                         gamma= (int16)(sqrt((double)((int32)alpha*beta)));              // verze pro PC
-                        
+                               // in q15
                         w[j]=0;
                         
                         Ch_ij=Ch+j;
-                        w_i=w;
+                        w_i=w; // in q15
                         
                         for (i=0;i<=j;i++)
@@ -736 +759 @@
                                 tau=*Ch_ij;
                                 //                              tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
-                                tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma;
-                                
-                                if (tmp_long>32767)
+                                tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma; // in qCh
+                                
+/*                              if (tmp_long>32767)
                                         tmp_long=32767;
                                 if (tmp_long<-32768)
-                                        tmp_long=-32768;
+                                        tmp_long=-32768;*/
                                 *Ch_ij=tmp_long;
                                 
                                 //                w_i+=((long)tau*sigma)>>15;
-                                *w_i=(((int32)*w_i<<15)+(int32)tau*sigma)>>15;
+                                tmp_long = ((int32)*w_i<<15)+((int32)tau*sigma<<(30-2*qCh));
+                                /*if (tmp_long>0)
+                                        *w_i=(tmp_long+(1<<14)-1)>>15;
+                                else 
+                                        *w_i=(tmp_long-(1<<14)+1)>>15;
+                                */ *w_i=(tmp_long)>>15;
                                 
                                 w_i++;
@@ -761 +789 @@
                         w_i++;
                 }
-        }
-}
-
+                printf("Kg: %d %d\n",w[0],w[1]);
+        }
+}
+