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 #include <stdio.h>
  12 #include <stdlib.h>
  13
  14 int transitions[6][6]={
  15                      {  0,  0,881,  0,730,  0},
  16                      {  0,  0, 50,  0,  0,230},
  17                      {881, 50,  0,  0,  0,  0},
  18                      {  0,  0,  0,  0,550,380},
  19                      {730,  0,  0,550,  0,  0},
  20                      {  0,230,  0,380,  0,  0}
  21                     };
  22
  23 /**
  24  * \brief
  25  * Substact initial position.
  26  */
  27 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
  28         data->pozice=data->pozice_raw-offset->pozice_raw;
  29         return;
  30 }
  31
  32 /*
  33  * \brief
  34  * Multiplication of 11 bit
  35  * Zaporne vysledky prvede na nulu.
  36  */
  37 uint16_t mult_cap(int32_t s,int d){
  38         int j;
  39         int res=0;
  40         for(j=0;j!=11;j++){
  41                 /* multiplicate as if maximum sinus value was unity */
  42                 res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
  43         }
  44         return res;
  45 }
  46
  47 /**
  48  * \brief
  49  * Attempt to resolve fail-state in case of irc cable malfunction.
  50  *
  51  * Pri prechodu mezi haly je pocitan rozdil aktualni a predpokladane
  52  * vzdalenosti od indexu. Pokud prekroci MAX_DIFF, je prepnuto na
  53  * komutaci jen pomoci halu.
  54  */
  55 #define MAX_DIFF 90
  56 void repairPositionErr(struct rpi_state* this){
  57         int new;
  58         int old;
  59         int trans;
  60         /* doslo k prechodu mezi haly */
  61         if (this->h1_old != this->spi_dat->hal1 ||
  62             this->h2_old != this->spi_dat->hal2 ||
  63             this->h3_old != this->spi_dat->hal3)
  64         {
  65                 new=this->spi_dat->hal1*4+this->spi_dat->hal2*2+this->spi_dat->hal3;
  66                 old=this->h1_old*4+this->h2_old*2+this->h3_old;
  67                 trans = transitions[new-1][old-1]; /*predpokladana pozice */
  68
  69                 /*pozice jsou v rozsahu 0-999*/
  70                 if (abs(this->index_dist-trans)>MAX_DIFF &&
  71                     abs(this->index_dist-trans-1000)>MAX_DIFF){
  72                         setIndexLost(this);
  73                 }
  74
  75                 /*nakonec preulozme stare hodnoty*/
  76                 this->h1_old = this->spi_dat->hal1;
  77                 this->h2_old = this->spi_dat->hal2;
  78                 this->h3_old = this->spi_dat->hal3;
  79         }
  80
  81 }
  82
  83 /**
  84  * \brief
  85  * Computation of distance to index.
  86  *
  87  * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
  88  * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
  89  *      to je 3999 bodu
  90  *      =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
  91  *              signalu indexu
  92  */
  93 void comIndDist(struct rpi_state* this){
  94         uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw;
  95         uint16_t dist;
  96         uint16_t index = this->spi_dat->index_position;
  97
  98         if (index<1999){                /*index e<0,1998> */
  99                 if (pos<index){                 /*pozice e<0,index-1> */
 100                         /*proti smeru h.r. od indexu*/
 101                         dist=pos+2000-index;
 102                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 103                         /*po smeru h.r. od indexu*/
 104                         dist=pos-index;
 105                 }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
 106                         goto index_lost;
 107                 }else{                          /*pozice e<index+2096,4095> */
 108                         /*proti smeru h.r. od indexu - podtecena pozice*/
 109                         dist=pos-index-2096;
 110                 }
 111         }else if (index<=2096){         /*index e<1999,2096>*/
 112                 if (pos<index-1999){            /*pozice e<0,index-2000> */
 113                         goto index_lost;
 114                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 115                         /*proti smeru h.r. od indexu*/
 116                         dist=pos+2000-index;
 117                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 118                         /*po smeru h.r. od indexu*/
 119                         dist=pos-index;
 120                 }else {                         /*pozice e<index+2000,4095> */
 121                         goto index_lost;
 122                 }
 123         }else{                          /*index e<2097,4095> */
 124                 if (pos<=index-2097){           /*pozice e<0,index-2097> */
 125                         /*po smeru h.r. od indexu - pretecena pozice*/
 126                         dist=4096+pos-index;
 127                 }else if (pos<index-1999){      /*pozice e<index-2096,index-2000> */
 128                         goto index_lost;
 129                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 130                         /*proti smeru h.r. od indexu*/
 131                         dist=pos+2000-index;
 132                 }else{                          /*pozice e<index,4095> */
 133                         /*po smeru h.r. od indexu*/
 134                         dist=pos-index;
 135                 }
 136         }
 137
 138         this->index_dist = dist;
 139
 140         repairPositionErr(this);
 141
 142         return;
 143
 144         index_lost:
 145                 setIndexLost(this);
 146                 return;
 147 }
 148
 149
 150 /*
 151  * \brief
 152  * Computate speed.
 153  */
 154 void compSpeed(struct rpi_state* this){
 155         signed long int spd;
 156         spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM];
 157         this->speed=(int32_t)spd;
 158 }