Changed computation of distance to index - there's no detection of index position...

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index cbc687979bba0521b9580a81c1bb5d322e382eb7..f666358a2d4711509d5b5184346f6f167d806186 100644 (file)
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -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);
@@ -399,15 +399,41 @@ 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_12 = 0x0FFF & data.pozice_raw;
+       if (data.index_position<2048){
+               if (pos_12<data.index_position){
+                       /*proti smeru h.r. od indexu*/
+                       rps.index_dist=pos_12+2000-data.index_position;
+               }else if (pos_12<=data.index_position+2048){
+                       /*po smeru h.r. od indexu*/
+                       rps.index_dist=pos_12-data.index_position;
+               }else{
+                       /*proti smeru h.r. od indexu - podtecena pozice*/
+                       rps.index_dist=pos_12-data.index_position-2096;
+               }
+       }else{
+               if (pos_12<data.index_position-2048){
+                       /*po smeru h.r. od indexu - pretecena pozice*/
+                       rps.index_dist=4096+pos_12-data.index_position;
+               }else if (pos_12<data.index_position){
+                       /*proti smeru h.r. od indexu*/
+                       rps.index_dist=pos_12+2000-data.index_position;
+               }else{
+                       /*po smeru h.r. od indexu*/
+                       rps.index_dist=pos_12-data.index_position;
+               }
+
+       }
 }
 /*
  * \brief