X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/fa2db689500f7faf91c5127ada91170e6b06f4df..067296cf27a01a3cb766e31c44d313707964d9c3:/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 5195150..6e539b2 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -34,7 +34,7 @@ #define PRIOR_LOW 20 #define THREAD_SHARED 0 -#define INIT_VALUE 0 /*init value for semaphor*/ +#define INIT_VALUE 1 /*init value for semaphor*/ #define PXMC_SIN_FIX_TAB_BITS 9 @@ -53,7 +53,8 @@ #define NSEC_PER_SEC (1000000000) /* The number of nsecs per sec. */ -struct sigaction sighnd; /*struktura pro signal handler*/ + + struct rpi_in data; struct rpi_state{ uint8_t test; @@ -89,13 +90,12 @@ inline void clk_disable(){ /** * \brief Signal handler pro Ctrl+C */ -void sighnd_fnc(){ +void appl_stop(){ spi_disable(); clk_disable(); /*muzeme zavrit semafor*/ sem_destroy(&thd_par_sem); printf("\nprogram bezpecne ukoncen\n"); - exit(0); } void substractOffset(struct rpi_in* data, struct rpi_in* offset){ @@ -143,7 +143,7 @@ void printData(){ printf("\npozice=%d\n",(int32_t)data_p.pozice); printf("chtena pozice=%d\n",s.desired_pos); printf("transfer count=%u\n",s.tf_count); - printf("raw_pozice=%d\n",(int32_t)data_p.pozice_raw); + printf("raw_pozice=%u\n",data_p.pozice_raw); printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF)); printf("index position=%u\n",data_p.index_position); printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3); @@ -227,7 +227,6 @@ void prepare_tx(uint8_t * tx){ * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru */ void * pos_monitor(void* param){ - set_priority(param); /*set priority*/ while(1){ printData(); usleep(1000000); /*1 Hz*/ @@ -400,15 +399,66 @@ inline void simple_hall_commutator(int duty){ } } /** - * Funkce pravidelne vycita data z motoru + * \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 */ -inline void comIndDist(){ - rps.index_dist=0x0FFF & (data.pozice_raw - data.index_position); - /* - * if distance is bigger than 2047, the distance underflown - * -> if 12th bit is set, substract 2096 - */ - rps.index_dist-=((rps.index_dist & 0x0800)>>11)*2096; +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 @@ -439,7 +489,6 @@ void * read_data(void* param){ 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*/ - set_priority(param); /*set priority*/ pocatek = spi_read(tx); clock_gettime(CLOCK_MONOTONIC ,&t); /* start after one second */ @@ -482,45 +531,13 @@ void * read_data(void* param){ } } + /** - * \brief Main function. + * \brief + * Commands detection. */ - -int main(){ +void poll_cmd(){ unsigned int tmp; - - /*nastaveni priorit vlaken*/ - struct thread_param tsp; - tsp.sch_policy = SCHED_FIFO; - - /*nastaveni signalu pro vypnuti pomoci Ctrl+C*/ - sighnd.sa_handler=&sighnd_fnc; - sigaction(SIGINT, &sighnd, NULL ); - - clk_init(); /* inicializace gpio hodin */ - spi_init(); /* iniicializace spi*/ - - /*semafor pro detekci zpracovani parametru vlaken*/ - sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE); - - /*vlakna*/ - pthread_t tid; /*identifikator vlakna*/ - pthread_attr_t attr; /*atributy vlakna*/ - pthread_attr_init(&attr); /*inicializuj implicitni atributy*/ - - - - /*ziskavani dat z motoru*//*vysoka priorita*/ - tsp.sch_prior = PRIOR_HIGH; - pthread_create(&tid, &attr, read_data, (void*)&tsp); - - /*vypisovani lokalni pozice*//*nizka priorita*/ - tsp.sch_prior = PRIOR_LOW; - sem_wait(&thd_par_sem); - pthread_create(&tid, &attr, pos_monitor, (void*)&tsp); - - - /* * Note: * pri pouziti scanf("%u",&simple_hall_duty); dochazelo @@ -581,6 +598,31 @@ int main(){ } } - return 0; + return ; } +/** + * \brief Main function. + */ +int main(){ + pthread_t base_thread_id; + clk_init(); /* inicializace gpio hodin */ + spi_init(); /* iniicializace spi*/ + + /*semafor pro detekci zpracovani parametru vlaken*/ + sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE); + setup_environment(); + + base_thread_id=pthread_self(); + + /*main control loop*/ + create_rt_task(&base_thread_id,PRIOR_HIGH,read_data,NULL); + + /*monitor of current state*/ + create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL); + + /*wait for commands*/ + poll_cmd(); + + return 0; +}