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 "pmsm_state.h"
  25 #include "cmd_proc.h"
  26 #include "controllers.h"
  27 #include "commutators.h"
  28
  29
  30
  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         //.MAX_DUTY=170,
  46         .spi_dat=&data,
  47         .test=0,
  48         .pwm1=0,.pwm2=0, .pwm3=0,
  49         .pwm1=0, .t_pwm2=0, .t_pwm3=0,
  50         .commutate=0,
  51         .duty=0,                        /* duty cycle of pwm */
  52         .index_dist=0,          /* distance to index position */
  53         .index_ok=0,
  54         .tf_count=0,            /*number of transfer*/
  55         .desired_pos=0,         /* desired position */
  56         .pos_reg_ena=0,
  57         .desired_spd=0,
  58         .spd_reg_ena=0,
  59         .old_pos={0},
  60         .spd_err_sum=0,
  61         .log_col_count=0,       /* pocet radku zaznamu */
  62         .log_col=0,
  63         .doLogs=0,
  64         .alpha_offset=960
  65 };
  66
  67 /**
  68  * \brief Initilizes GPCLK.
  69  */
  70 int clk_init()
  71 {
  72         initialise(); /*namapovani gpio*/
  73         initClock(PLLD_500_MHZ, 10, 0);
  74         gpioSetMode(4, FSEL_ALT0);
  75         return 0;
  76 }
  77 /*
  78  * \brief Terminates GPCLK.
  79  */
  80
  81 inline void clk_disable(){
  82         termClock(0);
  83 }
  84
  85
  86 /*
  87  * \brief
  88  * Free logs
  89  */
  90 void freeLogs(){
  91         int r;
  92         if (rps.log_col_count){
  93                 for (r=0;r<LOG_ROWS;r++){
  94                         free(rps.logs[r]);
  95                 }
  96         }
  97         rps.log_col_count=0;
  98         rps.doLogs=0;
  99 }
 100
 101 /*
 102  * \brief
 103  * Makes log.
 104  */
 105 void makeLog(){
 106         int r;
 107         if (rps.log_col==MAX_LOGS-1){
 108                 rps.doLogs=0;
 109                 return;
 110         }
 111         rps.logs[0][rps.log_col]=(int)rps.tf_count;
 112         rps.logs[1][rps.log_col]=(int)rps.spi_dat->pozice;
 113
 114         rps.logs[2][rps.log_col]=(int)rps.pwm1;
 115         rps.logs[3][rps.log_col]=(int)rps.pwm2;
 116         rps.logs[4][rps.log_col]=(int)rps.pwm3;
 117         rps.logs[5][rps.log_col]=rps.duty;
 118
 119         rps.logs[6][rps.log_col]=rps.desired_spd;
 120         rps.logs[7][rps.log_col]=rps.speed;
 121
 122         rps.logs[8][rps.log_col]=(int)(rps.spi_dat->ch1/rps.spi_dat->adc_m_count);
 123         rps.logs[9][rps.log_col]=(int)(rps.spi_dat->ch2/rps.spi_dat->adc_m_count);
 124         rps.logs[10][rps.log_col]=(int)(rps.spi_dat->ch0/rps.spi_dat->adc_m_count);
 125
 126         rps.log_col++;
 127      /*
 128         if (rps.log_col==rps.log_col_count-1){
 129                 rps.log_col_count*=2;
 130                 rps.log_col_count%=MAX_LOGS;
 131                 for (r=0;r<LOG_ROWS;r++){
 132                         rps.logs[r]=realloc(rps.logs[r],rps.log_col_count*sizeof(int));
 133                         if (rps.logs[r]==NULL){
 134                                 rps.doLogs=0;
 135                                 rps.error=1;
 136                         }
 137                 }
 138         }
 139      */
 140 }
 141
 142 /*
 143  * \brief
 144  * Pripravi psi buffer
 145  */
 146 void prepare_tx(uint8_t * tx){
 147
 148         /*Data format:
 149          * tx[4] - bity 95 downto 88 - bits that are sent first
 150          * tx[5] - bity 87 downto 80
 151          * tx[6] - bity 79 downto 72
 152          * tx[7] - bity 71 downto 64
 153          * tx[8] - bity 63 downto 56
 154          * tx[9] - bity 55 downto 48
 155          * tx[10] - bity 47 downto 40
 156          * tx[11] - bity 39 downto 32
 157          * tx[12] - bity 31 downto 24
 158          * tx[13] - bity 23 downto 16
 159          * tx[14] - bity 15 downto 8
 160          * tx[15] - bity 7 downto 0
 161          *
 162          * bit 95 - ADC reset
 163          * bit 94 - enable PWM1
 164          * bit 93 - enable PWM2
 165          * bit 92 - enable PWM3
 166          * bit 91 - shutdown1
 167          * bit 90 - shutdown2
 168          * bit 89 - shutdown3
 169          *      .
 170          *      .
 171          *      Unused
 172          *      .
 173          *      .
 174          * bits 47 .. 32 - match PWM1
 175          * bits 31 .. 16 - match PWM2
 176          * bits 15 .. 0  - match PWM3
 177          */
 178
 179
 180         uint16_t tmp;
 181
 182         /* keep the 11-bit cap*/
 183
 184         if (rps.pwm1>2047) rps.pwm1=2047;
 185         if (rps.pwm2>2047) rps.pwm2=2047;
 186         if (rps.pwm3>2047) rps.pwm3=2047;
 187
 188         tx[0]=rps.test; /*bit 94 - enable PWM1*/
 189
 190         /*now we have to switch the bytes due to endianess */
 191         /* ARMv6 & ARMv7 instructions are little endian */
 192         /*pwm1*/
 193         tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/
 194         tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/
 195
 196         /*pwm2*/
 197         tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/
 198         tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/
 199
 200         /*pwm3*/
 201         tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/
 202         tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/
 203
 204
 205 }
 206
 207
 208 /**
 209  * \brief Signal handler pro Ctrl+C
 210  */
 211 void appl_stop(){
 212         uint8_t tx[16];
 213         sem_wait(&rps.thd_par_sem);
 214
 215         memset(tx,0,16*sizeof(int));
 216         rps.pwm1=0;
 217         rps.pwm2=0;
 218         rps.pwm3=0;
 219         prepare_tx(tx);                 /*save the data to send*/
 220         data=spi_read(tx);
 221
 222         spi_disable();
 223         clk_disable();
 224         freeLogs();
 225         /*muzeme zavrit semafor*/
 226         sem_destroy(&rps.thd_par_sem);
 227         printf("\nprogram bezpecne ukoncen\n");
 228 }
 229
 230 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
 231         data->pozice=data->pozice_raw-offset->pozice_raw;
 232         return;
 233 }
 234
 235
 236
 237 /**
 238  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 239  */
 240 void * pos_monitor(void* param){
 241         while(1){
 242                 printData(&rps);
 243                 usleep(1000000);        /*1 Hz*/
 244         }
 245         return (void*)0;
 246 }
 247 /*
 248  * \brief
 249  * Multiplication of 11 bit
 250  * Zaporne vysledky prvede na nulu.
 251  */
 252 inline uint16_t mult_cap(int32_t s,int d){
 253         int j;
 254         int res=0;
 255         for(j=0;j!=11;j++){
 256                 /* multiplicate as if maximum sinus value was unity */
 257                 res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
 258         }
 259         return res;
 260 }
 261
 262
 263 /**
 264  * \brief
 265  * Computation of distance to index.
 266  *
 267  * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
 268  * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
 269  *      to je 3999 bodu
 270  *      =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
 271  *              signalu indexu
 272  */
 273 void comIndDist(){
 274         uint16_t pos = 0x0FFF & data.pozice_raw;
 275         uint16_t dist;
 276         uint16_t index = data.index_position;
 277
 278         if (index<1999){                /*index e<0,1998> */
 279                 if (pos<index){                 /*pozice e<0,index-1> */
 280                         /*proti smeru h.r. od indexu*/
 281                         dist=pos+2000-index;
 282                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 283                         /*po smeru h.r. od indexu*/
 284                         dist=pos-index;
 285                 }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
 286                         goto index_lost;
 287                 }else{                          /*pozice e<index+2096,4095> */
 288                         /*proti smeru h.r. od indexu - podtecena pozice*/
 289                         dist=pos-index-2096;
 290                 }
 291         }else if (index<=2096){         /*index e<1999,2096>*/
 292                 if (pos<index-1999){            /*pozice e<0,index-2000> */
 293                         goto index_lost;
 294                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 295                         /*proti smeru h.r. od indexu*/
 296                         dist=pos+2000-index;
 297                 }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
 298                         /*po smeru h.r. od indexu*/
 299                         dist=pos-index;
 300                 }else {                         /*pozice e<index+2000,4095> */
 301                         goto index_lost;
 302                 }
 303         }else{                          /*index e<2097,4095> */
 304                 if (pos<=index-2097){           /*pozice e<0,index-2097> */
 305                         /*po smeru h.r. od indexu - pretecena pozice*/
 306                         dist=4096+pos-index;
 307                 }else if (pos<index-1999){      /*pozice e<index-2096,index-2000> */
 308                         goto index_lost;
 309                 }else if (pos<index){           /*pozice e<index-1999,index-1> */
 310                         /*proti smeru h.r. od indexu*/
 311                         dist=pos+2000-index;
 312                 }else{                          /*pozice e<index,4095> */
 313                         /*po smeru h.r. od indexu*/
 314                         dist=pos-index;
 315                 }
 316         }
 317
 318         rps.index_dist = dist;
 319         return;
 320
 321         index_lost:
 322                 rps.index_ok=0;
 323                 return;
 324 }
 325
 326
 327 /*
 328  * \brief
 329  * Computate speed.
 330  */
 331 void compSpeed(){
 332         signed long int spd;
 333         spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM];
 334         rps.speed=(int32_t)spd;
 335 }
 336
 337 /*
 338  * \brief
 339  * Feedback loop.
 340  * TODO: replace bunch of 'IFs' with Object-like pattern
 341  */
 342 void * read_data(void* param){
 343         int i;
 344         struct rpi_in pocatek;
 345         struct timespec t;
 346         int interval = 1000000; /* 1ms ~ 1kHz*/
 347         uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;
 348         char first=1;
 349         uint16_t last_index;                            /*we have index up-to date*/
 350         pocatek = spi_read(tx);
 351         clock_gettime(CLOCK_MONOTONIC ,&t);
 352         /* start after one second */
 353         t.tv_sec++;
 354                 while(1){
 355                         /* wait until next shot */
 356                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
 357                         sem_wait(&rps.thd_par_sem);             /*---take semaphore---*/
 358
 359                         /*old positions*/
 360                         rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
 361                         prepare_tx(tx);                 /*save the data to send*/
 362                         data = spi_read(tx);            /*exchange data*/
 363                         /*subtract initiate postion */
 364                         rps.tf_count++;
 365                         substractOffset(&data,&pocatek);
 366                         compSpeed();                    /*spocita rychlost*/
 367
 368                         if (!rps.index_ok){
 369                                 if (first){
 370                                         last_index=data.index_position;
 371                                         first=0;
 372                                 }else if (last_index!=data.index_position){
 373                                         rps.index_ok=1;
 374                                         comIndDist();   /*vypocet vzdalenosti indexu*/
 375                                 }
 376                         }else{ /*index je v poradku*/
 377                                 comIndDist();           /*vypocet vzdalenosti indexu*/
 378                         }
 379
 380                         /* pocitame sirku plneni podle potreb rizeni*/
 381                         if (rps.pos_reg_ena){           /*pozicni rizeni*/
 382                                 pos_pid(&rps);
 383                         }else if(rps.spd_reg_ena){      /*rizeni na rychlost*/
 384                                 spd_pid(&rps);
 385                         }
 386
 387                         /* sirka plneni prepoctena na jednotlive pwm */
 388                         if (rps.index_ok && rps.commutate){
 389                                 /*simple_ind_dist_commutator(rps.duty);*/
 390                                 /*sin_commutator(rps.duty);*/
 391                                 inv_trans_comm(&rps);
 392                                 inv_trans_comm_2(&rps);
 393                         }else if(!rps.index_ok && rps.commutate){
 394                                 simple_hall_commutator(&rps);
 395                         }
 396
 397                         /*zalogujeme hodnoty*/
 398                         if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
 399                                 makeLog();
 400                         }
 401
 402                         sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
 403
 404                         /* calculate next shot */
 405                         t.tv_nsec += interval;
 406
 407                         while (t.tv_nsec >= NSEC_PER_SEC) {
 408                                 t.tv_nsec -= NSEC_PER_SEC;
 409                                 t.tv_sec++;
 410                         }
 411
 412                 }
 413 }
 414
 415
 416 /**
 417  * \brief Main function.
 418  */
 419
 420 int main(){
 421         pthread_t base_thread_id;
 422         clk_init();             /* inicializace gpio hodin */
 423         spi_init();             /* iniicializace spi*/
 424
 425         /*semafor pro detekci zpracovani parametru vlaken*/
 426         sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
 427         setup_environment();
 428
 429         base_thread_id=pthread_self();
 430
 431         /*main control loop*/
 432         create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL);
 433
 434         /*monitor of current state*/
 435         create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
 436
 437         /*wait for commands*/
 438         poll_cmd(&rps);
 439
 440         return 0;
 441 }