Complete commutation keeping vector of stator magnetic field perpendicular to vector...

[fpga/rpi-motor-control.git] / 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 4a03c3adff0246e6f76967275807c3f92fea714a..5759c970e92d0ceaf0fc745ba9bd565d4bdebf6f 100644 (file)
--- 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 "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
 
 
 #define PRUM_PROUD     2061


@@ -33,11 +35,27 @@
 #define THREAD_SHARED  0
 #define INIT_VALUE     0       /*init value for semaphor*/
 
 #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;
 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 */
        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("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("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));
        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));


@@ -177,6 +196,7 @@ void prepare_tx(uint8_t * tx){
        uint16_t tmp;
 
        /* keep the cap*/
        uint16_t tmp;
 
        /* keep the cap*/

+

        if (rps.pwm1>2047) rps.pwm1=2047;
        if (rps.pwm2>2047) rps.pwm2=2047;
        if (rps.pwm3>2047) rps.pwm3=2047;
        if (rps.pwm1>2047) rps.pwm1=2047;
        if (rps.pwm2>2047) rps.pwm2=2047;
        if (rps.pwm3>2047) rps.pwm3=2047;


@@ -210,13 +230,85 @@ void * pos_monitor(void* param){
        }
        return (void*)0;
 }
        }
        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,
  */
 /*
  * \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) ||
        if (duty>=0){ /* clockwise - so that position increase */
                /* pwm3 */
                if ((rps.index_dist>=45 && rps.index_dist<=373) ||


@@ -363,7 +455,8 @@ void * read_data(void* param){
                        }
                        pid();
                        if (rps.index_ok && rps.commutate){
                        }
                        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);
                        }
                        }else if(!rps.index_ok && rps.commutate){
                                simple_hall_commutator(rps.duty);
                        }