X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/534ec7b705dc81c66501a108dc864256ef32651f..70b98ccdfac6bb0b8284cb0b9b702e5eddb86a7c:/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 4a03c3a..c9b9fe9 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -19,6 +19,8 @@ #include "rpin.h" /*gpclk*/ #include "rp_spi.h" /*spi*/ #include "misc.h" /*structure for priorities*/ +#include "pxmc_sin_fixed.h" /*to test sin commutation */ + #define PRUM_PROUD 2061 @@ -33,11 +35,27 @@ #define THREAD_SHARED 0 #define INIT_VALUE 0 /*init value for semaphor*/ + +#define PXMC_SIN_FIX_TAB_BITS 9 +#define PXMC_SIN_FIX_IDX_SLR 23 +#define PXMC_SIN_FIX_XD_MASK 0x007fffff +#define PXMC_SIN_FIX_XD_SLR 8 +#define PXMC_SIN_FIX_A_MASK 0xffffc000 +#define PXMC_SIN_FIX_B_SLL 19 +#define PXMC_SIN_FIX_B_SAR 16 +#define PXMC_SIN_FIX_B_XD_SAR 6 +#define PXMC_SIN_FIX_ZIC_MASK 0x00002000 +#define PXMC_SIN_FIX_ZIC_BIT 13 + +#define PXMC_SIN_FIX_PI2 0x40000000 +#define PXMC_SIN_FIX_2PI3 0x55555555 + struct sigaction sighnd; /*struktura pro signal handler*/ struct rpi_in data; struct rpi_state{ uint8_t test; uint16_t pwm1, pwm2, pwm3; + uint16_t t_pwm1, t_pwm2, t_pwm3; char commutate; int duty; /* duty cycle of pwm */ uint16_t index_dist; /* distance to index position */ @@ -125,13 +143,14 @@ void printData(){ 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("distance to index=%u\n",s.index_dist); + printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_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)); @@ -166,36 +185,38 @@ void prepare_tx(uint8_t * tx){ * bit 89 - shutdown3 * . * . + * Unused * . - * bits 66 .. 56 - match PWM1 - * bits 55 .. 45 - match PWM2 - * bit 11,12 - Unused - * bits 42 .. 32 - match PWM3 + * . + * bits 47 .. 32 - match PWM1 + * bits 31 .. 16 - match PWM2 + * bits 15 .. 0 - match PWM3 */ uint16_t tmp; - /* keep the cap*/ + /* keep the 11-bit cap*/ + if (rps.pwm1>2047) 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[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&rps.pwm1)[1]); /*MSB*/ - tx[8]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/ + tx[10]=((uint8_t*)&rps.pwm1)[1]; /*MSB*/ + tx[11]=((uint8_t*)&rps.pwm1)[0]; /*LSB*/ /*pwm2*/ - tmp=rps.pwm2; - tmp<<=5; - tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/ - tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/ + tx[12]=((uint8_t*)&rps.pwm2)[1]; /*MSB*/ + tx[13]=((uint8_t*)&rps.pwm2)[0]; /*LSB*/ /*pwm3*/ - tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&rps.pwm3)[1]); /*MSB*/ - tx[11]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/ + tx[14]=((uint8_t*)&rps.pwm3)[1]; /*MSB*/ + tx[15]=((uint8_t*)&rps.pwm3)[0]; /*LSB*/ } @@ -210,13 +231,85 @@ void * pos_monitor(void* param){ } return (void*)0; } + +inline +int sin_commutator(int duty){ + #define DEGREE_60 715827883 + #define DEGREE_120 1431655765 + #define DEGREE_180 2147483648 + #define DEGREE_240 2863311531 + #define DEGREE_300 3579139413 + uint32_t j,pos; + int32_t sin; + uint16_t pwm; + pos=rps.index_dist*4294967; + if (duty>=0){ /*clockwise rotation*/ + /* 1st phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/ + pwm=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); + } + rps.pwm1=pwm; + + /* 2nd phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/ + pwm=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); + } + rps.pwm2=pwm; + + /* 3rd phase */ + sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/ + pwm=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); + } + rps.pwm3=pwm; + }else{ + duty=-duty; + + /* 1st phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/ + pwm=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); + } + rps.pwm1=pwm; + + /* 2nd phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/ + pwm=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); + } + rps.pwm2=pwm; + + /* 3rd phase */ + sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/ + pwm=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j)); + } + rps.pwm3=pwm; + } + return 0; +} /* * \brief * Test function to be placed in controll loop. * Switches PWM's at point where they produce same force. * This points are found thanks to IRC position, */ -inline void simple_ind_dist_commutator(int duty){ +inline +void simple_ind_dist_commutator(int duty){ if (duty>=0){ /* clockwise - so that position increase */ /* pwm3 */ if ((rps.index_dist>=45 && rps.index_dist<=373) || @@ -346,8 +439,8 @@ void * read_data(void* param){ set_priority(param); /*set priority*/ pocatek = spi_read(tx); while(1){ - prepare_tx(tx); /*save the data to send*/ sem_wait(&thd_par_sem); /*---take semaphore---*/ + prepare_tx(tx); /*save the data to send*/ data = spi_read(tx); /*exchange data*/ /*subtract initiate postion */ rps.tf_count++; @@ -363,12 +456,13 @@ void * read_data(void* param){ } pid(); if (rps.index_ok && rps.commutate){ - simple_ind_dist_commutator(rps.duty); + /*simple_ind_dist_commutator(rps.duty);*/ + sin_commutator(rps.duty); }else if(!rps.index_ok && rps.commutate){ simple_hall_commutator(rps.duty); } sem_post(&thd_par_sem); /*--post semaphore---*/ - usleep(1000); /*1kHz*/ + usleep(500); /*1kHz*/ } }