pmsm-control/test_sw/commutators.c

   1 /**
   2  * \brief
   3  * Implementation of commutators and transformations.
   4  */
   5
   6 #include "commutators.h"
   7 #include "pxmc_sin_fixed.h"     /*sinus function*/
   8
   9
  10 /*
  11  * \brief
  12  * No commutation.
  13  */
  14 void zero_commutator(struct rpi_state* this){
  15         return;
  16 }
  17
  18 /*
  19  * \brief
  20  * Computes minimum value of three numbers.
  21  * Input values must be in range <-2^28;2^28>.
  22  */
  23 static int32_t min(int32_t x, int32_t y, int32_t z){
  24         int32_t diff,sign;
  25
  26         diff=x-y; /*rozdil*/
  27         sign=(*((uint32_t*)&diff))>>31; /*znamenko -> detekuje, ze y je vetsi*/
  28         x=y+sign*diff; /*ulozime mensi cislo, pokud sign>0, pak diff<0 */
  29
  30         diff=x-z; /*rozdil*/
  31         sign=(*((uint32_t*)&diff))>>31; /*znamenko -> detekuje, ze z je vetsi*/
  32         x=z+sign*diff; /*ulozime mensi cislo, pokud sign>0, pak diff<0 */
  33
  34         return x;
  35 }
  36
  37 /*
  38  * \brief
  39  * Transformace pro uhel pocitany po smeru hodinovych rucicek
  40  */
  41 static void dq2alphabeta(int32_t *alpha, int32_t *beta, int d, int q, int32_t sin, int32_t cos){
  42         *alpha=cos*d+sin*q;
  43         *beta=-sin*d+cos*q;
  44         return;
  45 }
  46
  47 /**
  48  * \brief
  49  * Zpetna Clarkova transformace
  50  */
  51 static void alphabeta2pwm3(int32_t * ia, int32_t * ib, int32_t *ic,int32_t alpha, int32_t beta){
  52         *ia=alpha;
  53         *ib=-alpha/2+beta*887/1024;
  54         *ic=-alpha/2-beta*887/1024;
  55 }
  56
  57 /*
  58  * \brief
  59  * Preocita napeti na jednotlivych civkach na napeti,
  60  *      ktera budou privedena na svorky motoru.
  61  *      Tedy na A(yel)-pwm1, B(red)-pwm2, C(blk)-pwm3
  62  */
  63 static void transDelta(int32_t * u1, int32_t * u2, int32_t *u3, int32_t ub , int32_t uc){
  64         int32_t t;
  65
  66         /*vypocte napeti tak, aby odpovidaly rozdily*/
  67         *u1=uc;
  68         *u2=uc+ub;
  69         *u3=0;
  70
  71         /*najde zaporne napeti*/
  72         t=min(*u1,*u2,*u3);
  73
  74         /*dorovna zaporna napeti na nulu*/
  75         *u1-=t;
  76         *u2-=t;
  77         *u3-=t;
  78 }
  79
  80 /**
  81  * \brief
  82  * Simple vector-control commutator without delta-transformation.
  83  * Nearly same as sin_commuatator.
  84  */
  85 void inv_trans_comm(struct rpi_state* this){
  86         uint32_t pos;
  87         int32_t sin, cos;
  88         int32_t alpha, beta;
  89         int32_t pwma,pwmb,pwmc;
  90         pos=this->index_dist;
  91         /*melo by byt urceno co nejpresneji, aby faze 'a' splyvala s osou 'alpha'*/
  92         pos+=717;
  93         /*use it as cyclic 32-bit logic*/
  94         pos*=4294967;
  95         pxmc_sincos_fixed_inline(&sin, &cos, pos, 16);
  96         dq2alphabeta(&alpha, &beta,0,this->duty, sin, cos);
  97         alpha>>=16;
  98         beta>>=16;
  99         alphabeta2pwm3(&pwma,&pwmb, &pwmc,alpha,beta);
 100
 101         if (pwma<0) pwma=0;
 102         if (pwmb<0) pwmb=0;
 103         if (pwmc<0) pwmc=0;
 104
 105         /*debugovaci vystupy - pouze vypisy hodnot*/
 106         this->t_pwm1=(uint16_t)pwma;
 107         this->t_pwm3=(uint16_t)pwmb;
 108         this->t_pwm2=(uint16_t)pwmc;
 109 }
 110
 111 /*
 112  * \brief
 113  * Robust vector-control commuator with Delta-transformation.
 114  */
 115 void inv_trans_comm_2(struct rpi_state* this){
 116         uint32_t pos;
 117         int32_t sin, cos;
 118         int32_t alpha, beta;
 119         int32_t ua,ub,uc;
 120         int32_t ia,ib,ic;
 121         int32_t u1,u2,u3;
 122         pos=this->index_dist;
 123
 124         pos+=this->alpha_offset; /*zarovnani faze 'a' s osou 'alpha'*/
 125
 126         /*pro výpočet sin a cos je pouzita 32-bit cyklicka logika*/
 127         pos*=4294967;
 128         pxmc_sincos_fixed_inline(&sin, &cos, pos, 16);
 129
 130         dq2alphabeta(&alpha, &beta,0,this->duty, sin, cos);
 131         alpha>>=16;
 132         beta>>=16;
 133
 134         alphabeta2pwm3(&ia,&ib, &ic,alpha,beta);
 135
 136         ua=ia;
 137         ub=ib;
 138         uc=ic;
 139
 140         transDelta(&u1,&u2, &u3,ub,uc);
 141
 142         this->pwm1=(uint16_t)u1;
 143         this->pwm2=(uint16_t)u2;
 144         this->pwm3=(uint16_t)u3;
 145 }
 146
 147 /**
 148  * \brief
 149  * Simple voltage commutation, takes use of sin finction.
 150  */
 151 void sin_commutator(struct rpi_state* this){
 152         #define DEGREE_60        715827883
 153         #define DEGREE_120      1431655765
 154         #define DEGREE_180      2147483648
 155         #define DEGREE_240      2863311531
 156         #define DEGREE_300      3579139413
 157         uint32_t j,pos;
 158         int32_t sin;
 159         int32_t pwm;
 160         int duty=this->duty;
 161         pos=this->index_dist;
 162         /*aby prictene uhly mohla byt kulata cisla, musime index posunout*/
 163         pos+=38;
 164         /*use it as cyclic 32-bit logic*/
 165         pos*=4294967;
 166         if (this->duty>=0){     /*clockwise rotation*/
 167                 /* 1st phase */
 168                 sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/
 169                 pwm=sin*duty/1024;
 170                 if (pwm<0) pwm=0;
 171                 this->pwm1=(uint16_t)pwm;
 172
 173                 /* 2nd phase */
 174                 sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/
 175                 pwm=sin*duty/1024;
 176                 if (pwm<0) pwm=0;
 177                 this->pwm2=(uint16_t)pwm;
 178
 179                 /* 3rd phase */
 180                 sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/
 181                 pwm=sin*duty/1024;
 182                 if (pwm<0) pwm=0;
 183                 this->pwm3=(uint16_t)pwm;
 184         }else{
 185                 duty=-duty;
 186
 187                 /* 1st phase */
 188                 sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/
 189                 pwm=sin*duty/1024;
 190                 if (pwm<0) pwm=0;
 191                 this->pwm1=(uint16_t)pwm;
 192
 193                 /* 2nd phase */
 194                 sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/
 195                 pwm=sin*duty/1024;
 196                 if (pwm<0) pwm=0;
 197                 this->pwm2=(uint16_t)pwm;
 198
 199                 /* 3rd phase */
 200                 sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/
 201                 pwm=sin*duty/1024;
 202                 if (pwm<0) pwm=0;
 203                 this->pwm3=(uint16_t)pwm;
 204         }
 205 }
 206
 207 /*
 208  * \brief
 209  * Test function to be placed in controll loop.
 210  * Switches PWM's at point where they produce same force.
 211  * This points are found thanks to IRC position,
 212  */
 213 void simple_ind_dist_commutator(struct rpi_state* this){
 214         int duty=this->duty;
 215         uint16_t index_dist=this->index_dist;
 216         if (duty>=0){ /* clockwise - so that position increase */
 217                 /* pwm3 */
 218                 if ((index_dist>=45 && index_dist<=373) ||
 219                 (index_dist>=1048 && index_dist<=1377)){
 220                         this->pwm1=0;
 221                         this->pwm2=0;
 222                         this->pwm3=duty;
 223                         /* pwm1 */
 224                 }else if ((index_dist>=373 && index_dist<=711) ||
 225                 (index_dist>=1377 && index_dist<=1711)){
 226                         this->pwm1=duty;
 227                         this->pwm2=0;
 228                         this->pwm3=0;
 229                         /* pwm2 */
 230                 }else if ((index_dist>=0 && index_dist<=45) ||
 231                 (index_dist>=711 && index_dist<=1048) ||
 232                 (index_dist>=1711 && index_dist<=1999)){
 233                         this->pwm1=0;
 234                         this->pwm2=duty;
 235                         this->pwm3=0;
 236                 }
 237         }else{  /*counter-clockwise - position decrease */
 238                 /* pwm3 */
 239                 if ((index_dist>=544 && index_dist<=881) ||
 240                 (index_dist>=1544 && index_dist<=1878)){
 241                         this->pwm1=0;
 242                         this->pwm2=0;
 243                         this->pwm3=-duty;
 244                         /* pwm1 */
 245                 }else if ((index_dist>=0 && index_dist<=211) ||
 246                 (index_dist>=881 && index_dist<=1210) ||
 247                 (index_dist>=1878 && index_dist<=1999)){
 248                         this->pwm1=-duty;
 249                         this->pwm2=0;
 250                         this->pwm3=0;
 251                         /* pwm2 */
 252                 }else if ((index_dist>=211 && index_dist<=544) ||
 253                 (index_dist>=1210 && index_dist<=1544)){
 254                         this->pwm1=0;
 255                         this->pwm2=-duty;
 256                         this->pwm3=0;
 257                 }
 258         }
 259 }
 260
 261 /*
 262  * \brief
 263  * Test function to be placed in controll loop.
 264  * Switches PWM's at point where they produce same force
 265  */
 266 void simple_hall_commutator(struct rpi_state* this){
 267         int duty=this->duty;
 268         int8_t hal1=this->spi_dat->hal1;
 269         int8_t hal2=this->spi_dat->hal2;
 270         int8_t hal3=this->spi_dat->hal3;
 271         if (duty>=0){ /* clockwise - so that position increase */
 272                 /* pwm3 */
 273                 if (hal2 && !hal3){
 274                         this->pwm1=0;
 275                         this->pwm2=0;
 276                         this->pwm3=duty;
 277                         /* pwm1 */
 278                 }else if (hal1 && !hal2){
 279                         this->pwm1=duty;
 280                         this->pwm2=0;
 281                         this->pwm3=0;
 282                         /* pwm2 */
 283                 }else if (!hal1 && hal3){
 284                         this->pwm1=0;
 285                         this->pwm2=duty;
 286                         this->pwm3=0;
 287                 }
 288         }else{  /*counter-clockwise - position decrease */
 289                 /* pwm3 */
 290                 if (!hal2 && hal3){
 291                         this->pwm1=0;
 292                         this->pwm2=0;
 293                         this->pwm3=-duty;
 294                         /* pwm1 */
 295                 }else if (!hal1 && hal2){
 296                         this->pwm1=-duty;
 297                         this->pwm2=0;
 298                         this->pwm3=0;
 299                         /* pwm2 */
 300                 }else if (hal1 && !hal3){
 301                         this->pwm1=0;
 302                         this->pwm2=-duty;
 303                         this->pwm3=0;
 304                 }
 305         }
 306 }