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);
}
}
/**
- * 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 = 0x0FFF & data.pozice_raw;
+ uint16_t dist;
+ uint16_t index = data.index_position;
+
+ if (index<2048){
+ if (pos<index){
+ /*proti smeru h.r. od indexu*/
+ dist=pos+2000-index;
+ }else if (pos<=index+2048){
+ /*po smeru h.r. od indexu*/
+ dist=pos-index;
+ }else{
+ /*proti smeru h.r. od indexu - podtecena pozice*/
+ dist=pos-index-2096;
+ }
+ }else{
+ if (pos<index-2048){
+ /*po smeru h.r. od indexu - pretecena pozice*/
+ dist=4096+pos-index;
+ }else if (pos<index){
+ /*proti smeru h.r. od indexu*/
+ dist=pos+2000-index;
+ }else{
+ /*po smeru h.r. od indexu*/
+ dist=pos-index;
+ }
+
+ }
+ rps.index_dist = dist;
+ return;
}
/*
* \brief