From: Pavel Pisa <pisa@cmp.felk.cvut.cz>
Date: Tue, 12 May 2015 09:34:20 +0000 (+0200)
Subject: RPi PXMC Test: add evaluation and extension of index position to 32 bits.
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control-pxmc.git/commitdiff_plain/dcc91599cf41c794a11be892bfd86db1a2897a02

RPi PXMC Test: add evaluation and extension of index position to 32 bits.

Signed-off-by: Pavel Pisa <pisa@cmp.felk.cvut.cz>
---

diff --git a/src/app/rpi-pmsm-test1/pxmc_spimc.h b/src/app/rpi-pmsm-test1/pxmc_spimc.h
index 1bc4e40..b26f71f 100644
--- a/src/app/rpi-pmsm-test1/pxmc_spimc.h
+++ b/src/app/rpi-pmsm-test1/pxmc_spimc.h
@@ -16,6 +16,7 @@ typedef struct spimc_state_t {
   uint32_t pwm[SPIMC_CHAN_COUNT];
   uint32_t act_pos;
   uint32_t index_pos;
+  uint32_t index_occur;
   uint32_t pos_offset;
   int32_t  curadc_val[SPIMC_CHAN_COUNT];
   int32_t  curadc_offs[SPIMC_CHAN_COUNT];
diff --git a/src/app/rpi-pmsm-test1/rpi_spi.c b/src/app/rpi-pmsm-test1/rpi_spi.c
index 084c361..5e93cc1 100644
--- a/src/app/rpi-pmsm-test1/rpi_spi.c
+++ b/src/app/rpi-pmsm-test1/rpi_spi.c
@@ -21,6 +21,9 @@
 
 #include "pxmc_spimc.h"
 
+#define SPIMC_INDEX_BITS 12
+#define SPIMC_INDEX_MASK ((1 << SPIMC_INDEX_BITS) - 1)
+
 static uint8_t spimc_mode = 0;
 static uint8_t spimc_bits = 8;
 static uint32_t spimc_speed = 500000;
@@ -38,9 +41,11 @@ int spimc_transfer(spimc_state_t *spimcst)
 	uint8_t *rx = spimcst->rx_buf;
 	int ret;
 	uint32_t pwm1, pwm2, pwm3;
+	uint32_t idx;
+	int32_t  idxdiff;
 
-        memset(tx, 0, SPIMC_TRANSFER_SIZE);
-        memset(rx, 0, SPIMC_TRANSFER_SIZE);
+	memset(tx, 0, SPIMC_TRANSFER_SIZE);
+	memset(rx, 0, SPIMC_TRANSFER_SIZE);
 
 	/*Data format:
 	 * rx[0] - bity 127 downto 120 the first income bit..127
@@ -164,10 +169,19 @@ int spimc_transfer(spimc_state_t *spimcst)
 	 * 	92..88 in rx[4] last 5 bits (from left)
 	 * 	87..81 in rx[5] first 7 bits (from left)
 	 */
-	spimcst->index_pos = 0x1F & rx[4];
-	spimcst->index_pos <<= 8;
-	spimcst->index_pos |= 0xFE & rx[5];
-	spimcst->index_pos >>= 1;
+	idx = 0x1F & rx[4];
+	idx <<= 8;
+	idx |= 0xFE & rx[5];
+	idx >>= 1;
+
+	if ((idx ^ spimcst->index_pos) & SPIMC_INDEX_MASK) {
+		idxdiff = (idx - spimcst->act_pos +
+		           (1 << (SPIMC_INDEX_BITS - 1))) & SPIMC_INDEX_MASK;
+		idxdiff -= 1 << (SPIMC_INDEX_BITS - 1);
+		idx = spimcst->act_pos + idxdiff;
+		spimcst->index_pos = idx;
+		spimcst->index_occur += 1;
+	}
 
 	/* current measurments count
 	 * bits 80 downto 72