X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/b4d468f4be4d8e91bbd775b5b37f0ae431ae6648..48407cad35d152771ac1a86652dae3eede6741a2:/pmsm-control/test_sw/comp.c

diff --git a/pmsm-control/test_sw/comp.c b/pmsm-control/test_sw/comp.c
new file mode 100644
index 0000000..49d3546
--- /dev/null
+++ b/pmsm-control/test_sw/comp.c
@@ -0,0 +1,108 @@
+/**
+ * \brief
+ * Computatations of position, index, speed.
+ *
+ */
+
+#include "comp.h"
+#include "pmsm_state.h"
+#include "rp_spi.h"
+
+/**
+ * \brief
+ * Substact initial position.
+ */
+void substractOffset(struct rpi_in* data, struct rpi_in* offset){
+	data->pozice=data->pozice_raw-offset->pozice_raw;
+	return;
+}
+
+/*
+ * \brief
+ * Multiplication of 11 bit
+ * Zaporne vysledky prvede na nulu.
+ */
+uint16_t mult_cap(int32_t s,int d){
+	int j;
+	int res=0;
+	for(j=0;j!=11;j++){
+		/* multiplicate as if maximum sinus value was unity */
+		res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
+	}
+	return res;
+}
+
+
+/**
+ * \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
+ */
+void comIndDist(struct rpi_state* this){
+	uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw;
+	uint16_t dist;
+	uint16_t index = this->spi_dat->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;
+		}
+	}
+
+	this->index_dist = dist;
+	return;
+
+	index_lost:
+		this->index_ok=0;
+		return;
+}
+
+
+/*
+ * \brief
+ * Computate speed.
+ */
+void compSpeed(struct rpi_state* this){
+	signed long int spd;
+	spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM];
+	this->speed=(int32_t)spd;
+}