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