Speed, index and position computations moved to separate file.

diff --git a/pmsm-control/test_sw/Makefile b/pmsm-control/test_sw/Makefile
index 24d2ea56de1f288b1727eedd1753dd40f36b59d7..778b3368916132ce56afae7dba12dc4640f3c7ba 100644 (file)
--- a/pmsm-control/test_sw/Makefile
+++ b/pmsm-control/test_sw/Makefile
@@ -49,5 +49,5 @@ blikej: howto_gpio.o rpi_hw.o
 spi: rp_spi.o
        gcc -o spi rp_spi.c
 #pro rpi
 spi: rp_spi.o
        gcc -o spi rp_spi.c
 #pro rpi

-pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o
-       gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o  misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o -lpthread -lrt
+pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o comp.o
+       gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o  misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o comp.o -lpthread -lrt

diff --git a/pmsm-control/test_sw/comp.c b/pmsm-control/test_sw/comp.c
new file mode 100644 (file)
index 0000000..49d3546
--- /dev/null
+++ b/pmsm-control/test_sw/comp.c
@@ -0,0 +1,108 @@
+/**
+ * \brief
+ * Computatations of position, index, speed.
+ *
+ */
+
+#include "comp.h"
+#include "pmsm_state.h"
+#include "rp_spi.h"
+
+/**
+ * \brief
+ * Substact initial position.
+ */
+void substractOffset(struct rpi_in* data, struct rpi_in* offset){
+       data->pozice=data->pozice_raw-offset->pozice_raw;
+       return;
+}
+
+/*
+ * \brief
+ * Multiplication of 11 bit
+ * Zaporne vysledky prvede na nulu.
+ */
+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(struct rpi_state* this){
+       uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw;
+       uint16_t dist;
+       uint16_t index = this->spi_dat->index_position;
+
+       if (index<1999){                /*index e<0,1998> */
+               if (pos<index){                 /*pozice e<0,index-1> */
+                       /*proti smeru h.r. od indexu*/
+                       dist=pos+2000-index;
+               }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
+                       /*po smeru h.r. od indexu*/
+                       dist=pos-index;
+               }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
+                       goto index_lost;
+               }else{                          /*pozice e<index+2096,4095> */
+                       /*proti smeru h.r. od indexu - podtecena pozice*/
+                       dist=pos-index-2096;
+               }
+       }else if (index<=2096){         /*index e<1999,2096>*/
+               if (pos<index-1999){            /*pozice e<0,index-2000> */
+                       goto index_lost;
+               }else if (pos<index){           /*pozice e<index-1999,index-1> */
+                       /*proti smeru h.r. od indexu*/
+                       dist=pos+2000-index;
+               }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
+                       /*po smeru h.r. od indexu*/
+                       dist=pos-index;
+               }else {                         /*pozice e<index+2000,4095> */
+                       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<index-1999){      /*pozice e<index-2096,index-2000> */
+                       goto index_lost;
+               }else if (pos<index){           /*pozice e<index-1999,index-1> */
+                       /*proti smeru h.r. od indexu*/
+                       dist=pos+2000-index;
+               }else{                          /*pozice e<index,4095> */
+                       /*po smeru h.r. od indexu*/
+                       dist=pos-index;
+               }
+       }
+
+       this->index_dist = dist;
+       return;
+
+       index_lost:
+               this->index_ok=0;
+               return;
+}
+
+
+/*
+ * \brief
+ * Computate speed.
+ */
+void compSpeed(struct rpi_state* this){
+       signed long int spd;
+       spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM];
+       this->speed=(int32_t)spd;
+}

diff --git a/pmsm-control/test_sw/comp.h b/pmsm-control/test_sw/comp.h
new file mode 100644 (file)
index 0000000..d71f066
--- /dev/null
+++ b/pmsm-control/test_sw/comp.h
@@ -0,0 +1,37 @@
+/**
+ * \brief
+ * Computatations of position, index, speed.
+ *
+ */
+#ifndef COMP
+#define COMP
+
+struct rpi_in;
+struct rpi_state;
+
+/**
+ * \brief
+ * Substact initial position.
+ */
+void substractOffset(struct rpi_in* data, struct rpi_in* offset);
+
+/**
+ * \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(struct rpi_state*);
+
+/*
+ * \brief
+ * Computates speed.
+ */
+void compSpeed(struct rpi_state*);
+
+#endif /*COMP*/
+

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index f4ce30b7c3c35f70a6d24cd0e947c8b2f420fbba..56b654417374c35d540dbc693c0f18d2bdb54443 100644 (file)
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -25,6 +25,7 @@
 #include "cmd_proc.h"
 #include "controllers.h"
 #include "commutators.h"
 #include "cmd_proc.h"
 #include "controllers.h"
 #include "commutators.h"

+#include "comp.h"

 
 
 
 
 
 


@@ -163,13 +164,6 @@ void appl_stop(){
         printf("\nprogram bezpecne ukoncen\n");
 }
 
         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
  */
 /**
  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
  */


@@ -180,97 +174,6 @@ void * pos_monitor(void* param){
        }
        return (void*)0;
 }
        }
        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<index){                 /*pozice e<0,index-1> */
-                       /*proti smeru h.r. od indexu*/
-                       dist=pos+2000-index;
-               }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
-                       /*po smeru h.r. od indexu*/
-                       dist=pos-index;
-               }else if (pos<index+2096){      /*pozice e<index+2000,index+2095> */
-                       goto index_lost;
-               }else{                          /*pozice e<index+2096,4095> */
-                       /*proti smeru h.r. od indexu - podtecena pozice*/
-                       dist=pos-index-2096;
-               }
-       }else if (index<=2096){         /*index e<1999,2096>*/
-               if (pos<index-1999){            /*pozice e<0,index-2000> */
-                       goto index_lost;
-               }else if (pos<index){           /*pozice e<index-1999,index-1> */
-                       /*proti smeru h.r. od indexu*/
-                       dist=pos+2000-index;
-               }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
-                       /*po smeru h.r. od indexu*/
-                       dist=pos-index;
-               }else {                         /*pozice e<index+2000,4095> */
-                       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<index-1999){      /*pozice e<index-2096,index-2000> */
-                       goto index_lost;
-               }else if (pos<index){           /*pozice e<index-1999,index-1> */
-                       /*proti smeru h.r. od indexu*/
-                       dist=pos+2000-index;
-               }else{                          /*pozice e<index,4095> */
-                       /*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
 
 /*
  * \brief


@@ -304,7 +207,7 @@ void * read_data(void* param){
                        /*subtract initiate postion */
                        rps.tf_count++;
                        substractOffset(&data,poc.spi_dat);
                        /*subtract initiate postion */
                        rps.tf_count++;
                        substractOffset(&data,poc.spi_dat);

-                       compSpeed();                    /*spocita rychlost*/
+                       compSpeed(&rps);                        /*spocita rychlost*/

 
                        if (!rps.index_ok){
                                if (first){
 
                        if (!rps.index_ok){
                                if (first){


@@ -312,10 +215,10 @@ void * read_data(void* param){
                                        first=0;
                                }else if (last_index!=data.index_position){
                                        rps.index_ok=1;
                                        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*/
                                }
                        }else{ /*index je v poradku*/

-                               comIndDist();           /*vypocet vzdalenosti indexu*/
+                               comIndDist(&rps);               /*vypocet vzdalenosti indexu*/

                        }
 
                        /* pocitame sirku plneni podle potreb rizeni*/
                        }
 
                        /* pocitame sirku plneni podle potreb rizeni*/