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