Correction.

[fpga/rpi-motor-control.git] / pmsm-control / test_sw / main_pmsm.c

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index f666358a2d4711509d5b5184346f6f167d806186..26e83089f0ead662a149ba3c1879fcd44ddb28cd 100644 (file)
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -233,7 +233,20 @@ void * pos_monitor(void* param){
        }
        return (void*)0;
 }
-
+/*
+ * \brief
+ * Multiplication of 11 bit
+ * Zaporne vysledky prvede na nulu.
+ */
+inline uint16_t mult_cap(int32_t s,int d){
+       int j;
+       int res=0;
+       for(j=0;j!=11;j++){
+               /* multiplicate as if maximum sinus value was unity */
+               res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
+       }
+       return res;
+}
 inline
 int sin_commutator(int duty){
        #define DEGREE_60        715827883
@@ -243,64 +256,33 @@ int sin_commutator(int duty){
        #define DEGREE_300      3579139413
        uint32_t j,pos;
        int32_t sin;
-       uint16_t pwm;
        pos=rps.index_dist*4294967;
        if (duty>=0){   /*clockwise rotation*/
                /* 1st phase */
                sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/
-                pwm=0;
-                for(j=0;j!=11;j++){
-                       /* multiplicate as if maximum sinus value was unity */
-                        pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
-                }
-                rps.pwm1=pwm;
+                rps.pwm1=mult_cap(sin, duty);
 
                 /* 2nd phase */
                 sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/
-                pwm=0;
-                for(j=0;j!=11;j++){
-                       /* multiplicate as if maximum sinus value was unity */
-                        pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
-                }
-                rps.pwm2=pwm;
+                rps.pwm2=mult_cap(sin, duty);
 
                 /* 3rd phase */
                 sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/
-                pwm=0;
-                for(j=0;j!=11;j++){
-                       /* multiplicate as if maximum sinus value was unity */
-                        pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
-                }
-                rps.pwm3=pwm;
+                rps.pwm3=mult_cap(sin, duty);
         }else{
                duty=-duty;
 
                /* 1st phase */
                sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/
-                pwm=0;
-                for(j=0;j!=11;j++){
-                       /* multiplicate as if maximum sinus value was unity */
-                        pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
-                }
-                rps.pwm1=pwm;
+                rps.pwm1=mult_cap(sin, duty);
 
                 /* 2nd phase */
                 sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/
-                pwm=0;
-                for(j=0;j!=11;j++){
-                       /* multiplicate as if maximum sinus value was unity */
-                        pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
-                }
-                rps.pwm2=pwm;
+                rps.pwm2=mult_cap(sin, duty);
 
                 /* 3rd phase */
                 sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/
-                pwm=0;
-                for(j=0;j!=11;j++){
-                       /* multiplicate as if maximum sinus value was unity */
-                        pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
-                }
-                rps.pwm3=pwm;
+                rps.pwm3=mult_cap(sin, duty);
         }
         return 0;
 }
@@ -409,31 +391,56 @@ inline void simple_hall_commutator(int duty){
  *             signalu indexu
  */
 void comIndDist(){
-       uint16_t pos_12 = 0x0FFF & data.pozice_raw;
-       if (data.index_position<2048){
-               if (pos_12<data.index_position){
+       uint16_t pos = 0x0FFF & data.pozice_raw;
+       uint16_t dist;
+       uint16_t index = data.index_position;
+
+       if (index<1999){                /*index e<0,1998> */
+               if (pos<index){                 /*pozice e<0,index-1> */
                        /*proti smeru h.r. od indexu*/
-                       rps.index_dist=pos_12+2000-data.index_position;
-               }else if (pos_12<=data.index_position+2048){
+                       dist=pos+2000-index;
+               }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
                        /*po smeru h.r. od indexu*/
-                       rps.index_dist=pos_12-data.index_position;
-               }else{
+                       dist=pos-index;
+               }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
+                       goto index_lost;
+               }else{                          /*pozice e<index+2096,4095> */
                        /*proti smeru h.r. od indexu - podtecena pozice*/
-                       rps.index_dist=pos_12-data.index_position-2096;
+                       dist=pos-index-2096;
                }
-       }else{
-               if (pos_12<data.index_position-2048){
+       }else if (index<=2096){         /*index e<1999,2096>*/
+               if (pos<index-1999){            /*pozice e<0,index-2000> */
+                       goto index_lost;
+               }else if (pos<index){           /*pozice e<index-1999,index-1> */
+                       /*proti smeru h.r. od indexu*/
+                       dist=pos+2000-index;
+               }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
+                       /*po smeru h.r. od indexu*/
+                       dist=pos-index;
+               }else {                         /*pozice e<index+2000,4095> */
+                       goto index_lost;
+               }
+       }else{                          /*index e<2097,4095> */
+               if (pos<=index-2097){           /*pozice e<0,index-2097> */
                        /*po smeru h.r. od indexu - pretecena pozice*/
-                       rps.index_dist=4096+pos_12-data.index_position;
-               }else if (pos_12<data.index_position){
+                       dist=4096+pos-index;
+               }else if (pos<index-1999){      /*pozice e<index-2096,index-2000> */
+                       goto index_lost;
+               }else if (pos<index){           /*pozice e<index-1999,index-1> */
                        /*proti smeru h.r. od indexu*/
-                       rps.index_dist=pos_12+2000-data.index_position;
-               }else{
+                       dist=pos+2000-index;
+               }else{                          /*pozice e<index,4095> */
                        /*po smeru h.r. od indexu*/
-                       rps.index_dist=pos_12-data.index_position;
+                       dist=pos-index;
                }
-
        }
+
+       rps.index_dist = dist;
+       return;
+
+       index_lost:
+               rps.index_ok=0;
+               return;
 }
 /*
  * \brief