-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;
+ }
+
+ }