X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/f747a68dd84aab260a8553829ae5da1ee4bf321a..8f3fb13ef9c8da9ac312b3b3b0ea4239306b986f:/pmsm-control/test_sw/main_pmsm.c diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c index a9c6329..25b0acc 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -25,9 +25,8 @@ #include "cmd_proc.h" #include "controllers.h" #include "commutators.h" - - - +#include "comp.h" +#include "logs.h" #define PRIOR_KERN 50 #define PRIOR_HIGH 49 @@ -82,129 +81,6 @@ inline void clk_disable(){ termClock(0); } - -/* - * \brief - * Free logs - */ -void freeLogs(){ - int r; - if (rps.log_col_count){ - for (r=0;rpozice; - - rps.logs[2][rps.log_col]=(int)rps.pwm1; - rps.logs[3][rps.log_col]=(int)rps.pwm2; - rps.logs[4][rps.log_col]=(int)rps.pwm3; - rps.logs[5][rps.log_col]=rps.duty; - - rps.logs[6][rps.log_col]=rps.desired_spd; - rps.logs[7][rps.log_col]=rps.speed; - - rps.logs[8][rps.log_col]=(int)(rps.spi_dat->ch1/rps.spi_dat->adc_m_count); - rps.logs[9][rps.log_col]=(int)(rps.spi_dat->ch2/rps.spi_dat->adc_m_count); - rps.logs[10][rps.log_col]=(int)(rps.spi_dat->ch0/rps.spi_dat->adc_m_count); - - rps.log_col++; - /* - if (rps.log_col==rps.log_col_count-1){ - rps.log_col_count*=2; - rps.log_col_count%=MAX_LOGS; - for (r=0;r2047) rps.pwm1=2047; - if (rps.pwm2>2047) rps.pwm2=2047; - if (rps.pwm3>2047) rps.pwm3=2047; - - tx[0]=rps.test; /*bit 94 - enable PWM1*/ - - /*now we have to switch the bytes due to endianess */ - /* ARMv6 & ARMv7 instructions are little endian */ - /*pwm1*/ - tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/ - tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/ - - /*pwm2*/ - tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/ - tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/ - - /*pwm3*/ - tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/ - tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/ - - -} - - /** * \brief Signal handler pro Ctrl+C */ @@ -216,24 +92,16 @@ void appl_stop(){ rps.pwm1=0; rps.pwm2=0; rps.pwm3=0; - prepare_tx(tx); /*save the data to send*/ - data=spi_read(tx); + spi_read(&rps); spi_disable(); clk_disable(); - freeLogs(); + freeLogs(&rps); /*muzeme zavrit semafor*/ sem_destroy(&rps.thd_par_sem); printf("\nprogram bezpecne ukoncen\n"); } -void substractOffset(struct rpi_in* data, struct rpi_in* offset){ - data->pozice=data->pozice_raw-offset->pozice_raw; - return; -} - - - /** * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru */ @@ -244,95 +112,6 @@ void * pos_monitor(void* param){ } return (void*)0; } -/* - * \brief - * Multiplication of 11 bit - * Zaporne vysledky prvede na nulu. - */ -inline uint16_t mult_cap(int32_t s,int d){ - int j; - int res=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j)); - } - return res; -} - - -/** - * \brief - * Computation of distance to index. - * - * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu - * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu - - * to je 3999 bodu - * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate - * signalu indexu - */ -void comIndDist(){ - uint16_t pos = 0x0FFF & data.pozice_raw; - uint16_t dist; - uint16_t index = data.index_position; - - if (index<1999){ /*index e<0,1998> */ - if (pos */ - /*proti smeru h.r. od indexu*/ - dist=pos+2000-index; - }else if (pos<=index+1999){ /*pozice e */ - /*po smeru h.r. od indexu*/ - dist=pos-index; - }else if (pos */ - goto index_lost; - }else{ /*pozice e */ - /*proti smeru h.r. od indexu - podtecena pozice*/ - dist=pos-index-2096; - } - }else if (index<=2096){ /*index e<1999,2096>*/ - if (pos */ - goto index_lost; - }else if (pos */ - /*proti smeru h.r. od indexu*/ - dist=pos+2000-index; - }else if (pos<=index+1999){ /*pozice e */ - /*po smeru h.r. od indexu*/ - dist=pos-index; - }else { /*pozice e */ - goto index_lost; - } - }else{ /*index e<2097,4095> */ - if (pos<=index-2097){ /*pozice e<0,index-2097> */ - /*po smeru h.r. od indexu - pretecena pozice*/ - dist=4096+pos-index; - }else if (pos */ - goto index_lost; - }else if (pos */ - /*proti smeru h.r. od indexu*/ - dist=pos+2000-index; - }else{ /*pozice e */ - /*po smeru h.r. od indexu*/ - dist=pos-index; - } - } - - rps.index_dist = dist; - return; - - index_lost: - rps.index_ok=0; - return; -} - - -/* - * \brief - * Computate speed. - */ -void compSpeed(){ - signed long int spd; - spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM]; - rps.speed=(int32_t)spd; -} /* * \brief @@ -342,12 +121,16 @@ void compSpeed(){ void * read_data(void* param){ int i; struct rpi_in pocatek; + struct rpi_state poc={ + .spi_dat=&pocatek, + .test=0, + .pwm1=0, .pwm1=0, .pwm3=0 + }; struct timespec t; int interval = 1000000; /* 1ms ~ 1kHz*/ - uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ; char first=1; uint16_t last_index; /*we have index up-to date*/ - pocatek = spi_read(tx); + spi_read(&poc); /*pocatecni informace*/ clock_gettime(CLOCK_MONOTONIC ,&t); /* start after one second */ t.tv_sec++; @@ -358,12 +141,11 @@ void * read_data(void* param){ /*old positions*/ rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice; - prepare_tx(tx); /*save the data to send*/ - data = spi_read(tx); /*exchange data*/ + spi_read(&rps); /*exchange data*/ /*subtract initiate postion */ rps.tf_count++; - substractOffset(&data,&pocatek); - compSpeed(); /*spocita rychlost*/ + substractOffset(&data,poc.spi_dat); + compSpeed(&rps); /*spocita rychlost*/ if (!rps.index_ok){ if (first){ @@ -371,10 +153,10 @@ void * read_data(void* param){ first=0; }else if (last_index!=data.index_position){ rps.index_ok=1; - comIndDist(); /*vypocet vzdalenosti indexu*/ + comIndDist(&rps); /*vypocet vzdalenosti indexu*/ } }else{ /*index je v poradku*/ - comIndDist(); /*vypocet vzdalenosti indexu*/ + comIndDist(&rps); /*vypocet vzdalenosti indexu*/ } /* pocitame sirku plneni podle potreb rizeni*/ @@ -396,7 +178,7 @@ void * read_data(void* param){ /*zalogujeme hodnoty*/ if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){ - makeLog(); + makeLog(&rps); } sem_post(&rps.thd_par_sem); /*--post semaphore---*/