X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/b7f2d913caef06457b1b68889895ba01f141935f..067296cf27a01a3cb766e31c44d313707964d9c3:/pmsm-control/test_sw/main_pmsm.c

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index cbc6879..6e539b2 100644
--- 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,66 @@ 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 = 0x0FFF & data.pozice_raw;
+	uint16_t dist;
+	uint16_t index = data.index_position;
+
+	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+1999){	/*pozice e<index,index+1999> */
+			/*po smeru h.r. od indexu*/
+			dist=pos-index;
+		}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 (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-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{ 				/*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