pmsm-control/test_sw/comp.c

   1 /**
   2  * \brief
   3  * Computatations of position, index, speed.
   4  *
   5  */
   6
   7 #include "comp.h"
   8 #include "pmsm_state.h"
   9 #include "rp_spi.h"
  10
  11 /**
  12  * \brief
  13  * Substact initial position.
  14  */
  15 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
  16         data->pozice=data->pozice_raw-offset->pozice_raw;
  17         return;
  18 }
  19
  20 /*
  21  * \brief
  22  * Multiplication of 11 bit
  23  * Zaporne vysledky prvede na nulu.
  24  */
  25 uint16_t mult_cap(int32_t s,int d){
  26         int j;
  27         int res=0;
  28         for(j=0;j!=11;j++){
  29                 /* multiplicate as if maximum sinus value was unity */
  30                 res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
  31         }
  32         return res;
  33 }
  34
  35
  36 /**
  37  * \brief
  38  * Computation of distance to index.
  39  *
  40  * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
  41  * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
  42  *      to je 3999 bodu
  43  *      =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
  44  *              signalu indexu
  45  */
  46 void comIndDist(struct rpi_state* this){
  47         uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw;
  48         uint16_t dist;
  49         uint16_t index = this->spi_dat->index_position;
  50
  51         if (index<1999){                /*index e<0,1998> */
  52                 if (pos<index){                 /*pozice e<0,index-1> */
  53                         /*proti smeru h.r. od indexu*/
  54                         dist=pos+2000-index;
  55                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
  56                         /*po smeru h.r. od indexu*/
  57                         dist=pos-index;
  58                 }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
  59                         goto index_lost;
  60                 }else{                          /*pozice e<index+2096,4095> */
  61                         /*proti smeru h.r. od indexu - podtecena pozice*/
  62                         dist=pos-index-2096;
  63                 }
  64         }else if (index<=2096){         /*index e<1999,2096>*/
  65                 if (pos<index-1999){            /*pozice e<0,index-2000> */
  66                         goto index_lost;
  67                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
  68                         /*proti smeru h.r. od indexu*/
  69                         dist=pos+2000-index;
  70                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
  71                         /*po smeru h.r. od indexu*/
  72                         dist=pos-index;
  73                 }else {                         /*pozice e<index+2000,4095> */
  74                         goto index_lost;
  75                 }
  76         }else{                          /*index e<2097,4095> */
  77                 if (pos<=index-2097){           /*pozice e<0,index-2097> */
  78                         /*po smeru h.r. od indexu - pretecena pozice*/
  79                         dist=4096+pos-index;
  80                 }else if (pos<index-1999){      /*pozice e<index-2096,index-2000> */
  81                         goto index_lost;
  82                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
  83                         /*proti smeru h.r. od indexu*/
  84                         dist=pos+2000-index;
  85                 }else{                          /*pozice e<index,4095> */
  86                         /*po smeru h.r. od indexu*/
  87                         dist=pos-index;
  88                 }
  89         }
  90
  91         this->index_dist = dist;
  92         return;
  93
  94         index_lost:
  95                 this->index_ok=0;
  96                 return;
  97 }
  98
  99
 100 /*
 101  * \brief
 102  * Computate speed.
 103  */
 104 void compSpeed(struct rpi_state* this){
 105         signed long int spd;
 106         spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM];
 107         this->speed=(int32_t)spd;
 108 }