pmsm-control/test_sw/main_pmsm.c

   1 /**
   2  * \file main_pmsm.c
   3  * \author Martin Prudek
   4  * \brief Mainfile pro pmsm control.
   5  */
   6
   7 #ifndef NULL
   8 #define NULL (void*) 0
   9 #endif /*NULL*/
  10
  11
  12 #include <stdlib.h>     /*exit*/
  13 #include <signal.h>     /*signal handler Ctrl+C*/
  14 #include <stdio.h>      /*printf*/
  15 #include <sched.h>      /*sheduler*/
  16 #include <unistd.h>     /*usleep*/
  17 #include <pthread.h>    /*threads*/
  18 #include <time.h>       /*nanosleep*/
  19 #include <string.h>
  20
  21 #include "rpin.h"       /*gpclk*/
  22 #include "rp_spi.h"     /*spi*/
  23 #include "misc.h"       /*structure for priorities*/
  24 #include "pxmc_sin_fixed.h"     /*to test sin commutation */
  25 #include "pmsm_state.h"
  26 #include "cmd_proc.h"
  27
  28 #define MAX_DUTY        170
  29 #define PID_P           0.3
  30 #define PID_P_S         0.3
  31
  32 #define PRIOR_KERN      50
  33 #define PRIOR_HIGH      49
  34 #define PRIOR_LOW       20
  35
  36 #define THREAD_SHARED   0
  37 #define INIT_VALUE      1       /*init value for semaphor*/
  38
  39
  40 #define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
  41
  42
  43 struct rpi_in data;
  44 struct rpi_state rps={
  45         .spi_dat=&data,
  46         .test=0,
  47         .pwm1=0,.pwm2=0, .pwm3=0,
  48         .pwm1=0, .t_pwm2=0, .t_pwm3=0,
  49         .commutate=0,
  50         .duty=0,                        /* duty cycle of pwm */
  51         .index_dist=0,          /* distance to index position */
  52         .index_ok=0,
  53         .tf_count=0,            /*number of transfer*/
  54         .desired_pos=0,         /* desired position */
  55         .pos_reg_ena=0,
  56         .desired_spd=0,
  57         .spd_reg_ena=0,
  58         .old_pos={0}
  59 };
  60
  61 /**
  62  * \brief Initilizes GPCLK.
  63  */
  64 int clk_init()
  65 {
  66         initialise(); /*namapovani gpio*/
  67         initClock(PLLD_500_MHZ, 10, 0);
  68         gpioSetMode(4, FSEL_ALT0);
  69         return 0;
  70 }
  71 /*
  72  * \brief Terminates GPCLK.
  73  */
  74
  75 inline void clk_disable(){
  76         termClock(0);
  77 }
  78 /*
  79  * \brief
  80  * Count minimum value of three numbers.
  81  * Input values must be in range <-2^28;2^28>.
  82  */
  83 int32_t min(int32_t x, int32_t y, int32_t z){
  84         int32_t diff,sign;
  85
  86         diff=x-y; /*rozdil*/
  87         sign=(*((uint32_t*)&diff))>>31; /*znamenko -> detekuje, ze y je vetsi*/
  88         x=y+sign*diff; /*ulozime mensi cislo, pokud sign>0, pak diff<0 */
  89
  90         diff=x-z; /*rozdil*/
  91         sign=(*((uint32_t*)&diff))>>31; /*znamenko -> detekuje, ze z je vetsi*/
  92         x=z+sign*diff; /*ulozime mensi cislo, pokud sign>0, pak diff<0 */
  93
  94         return x;
  95 }
  96 /*
  97  * \brief
  98  * Pripravi psi buffer
  99  */
 100 void prepare_tx(uint8_t * tx){
 101
 102         /*Data format:
 103          * tx[4] - bity 95 downto 88 - bits that are sent first
 104          * tx[5] - bity 87 downto 80
 105          * tx[6] - bity 79 downto 72
 106          * tx[7] - bity 71 downto 64
 107          * tx[8] - bity 63 downto 56
 108          * tx[9] - bity 55 downto 48
 109          * tx[10] - bity 47 downto 40
 110          * tx[11] - bity 39 downto 32
 111          * tx[12] - bity 31 downto 24
 112          * tx[13] - bity 23 downto 16
 113          * tx[14] - bity 15 downto 8
 114          * tx[15] - bity 7 downto 0
 115          *
 116          * bit 95 - ADC reset
 117          * bit 94 - enable PWM1
 118          * bit 93 - enable PWM2
 119          * bit 92 - enable PWM3
 120          * bit 91 - shutdown1
 121          * bit 90 - shutdown2
 122          * bit 89 - shutdown3
 123          *      .
 124          *      .
 125          *      Unused
 126          *      .
 127          *      .
 128          * bits 47 .. 32 - match PWM1
 129          * bits 31 .. 16 - match PWM2
 130          * bits 15 .. 0  - match PWM3
 131          */
 132
 133
 134         uint16_t tmp;
 135
 136         /* keep the 11-bit cap*/
 137
 138         if (rps.pwm1>2047) rps.pwm1=2047;
 139         if (rps.pwm2>2047) rps.pwm2=2047;
 140         if (rps.pwm3>2047) rps.pwm3=2047;
 141
 142         tx[0]=rps.test; /*bit 94 - enable PWM1*/
 143
 144         /*now we have to switch the bytes due to endianess */
 145         /* ARMv6 & ARMv7 instructions are little endian */
 146         /*pwm1*/
 147         tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/
 148         tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/
 149
 150         /*pwm2*/
 151         tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/
 152         tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/
 153
 154         /*pwm3*/
 155         tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/
 156         tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/
 157
 158
 159 }
 160
 161
 162 /**
 163  * \brief Signal handler pro Ctrl+C
 164  */
 165 void appl_stop(){
 166         uint8_t tx[16];
 167         sem_wait(&rps.thd_par_sem);
 168
 169         memset(tx,0,16*sizeof(int));
 170         rps.pwm1=0;
 171         rps.pwm2=0;
 172         rps.pwm3=0;
 173         prepare_tx(tx);                 /*save the data to send*/
 174         data=spi_read(tx);
 175
 176         spi_disable();
 177         clk_disable();
 178         /*muzeme zavrit semafor*/
 179         sem_destroy(&rps.thd_par_sem);
 180         printf("\nprogram bezpecne ukoncen\n");
 181 }
 182
 183 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
 184         data->pozice=data->pozice_raw-offset->pozice_raw;
 185         return;
 186 }
 187 /*
 188  * \brief
 189  * Transformace pro uhel pocitany po smeru hodinovych rucicek
 190  */
 191 void dq2alphabeta(int32_t *alpha, int32_t *beta, int d, int q, int32_t sin, int32_t cos){
 192         *alpha=cos*d+sin*q;
 193         *beta=-sin*d+cos*q;
 194         return;
 195 }
 196 void alphabeta2pwm3(int32_t * ia, int32_t * ib, int32_t *ic,int32_t alpha, int32_t beta){
 197         *ia=alpha;
 198         *ib=-alpha/2+beta*887/1024;
 199         *ic=-alpha/2-beta*887/1024;
 200 }
 201 /*
 202  * \brief
 203  * Preocita napeti na jednotlivych civkach na napeti,
 204  *      ktera budou privedena na svorky motoru.
 205  *      Tedy na A(yel)-pwm1, B(red)-pwm2, C(blk)-pwm3
 206  */
 207 void transDelta(int32_t * u1, int32_t * u2, int32_t *u3, int32_t ub , int32_t uc){
 208         int32_t t;
 209
 210         /*vypocte napeti tak, aby odpovidaly rozdily*/
 211         *u1=uc;
 212         *u2=uc+ub;
 213         *u3=0;
 214
 215         /*najde zaporne napeti*/
 216         t=min(*u1,*u2,*u3);
 217
 218         /*dorovna zaporna napeti na nulu*/
 219         *u1-=t;
 220         *u2-=t;
 221         *u3-=t;
 222 }
 223 void inv_trans_comm(int duty){
 224         uint32_t pos;
 225         int32_t sin, cos;
 226         int32_t alpha, beta;
 227         int32_t pwma,pwmb,pwmc;
 228         pos=rps.index_dist;
 229         /*melo by byt urceno co nejpresneji, aby faze 'a' splyvala s osou 'alpha'*/
 230         pos+=717;
 231         /*use it as cyclic 32-bit logic*/
 232         pos*=4294967;
 233         pxmc_sincos_fixed_inline(&sin, &cos, pos, 16);
 234         dq2alphabeta(&alpha, &beta,0,duty, sin, cos);
 235         alpha>>=16;
 236         beta>>=16;
 237         alphabeta2pwm3(&pwma,&pwmb, &pwmc,alpha,beta);
 238
 239         if (pwma<0) pwma=0;
 240         if (pwmb<0) pwmb=0;
 241         if (pwmc<0) pwmc=0;
 242
 243
 244         rps.t_pwm1=(uint16_t)pwma;
 245         rps.t_pwm3=(uint16_t)pwmb;
 246         rps.t_pwm2=(uint16_t)pwmc;
 247 }
 248
 249 void inv_trans_comm_2(int duty){
 250         uint32_t pos;
 251         int32_t sin, cos;
 252         int32_t alpha, beta;
 253         int32_t ua,ub,uc;
 254         int32_t ia,ib,ic;
 255         int32_t u1,u2,u3;
 256         pos=rps.index_dist;
 257
 258         pos+=960; /*zarovnani faze 'a' s osou 'alpha'*/
 259
 260         /*pro výpočet sin a cos je pouzita 32-bit cyklicka logika*/
 261         pos*=4294967;
 262         pxmc_sincos_fixed_inline(&sin, &cos, pos, 16);
 263
 264         dq2alphabeta(&alpha, &beta,0,duty, sin, cos);
 265         alpha>>=16;
 266         beta>>=16;
 267
 268         alphabeta2pwm3(&ia,&ib, &ic,alpha,beta);
 269
 270         ua=ia;
 271         ub=ib;
 272         uc=ic;
 273
 274         transDelta(&u1,&u2, &u3,ub,uc);
 275
 276         rps.pwm1=(uint16_t)u1;
 277         rps.pwm2=(uint16_t)u2;
 278         rps.pwm3=(uint16_t)u3;
 279 }
 280
 281 /**
 282  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 283  */
 284 void * pos_monitor(void* param){
 285         while(1){
 286                 printData(&rps);
 287                 usleep(1000000);        /*1 Hz*/
 288         }
 289         return (void*)0;
 290 }
 291 /*
 292  * \brief
 293  * Multiplication of 11 bit
 294  * Zaporne vysledky prvede na nulu.
 295  */
 296 inline uint16_t mult_cap(int32_t s,int d){
 297         int j;
 298         int res=0;
 299         for(j=0;j!=11;j++){
 300                 /* multiplicate as if maximum sinus value was unity */
 301                 res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
 302         }
 303         return res;
 304 }
 305 inline
 306 int sin_commutator(int duty){
 307         #define DEGREE_60        715827883
 308         #define DEGREE_120      1431655765
 309         #define DEGREE_180      2147483648
 310         #define DEGREE_240      2863311531
 311         #define DEGREE_300      3579139413
 312         uint32_t j,pos;
 313         int32_t sin;
 314         pos=rps.index_dist;
 315         int32_t pwm;
 316         /*aby prictene uhly mohla byt kulata cisla, musime index posunout*/
 317         pos+=38;
 318         /*use it as cyclic 32-bit logic*/
 319         pos*=4294967;
 320         if (duty>=0){   /*clockwise rotation*/
 321                 /* 1st phase */
 322                 sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/
 323                 pwm=sin*duty/1024;
 324                 if (pwm<0) pwm=0;
 325                 rps.pwm1=(uint16_t)pwm;
 326
 327                 /* 2nd phase */
 328                 sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/
 329                 pwm=sin*duty/1024;
 330                 if (pwm<0) pwm=0;
 331                 rps.pwm2=(uint16_t)pwm;
 332
 333                 /* 3rd phase */
 334                 sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/
 335                 pwm=sin*duty/1024;
 336                 if (pwm<0) pwm=0;
 337                 rps.pwm3=(uint16_t)pwm;
 338         }else{
 339                 duty=-duty;
 340
 341                 /* 1st phase */
 342                 sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/
 343                 pwm=sin*duty/1024;
 344                 if (pwm<0) pwm=0;
 345                 rps.pwm1=(uint16_t)pwm;
 346
 347                 /* 2nd phase */
 348                 sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/
 349                 pwm=sin*duty/1024;
 350                 if (pwm<0) pwm=0;
 351                 rps.pwm2=(uint16_t)pwm;
 352
 353                 /* 3rd phase */
 354                 sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/
 355                 pwm=sin*duty/1024;
 356                 if (pwm<0) pwm=0;
 357                 rps.pwm3=(uint16_t)pwm;
 358         }
 359         return 0;
 360 }
 361 /*
 362  * \brief
 363  * Test function to be placed in controll loop.
 364  * Switches PWM's at point where they produce same force.
 365  * This points are found thanks to IRC position,
 366  */
 367 inline
 368 void simple_ind_dist_commutator(int duty){
 369         if (duty>=0){ /* clockwise - so that position increase */
 370                 /* pwm3 */
 371                 if ((rps.index_dist>=45 && rps.index_dist<=373) ||
 372                 (rps.index_dist>=1048 && rps.index_dist<=1377)){
 373                         rps.pwm1=0;
 374                         rps.pwm2=0;
 375                         rps.pwm3=duty;
 376                         /* pwm1 */
 377                 }else if ((rps.index_dist>=373 && rps.index_dist<=711) ||
 378                 (rps.index_dist>=1377 && rps.index_dist<=1711)){
 379                         rps.pwm1=duty;
 380                         rps.pwm2=0;
 381                         rps.pwm3=0;
 382                         /* pwm2 */
 383                 }else if ((rps.index_dist>=0 && rps.index_dist<=45) ||
 384                 (rps.index_dist>=711 && rps.index_dist<=1048) ||
 385                 (rps.index_dist>=1711 && rps.index_dist<=1999)){
 386                         rps.pwm1=0;
 387                         rps.pwm2=duty;
 388                         rps.pwm3=0;
 389                 }
 390         }else{  /*counter-clockwise - position decrease */
 391                 /* pwm3 */
 392                 if ((rps.index_dist>=544 && rps.index_dist<=881) ||
 393                 (rps.index_dist>=1544 && rps.index_dist<=1878)){
 394                         rps.pwm1=0;
 395                         rps.pwm2=0;
 396                         rps.pwm3=-duty;
 397                         /* pwm1 */
 398                 }else if ((rps.index_dist>=0 && rps.index_dist<=211) ||
 399                 (rps.index_dist>=881 && rps.index_dist<=1210) ||
 400                 (rps.index_dist>=1878 && rps.index_dist<=1999)){
 401                         rps.pwm1=-duty;
 402                         rps.pwm2=0;
 403                         rps.pwm3=0;
 404                         /* pwm2 */
 405                 }else if ((rps.index_dist>=211 && rps.index_dist<=544) ||
 406                 (rps.index_dist>=1210 && rps.index_dist<=1544)){
 407                         rps.pwm1=0;
 408                         rps.pwm2=-duty;
 409                         rps.pwm3=0;
 410                 }
 411         }
 412 }
 413 /*
 414  * \brief
 415  * Test function to be placed in controll loop.
 416  * Switches PWM's at point where they produce same force
 417  */
 418 inline void simple_hall_commutator(int duty){
 419         if (duty>=0){ /* clockwise - so that position increase */
 420                 /* pwm3 */
 421                 if (data.hal2 && !data.hal3){
 422                         rps.pwm1=0;
 423                         rps.pwm2=0;
 424                         rps.pwm3=duty;
 425                         /* pwm1 */
 426                 }else if (data.hal1 && !data.hal2){
 427                         rps.pwm1=duty;
 428                         rps.pwm2=0;
 429                         rps.pwm3=0;
 430                         /* pwm2 */
 431                 }else if (!data.hal1 && data.hal3){
 432                         rps.pwm1=0;
 433                         rps.pwm2=duty;
 434                         rps.pwm3=0;
 435                 }
 436         }else{  /*counter-clockwise - position decrease */
 437                 /* pwm3 */
 438                 if (!data.hal2 && data.hal3){
 439                         rps.pwm1=0;
 440                         rps.pwm2=0;
 441                         rps.pwm3=-duty;
 442                         /* pwm1 */
 443                 }else if (!data.hal1 && data.hal2){
 444                         rps.pwm1=-duty;
 445                         rps.pwm2=0;
 446                         rps.pwm3=0;
 447                         /* pwm2 */
 448                 }else if (data.hal1 && !data.hal3){
 449                         rps.pwm1=0;
 450                         rps.pwm2=-duty;
 451                         rps.pwm3=0;
 452                 }
 453         }
 454 }
 455 /**
 456  * \brief
 457  * Computation of distance to index.
 458  *
 459  * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
 460  * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
 461  *      to je 3999 bodu
 462  *      =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
 463  *              signalu indexu
 464  */
 465 void comIndDist(){
 466         uint16_t pos = 0x0FFF & data.pozice_raw;
 467         uint16_t dist;
 468         uint16_t index = data.index_position;
 469
 470         if (index<1999){                /*index e<0,1998> */
 471                 if (pos<index){                 /*pozice e<0,index-1> */
 472                         /*proti smeru h.r. od indexu*/
 473                         dist=pos+2000-index;
 474                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 475                         /*po smeru h.r. od indexu*/
 476                         dist=pos-index;
 477                 }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
 478                         goto index_lost;
 479                 }else{                          /*pozice e<index+2096,4095> */
 480                         /*proti smeru h.r. od indexu - podtecena pozice*/
 481                         dist=pos-index-2096;
 482                 }
 483         }else if (index<=2096){         /*index e<1999,2096>*/
 484                 if (pos<index-1999){            /*pozice e<0,index-2000> */
 485                         goto index_lost;
 486                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 487                         /*proti smeru h.r. od indexu*/
 488                         dist=pos+2000-index;
 489                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 490                         /*po smeru h.r. od indexu*/
 491                         dist=pos-index;
 492                 }else {                         /*pozice e<index+2000,4095> */
 493                         goto index_lost;
 494                 }
 495         }else{                          /*index e<2097,4095> */
 496                 if (pos<=index-2097){           /*pozice e<0,index-2097> */
 497                         /*po smeru h.r. od indexu - pretecena pozice*/
 498                         dist=4096+pos-index;
 499                 }else if (pos<index-1999){      /*pozice e<index-2096,index-2000> */
 500                         goto index_lost;
 501                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 502                         /*proti smeru h.r. od indexu*/
 503                         dist=pos+2000-index;
 504                 }else{                          /*pozice e<index,4095> */
 505                         /*po smeru h.r. od indexu*/
 506                         dist=pos-index;
 507                 }
 508         }
 509
 510         rps.index_dist = dist;
 511         return;
 512
 513         index_lost:
 514                 rps.index_ok=0;
 515                 return;
 516 }
 517 /*
 518  * \brief
 519  * Very simple PID regulator.
 520  * Now only with P-part so that the error doesnt go to zero.
 521  * TODO: add anti-wind up and I and D parts
 522  */
 523 inline void pos_pid(){
 524         int duty_tmp;
 525         duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice);
 526         if (duty_tmp>MAX_DUTY){
 527                 rps.duty=MAX_DUTY;
 528         }else if (duty_tmp<-MAX_DUTY){
 529                 rps.duty=-MAX_DUTY;
 530         }else{
 531                 rps.duty = duty_tmp;
 532         }
 533 }
 534 /*
 535  * \brief
 536  * Very simple PID regulator.
 537  * Now only with P-part so that the error doesnt go to zero.
 538  * FIXME: make better
 539  */
 540 inline void spd_pid(){
 541         int duty_tmp;
 542         duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice);
 543         if (duty_tmp>MAX_DUTY){
 544                 rps.duty=MAX_DUTY;
 545         }else if (duty_tmp<-MAX_DUTY){
 546                 rps.duty=-MAX_DUTY;
 547         }else{
 548                 rps.duty = duty_tmp;
 549         }
 550 }
 551
 552 /*
 553  * \brief
 554  * Computate speed.
 555  */
 556 void compSpeed(){
 557         signed long int spd;
 558         spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM];
 559         rps.speed=(int32_t)spd;
 560 }
 561
 562 /*
 563  * \brief
 564  * Feedback loop.
 565  * TODO: replace bunch of 'IFs' with Object-like pattern
 566  */
 567 void * read_data(void* param){
 568         int i;
 569         struct rpi_in pocatek;
 570         struct timespec t;
 571         int interval = 1000000; /* 1ms ~ 1kHz*/
 572         uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;
 573         char first=1;
 574         uint16_t last_index;                            /*we have index up-to date*/
 575         pocatek = spi_read(tx);
 576         clock_gettime(CLOCK_MONOTONIC ,&t);
 577         /* start after one second */
 578         t.tv_sec++;
 579                 while(1){
 580                         /* wait until next shot */
 581                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
 582                         sem_wait(&rps.thd_par_sem);             /*---take semaphore---*/
 583
 584                         /*old positions*/
 585                         rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
 586                         prepare_tx(tx);                 /*save the data to send*/
 587                         data = spi_read(tx);            /*exchange data*/
 588                         /*subtract initiate postion */
 589                         rps.tf_count++;
 590                         substractOffset(&data,&pocatek);
 591                         compSpeed();                    /*spocita rychlost*/
 592
 593                         if (!rps.index_ok){
 594                                 if (first){
 595                                         last_index=data.index_position;
 596                                         first=0;
 597                                 }else if (last_index!=data.index_position){
 598                                         rps.index_ok=1;
 599                                         comIndDist();   /*vypocet vzdalenosti indexu*/
 600                                 }
 601                         }else{ /*index je v poradku*/
 602                                 comIndDist();           /*vypocet vzdalenosti indexu*/
 603                         }
 604                         /* pocitame sirku plneni podle potreb rizeni*/
 605                         if (rps.pos_reg_ena){
 606                                 pos_pid();
 607                         }
 608                         /* sirka plneni prepoctena na jednotlive pwm */
 609                         if (rps.index_ok && rps.commutate){
 610                                 /*simple_ind_dist_commutator(rps.duty);*/
 611                                 /*sin_commutator(rps.duty);*/
 612                                 inv_trans_comm(rps.duty);
 613                                 inv_trans_comm_2(rps.duty);
 614                         }else if(!rps.index_ok && rps.commutate){
 615                                 simple_hall_commutator(rps.duty);
 616                         }
 617                         sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
 618
 619                         /* calculate next shot */
 620                         t.tv_nsec += interval;
 621
 622                         while (t.tv_nsec >= NSEC_PER_SEC) {
 623                                 t.tv_nsec -= NSEC_PER_SEC;
 624                                 t.tv_sec++;
 625                         }
 626
 627                 }
 628 }
 629
 630
 631 /**
 632  * \brief Main function.
 633  */
 634
 635 int main(){
 636         pthread_t base_thread_id;
 637         clk_init();             /* inicializace gpio hodin */
 638         spi_init();             /* iniicializace spi*/
 639
 640         /*semafor pro detekci zpracovani parametru vlaken*/
 641         sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
 642         setup_environment();
 643
 644         base_thread_id=pthread_self();
 645
 646         /*main control loop*/
 647         create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
 648
 649         /*monitor of current state*/
 650         create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
 651
 652         /*wait for commands*/
 653         poll_cmd(&rps);
 654
 655         return 0;
 656 }