Added detection os index signal lost.
authorMartin Prudek <prudemar@fel.cvut.cz>
Sun, 10 May 2015 10:55:21 +0000 (12:55 +0200)
committerMartin Prudek <prudemar@fel.cvut.cz>
Sun, 10 May 2015 10:55:21 +0000 (12:55 +0200)
pmsm-control/test_sw/main_pmsm.c

index 12f236d..6e539b2 100644 (file)
@@ -413,32 +413,52 @@ void comIndDist(){
        uint16_t dist;
        uint16_t index = data.index_position;
 
-       if (index<2048){
-               if (pos<index){
+       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+2048){
+               }else if (pos<=index+1999){     /*pozice e<index,index+1999> */
                        /*po smeru h.r. od indexu*/
                        dist=pos-index;
-               }else{
+               }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 (pos<index-2048){
+       }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){
+               }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{
+               }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