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
  19 #include "rpin.h"       /*gpclk*/
  20 #include "rp_spi.h"     /*spi*/
  21 #include "misc.h"       /*structure for priorities*/
  22
  23
  24 #define PRUM_PROUD      2061
  25 #define PRUM_SOUC       6183
  26
  27 #define PRIOR_KERN      50
  28 #define PRIOR_HIGH      49
  29 #define PRIOR_LOW       20
  30
  31 #define THREAD_SHARED   0
  32 #define INIT_VALUE      0       /*init value for semaphor*/
  33
  34 struct sigaction sighnd; /*struktura pro signal handler*/
  35 struct rpi_in data;
  36
  37 uint8_t test;
  38 uint16_t pwm1, pwm2, pwm3;
  39 char commutate;
  40 uint16_t simple_hall_duty;
  41
  42
  43 /**
  44  * \brief Initilizes GPCLK.
  45  */
  46 int clk_init()
  47 {
  48         initialise(); /*namapovani gpio*/
  49         initClock(PLLD_500_MHZ, 10, 0);
  50         gpioSetMode(4, FSEL_ALT0);
  51         return 0;
  52 }
  53 /*
  54  * \brief Terminates GPCLK.
  55  */
  56
  57 inline void clk_disable(){
  58         termClock(0);
  59 }
  60
  61 /**
  62  * \brief Signal handler pro Ctrl+C
  63  */
  64 void sighnd_fnc(){
  65         spi_disable();
  66         clk_disable();
  67         printf("\nprogram bezpecne ukoncen\n");
  68         exit(0);
  69 }
  70
  71 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
  72         data->pozice_raw=data->pozice;
  73         data->pozice-=offset->pozice;
  74         return;
  75 }
  76 /*
  77  * pocita procentualni odchylku od prumerneho proudu
  78  */
  79 float diff_p(float value){
  80         return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
  81 }
  82 /*
  83  * pocita procentualni odchylku od prumerneho souctu proudu
  84  */
  85 float diff_s(float value){
  86         return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
  87 }
  88 /*
  89  * tiskne potrebna data
  90  */
  91 void printData(struct rpi_in data){
  92         float cur0, cur1, cur2;
  93         int i;
  94         if (data.adc_m_count){
  95                 cur0=data.ch0/data.adc_m_count;
  96                 cur1=data.ch1/data.adc_m_count;
  97                 cur2=data.ch2/data.adc_m_count;
  98         }
  99         for (i = 0; i < 16; i++) {
 100                         if (!(i % 6))
 101                                 puts("");
 102                         printf("%.2X ", data.debug_rx[i]);
 103         }
 104         puts("");
 105         printf("\npozice=%d\n",(int32_t)data.pozice);
 106         printf("raw_pozice=%d\n",(int32_t)data.pozice_raw);
 107         printf("raw_pozice last11=%u\n",(data.pozice_raw&0x7FF));
 108         printf("index position=%d\n",(int16_t)data.index_position);
 109         printf("hal1=%d, hal2=%d, hal3=%d\n",data.hal1,data.hal2,data.hal3);
 110         printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&test),!!(0x20&test),!!(0x10&test));
 111         printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&test),!!(0x04&test),!!(0x02&test));
 112         printf("PWM1=%u(L.s.)\n",pwm1);
 113         printf("PWM2=%u(L.s.)\n",pwm2);
 114         printf("PWM3=%u(L.s.)\n",pwm3);
 115         printf("Pocet namerenych proudu=%u\n",data.adc_m_count);
 116         printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data.ch1,cur1,diff_p(cur1));
 117         printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch2,cur2,diff_p(cur2));
 118         printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch0,cur0,diff_p(cur0));
 119         printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
 120         printf("duty=%u\n",simple_hall_duty);
 121         if (commutate) printf("commutation in progress\n");
 122 }
 123 void prepare_tx(uint8_t * tx){
 124
 125         /*Data format:
 126          * tx[4] - bity 95 downto 88 - bits that are sent first
 127          * tx[5] - bity 87 downto 80
 128          * tx[6] - bity 79 downto 72
 129          * tx[7] - bity 71 downto 64
 130          * tx[8] - bity 63 downto 56
 131          * tx[9] - bity 55 downto 48
 132          * tx[10] - bity 47 downto 40
 133          * tx[11] - bity 39 downto 32
 134          * tx[12] - bity 31 downto 24
 135          * tx[13] - bity 23 downto 16
 136          * tx[14] - bity 15 downto 8
 137          * tx[15] - bity 7 downto 0
 138          *
 139          * bit 95 - ADC reset
 140          * bit 94 - enable PWM1
 141          * bit 93 - enable PWM2
 142          * bit 92 - enable PWM3
 143          * bit 91 - shutdown1
 144          * bit 90 - shutdown2
 145          * bit 89 - shutdown3
 146          *      .
 147          *      .
 148          *      .
 149          * bits 66 .. 56 - match PWM1
 150          * bits 55 .. 45 - match PWM2
 151          * bit 11,12 - Unused
 152          * bits 42 .. 32  - match PWM3
 153          */
 154
 155
 156         uint16_t tmp;
 157
 158         /* keep the cap*/
 159         if (pwm1>2047) pwm1=2047;
 160         if (pwm2>2047) pwm2=2047;
 161         if (pwm3>2047) pwm3=2047;
 162
 163         tx[0]=test; /*bit 94 - enable PWM1*/
 164
 165         /*pwm1*/
 166         tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&pwm1)[1]); /*MSB*/
 167         tx[8]=((uint8_t*)&pwm1)[0]; /*LSB*/
 168
 169         /*pwm2*/
 170         tmp=pwm2;
 171         tmp<<=5;
 172         tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/
 173         tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/
 174
 175         /*pwm3*/
 176         tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&pwm3)[1]); /*MSB*/
 177         tx[11]=((uint8_t*)&pwm3)[0]; /*LSB*/
 178
 179
 180 }
 181 /**
 182  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
 183  */
 184 void * pos_monitor(void* param){
 185         set_priority(param);            /*set priority*/
 186         while(1){
 187                 printData(data);
 188                 usleep(1000000);        /*1 Hz*/
 189         }
 190         return (void*)0;
 191 }
 192 /*
 193  * \brief
 194  * Test function to be placed in controll loop.
 195  * Clockwise rotation with PWM with 64 out of 2048 duty cycle.
 196  */
 197 inline void simple_hall_commutator(struct rpi_in data){
 198         /* pwm3 */
 199         if (data.hal2 && !data.hal3){
 200                 pwm1=0;
 201                 pwm2=0;
 202                 pwm3=simple_hall_duty;
 203         /* pwm1 */
 204         }else if (data.hal1 && !data.hal2){
 205                 pwm1=simple_hall_duty;
 206                 pwm2=0;
 207                 pwm3=0;
 208         /* pwm2 */
 209         }else if (!data.hal1 && data.hal3){
 210                 pwm1=0;
 211                 pwm2=simple_hall_duty;
 212                 pwm3=0;
 213         }
 214 }
 215 /**
 216  * Funkce pravidelne vycita data z motoru
 217  */
 218 void * read_data(void* param){
 219         int i;
 220         struct rpi_in pocatek;
 221         uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;
 222         set_priority(param);                            /*set priority*/
 223         pocatek = spi_read(tx);
 224                 while(1){
 225                         prepare_tx(tx);
 226                         data = spi_read(tx);
 227                         substractOffset(&data,&pocatek);
 228                         if (commutate){
 229                                 simple_hall_commutator(data);
 230                         }
 231                         usleep(1000);                           /*1kHz*/
 232                 }
 233 }
 234
 235 /**
 236  * \brief Main function.
 237  */
 238
 239 int main(){
 240         unsigned int tmp;
 241
 242         /*nastaveni priorit vlaken*/
 243         struct thread_param tsp;
 244         tsp.sch_policy = SCHED_FIFO;
 245
 246         /*nastaveni signalu pro vypnuti pomoci Ctrl+C*/
 247         sighnd.sa_handler=&sighnd_fnc;
 248         sigaction(SIGINT, &sighnd, NULL );
 249
 250         clk_init();             /* inicializace gpio hodin */
 251         spi_init();             /* iniicializace spi*/
 252
 253         /*semafor pro detekci zpracovani parametru vlaken*/
 254         sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE);
 255
 256         /*vlakna*/
 257         pthread_t tid;                  /*identifikator vlakna*/
 258         pthread_attr_t attr;            /*atributy vlakna*/
 259         pthread_attr_init(&attr);       /*inicializuj implicitni atributy*/
 260
 261
 262
 263         /*ziskavani dat z motoru*//*vysoka priorita*/
 264         tsp.sch_prior = PRIOR_HIGH;
 265         pthread_create(&tid, &attr, read_data, (void*)&tsp);
 266
 267         /*vypisovani lokalni pozice*//*nizka priorita*/
 268         tsp.sch_prior = PRIOR_LOW;
 269         pthread_create(&tid, &attr, pos_monitor, (void*)&tsp);
 270
 271
 272         /*muzeme zavrit semafor*/
 273         sem_destroy(&thd_par_sem);
 274         /*
 275          * Note:
 276          * pri pouziti scanf("%u",&simple_hall_duty); dochazelo
 277          * k preukladani hodnot na promenne test. Dost divne.
 278          */
 279         while (1){
 280                 scanf("%u",&tmp);
 281                 printf("volba=%u\n",tmp);
 282                 switch (tmp){
 283                 case 1:
 284                         scanf("%u",&tmp);
 285                         pwm1=tmp&0xFFF;
 286                         break;
 287                 case 2:
 288                         scanf("%u",&tmp);
 289                         pwm2=tmp&0xFFF;
 290                         break;
 291                 case 3:
 292                         scanf("%u",&tmp);
 293                         pwm3=tmp&0xFFF;
 294                         break;
 295                 case 4:
 296                         scanf("%u",&tmp);
 297                         test=tmp&0xFF;
 298                         break;
 299                 case 5:
 300                         commutate=!commutate;
 301                         /* switch off pwms at the end of commutation */
 302                         pwm1&=commutate*0xFFFF;
 303                         pwm2&=commutate*0xFFFF;
 304                         pwm3&=commutate*0xFFFF;
 305                         break;
 306                 case 6:
 307                         scanf("%u",&tmp);
 308                         simple_hall_duty=tmp&0xFFF;
 309                         break;
 310
 311                 default:
 312                         break;
 313                 }
 314
 315         }
 316         return 0;
 317 }