From: Martin Prudek Date: Sat, 18 Apr 2015 16:52:09 +0000 (+0200) Subject: State variables moved to new structure 'rpi_state'. Added semaphore to controll acces... X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/commitdiff_plain/a836c7be2ad5acf66edf1afd1538ffd4fa6f0f4c State variables moved to new structure 'rpi_state'. Added semaphore to controll access to them. Also added computation of distance to index position. --- diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c index 2757542..7e29253 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -33,11 +33,15 @@ struct sigaction sighnd; /*struktura pro signal handler*/ struct rpi_in data; - -uint8_t test; -uint16_t pwm1, pwm2, pwm3; -char commutate; -int simple_hall_duty; +struct rpi_state{ + uint8_t test; + uint16_t pwm1, pwm2, pwm3; + char commutate; + int simple_hall_duty; + uint16_t index_dist; /* distance to index position */ + unsigned char index_ok; + uint32_t tf_count; /*number of transfer*/ +}rps; /** @@ -64,6 +68,8 @@ inline void clk_disable(){ void sighnd_fnc(){ spi_disable(); clk_disable(); + /*muzeme zavrit semafor*/ + sem_destroy(&thd_par_sem); printf("\nprogram bezpecne ukoncen\n"); exit(0); } @@ -88,37 +94,48 @@ float diff_s(float value){ /* * tiskne potrebna data */ -void printData(struct rpi_in data){ +void printData(){ + struct rpi_in data_p; + struct rpi_state s; /*state*/ float cur0, cur1, cur2; int i; - if (data.adc_m_count){ - cur0=data.ch0/data.adc_m_count; - cur1=data.ch1/data.adc_m_count; - cur2=data.ch2/data.adc_m_count; + /* copy the data */ + sem_wait(&thd_par_sem); + data_p = data; + s=rps; + sem_post(&thd_par_sem); + + if (data_p.adc_m_count){ + cur0=data_p.ch0/data_p.adc_m_count; + cur1=data_p.ch1/data_p.adc_m_count; + cur2=data_p.ch2/data_p.adc_m_count; } for (i = 0; i < 16; i++) { if (!(i % 6)) puts(""); - printf("%.2X ", data.debug_rx[i]); + printf("%.2X ", data_p.debug_rx[i]); } puts(""); - printf("\npozice=%d\n",(int32_t)data.pozice); - printf("raw_pozice=%d\n",(int32_t)data.pozice_raw); - printf("raw_pozice last11=%u\n",(data.pozice_raw&0x7FF)); - printf("index position=%d\n",(int16_t)data.index_position); - printf("hal1=%d, hal2=%d, hal3=%d\n",data.hal1,data.hal2,data.hal3); - printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&test),!!(0x20&test),!!(0x10&test)); - printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&test),!!(0x04&test),!!(0x02&test)); - printf("PWM1=%u(L.s.)\n",pwm1); - printf("PWM2=%u(L.s.)\n",pwm2); - printf("PWM3=%u(L.s.)\n",pwm3); - printf("Pocet namerenych proudu=%u\n",data.adc_m_count); - printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data.ch1,cur1,diff_p(cur1)); - printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch2,cur2,diff_p(cur2)); - printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch0,cur0,diff_p(cur0)); + printf("\npozice=%d\n",(int32_t)data_p.pozice); + printf("transfer count=%u\n",s.tf_count); + printf("raw_pozice=%d\n",(int32_t)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("distance to index=%u\n",s.index_dist); + printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3); + printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test)); + printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test)); + printf("PWM1=%u(L.s.)\n",s.pwm1); + printf("PWM2=%u(L.s.)\n",s.pwm2); + printf("PWM3=%u(L.s.)\n",s.pwm3); + printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count); + printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1)); + printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2)); + printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0)); printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2)); - printf("duty=%d\n",simple_hall_duty); - if (commutate) printf("commutation in progress\n"); + printf("duty=%d\n",s.simple_hall_duty); + if (s.index_ok) printf("index ok\n"); + if (s.commutate) printf("commutation in progress\n"); } void prepare_tx(uint8_t * tx){ @@ -156,25 +173,25 @@ void prepare_tx(uint8_t * tx){ uint16_t tmp; /* keep the cap*/ - if (pwm1>2047) pwm1=2047; - if (pwm2>2047) pwm2=2047; - if (pwm3>2047) pwm3=2047; + if (rps.pwm1>2047) rps.pwm1=2047; + if (rps.pwm2>2047) rps.pwm2=2047; + if (rps.pwm3>2047) rps.pwm3=2047; - tx[0]=test; /*bit 94 - enable PWM1*/ + tx[0]=rps.test; /*bit 94 - enable PWM1*/ /*pwm1*/ - tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&pwm1)[1]); /*MSB*/ - tx[8]=((uint8_t*)&pwm1)[0]; /*LSB*/ + tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&rps.pwm1)[1]); /*MSB*/ + tx[8]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/ /*pwm2*/ - tmp=pwm2; + tmp=rps.pwm2; tmp<<=5; tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/ tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/ /*pwm3*/ - tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&pwm3)[1]); /*MSB*/ - tx[11]=((uint8_t*)&pwm3)[0]; /*LSB*/ + tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&rps.pwm3)[1]); /*MSB*/ + tx[11]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/ } @@ -184,7 +201,7 @@ void prepare_tx(uint8_t * tx){ void * pos_monitor(void* param){ set_priority(param); /*set priority*/ while(1){ - printData(data); + printData(); usleep(1000000); /*1 Hz*/ } return (void*)0; @@ -198,55 +215,80 @@ inline void simple_hall_commutator(struct rpi_in data, int duty){ if (duty>=0){ /* clockwise - so that position increase */ /* pwm3 */ if (data.hal2 && !data.hal3){ - pwm1=0; - pwm2=0; - pwm3=duty; + rps.pwm1=0; + rps.pwm2=0; + rps.pwm3=duty; /* pwm1 */ }else if (data.hal1 && !data.hal2){ - pwm1=duty; - pwm2=0; - pwm3=0; + rps.pwm1=duty; + rps.pwm2=0; + rps.pwm3=0; /* pwm2 */ }else if (!data.hal1 && data.hal3){ - pwm1=0; - pwm2=duty; - pwm3=0; + rps.pwm1=0; + rps.pwm2=duty; + rps.pwm3=0; } }else{ /*counter-clockwise - position decrease */ /* pwm3 */ if (!data.hal2 && data.hal3){ - pwm1=0; - pwm2=0; - pwm3=-duty; + rps.pwm1=0; + rps.pwm2=0; + rps.pwm3=-duty; /* pwm1 */ }else if (!data.hal1 && data.hal2){ - pwm1=-duty; - pwm2=0; - pwm3=0; + rps.pwm1=-duty; + rps.pwm2=0; + rps.pwm3=0; /* pwm2 */ }else if (data.hal1 && !data.hal3){ - pwm1=0; - pwm2=-duty; - pwm3=0; + rps.pwm1=0; + rps.pwm2=-duty; + rps.pwm3=0; } } } /** * Funkce pravidelne vycita data z motoru */ +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 * read_data(void* param){ int i; struct rpi_in pocatek; 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); while(1){ - prepare_tx(tx); - data = spi_read(tx); + prepare_tx(tx); /*save the data to send*/ + sem_wait(&thd_par_sem); /*---take semaphore---*/ + data = spi_read(tx); /*exchange data*/ + /*subtract initiate postion */ + rps.tf_count++; substractOffset(&data,&pocatek); - if (commutate){ - simple_hall_commutator(data,simple_hall_duty); + comIndDist(); + if (!rps.index_ok){ + if (first){ + last_index=data.index_position; + first=0; + }else if (last_index!=data.index_position){ + rps.index_ok=1; + } + } + if (rps.index_ok && rps.commutate){ + simple_hall_commutator(data,rps.simple_hall_duty); + }else if(!rps.index_ok && rps.commutate){ + simple_hall_commutator(data,rps.simple_hall_duty); } + sem_post(&thd_par_sem); /*--post semaphore---*/ usleep(1000); /*1kHz*/ } } @@ -285,11 +327,11 @@ int main(){ /*vypisovani lokalni pozice*//*nizka priorita*/ tsp.sch_prior = PRIOR_LOW; + sem_wait(&thd_par_sem); pthread_create(&tid, &attr, pos_monitor, (void*)&tsp); - /*muzeme zavrit semafor*/ - sem_destroy(&thd_par_sem); + /* * Note: * pri pouziti scanf("%u",&simple_hall_duty); dochazelo @@ -301,30 +343,42 @@ int main(){ switch (tmp){ case 1: scanf("%u",&tmp); - pwm1=tmp&0xFFF; + sem_wait(&thd_par_sem); + rps.pwm1=tmp&0xFFF; + sem_post(&thd_par_sem); break; case 2: scanf("%u",&tmp); - pwm2=tmp&0xFFF; + sem_wait(&thd_par_sem); + rps.pwm2=tmp&0xFFF; + sem_post(&thd_par_sem); break; case 3: scanf("%u",&tmp); - pwm3=tmp&0xFFF; + sem_wait(&thd_par_sem); + rps.pwm3=tmp&0xFFF; + sem_post(&thd_par_sem); break; case 4: scanf("%u",&tmp); - test=tmp&0xFF; + sem_wait(&thd_par_sem); + rps.test=tmp&0xFF; + sem_post(&thd_par_sem); break; case 5: - commutate=!commutate; + sem_wait(&thd_par_sem); + rps.commutate=!rps.commutate; /* switch off pwms at the end of commutation */ - pwm1&=commutate*0xFFFF; - pwm2&=commutate*0xFFFF; - pwm3&=commutate*0xFFFF; + rps.pwm1&=rps.commutate*0xFFFF; + rps.pwm2&=rps.commutate*0xFFFF; + rps.pwm3&=rps.commutate*0xFFFF; + sem_post(&thd_par_sem); break; case 6: scanf("%d",&tmp); - simple_hall_duty=tmp; + sem_wait(&thd_par_sem); + rps.simple_hall_duty=tmp; + sem_post(&thd_par_sem); break; default: