TUMBL firmware to compute D and Q components of winding current.

Signed-off-by: Pavel Pisa <ppisa@pikron.com>

diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c
index 678c8e3ad46a707af391f238f96eab215cbcf238..8e9cd7172879da758e7e5509e9a4a2d42a7bdae7 100644 (file)
--- a/hw/lx-rocon_firmware/firmware.c
+++ b/hw/lx-rocon_firmware/firmware.c
@@ -18,11 +18,15 @@
 #define SUPPRESS_CONDITIONALS 1
 #undef COMPUTE_PHASE_SECTOR
 
+#define PXMCC_CURADC_CHANNELS 3
+
 #include <stdint.h>
 #include "tumbl_addr.h"
 
 /* k3 = math.sqrt(3) / 2 */
 
+#define CONST16_K3   56756
+
 /* 1 / k3 * 65536 */
 
 #define RECI16_K3    75674
@@ -41,14 +45,24 @@ typedef struct pxmcc_axis_data_t {
   int32_t   ptsin;
   int32_t   ptcos;
   uint32_t  ptphs;
+  uint32_t  pwm_cycle;
 
   uint32_t  act_idle;
   uint32_t  min_idle;
   uint32_t  rx_done_sqn;
 } pxmcc_axis_data_t;
 
+typedef struct pxmcc_curadc_data_t {
+  int16_t   cur_val;
+  uint16_t  reserved1;
+  uint16_t  siroladc_offs;
+  uint16_t  siroladc_last;
+} pxmcc_curadc_data_t;
+
 pxmcc_axis_data_t pxmcc_axis[1];
 
+pxmcc_curadc_data_t pxmcc_curadc[PXMCC_CURADC_CHANNELS];
+
 void init_defvals(void)
 {
 }
@@ -100,6 +114,10 @@ void main(void)
   pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
   pxmcc->min_idle = 0;
   pxmcc->pwm_dq = 0;
+  pxmcc->pwm_cycle = 2500;
+  pxmcc_curadc[0].siroladc_offs = 0x0c17;
+  pxmcc_curadc[1].siroladc_offs = 0x0c66;
+  pxmcc_curadc[2].siroladc_offs = 0x0c66;
 
   asm volatile("": : : "memory");
 
@@ -283,5 +301,161 @@ void main(void)
       pxmcc->rx_done_sqn = last_rx_done_sqn;
       asm volatile("": : : "memory");
     }
+    {
+      int i;
+      pxmcc_curadc_data_t *curadc = pxmcc_curadc;
+      volatile uint32_t *siroladc = FPGA_LX_MASTER_RX_ADC0;
+      uint16_t val;
+
+      for (i = 0; i < PXMCC_CURADC_CHANNELS; ) {
+        val = *siroladc;
+
+        curadc->cur_val = val - curadc->siroladc_last
+                              - curadc->siroladc_offs;
+        curadc->siroladc_last = val;
+        curadc->reserved1=0x11aa;
+
+        i++;
+        curadc += 1;
+        siroladc += 2;
+        /* additional 3 required for 7 -> 8 change */
+        if (!(i & 7))
+          siroladc += 3;
+      }
+    }
+    {
+      int32_t  cur_alp;
+      int32_t  cur_bet;
+      int32_t  cur_d;
+      int32_t  cur_q;
+      uint32_t pwm[3];
+      uint32_t pwm1;
+      uint32_t pwm2;
+      uint32_t pwm3;
+      int32_t  cur[3];
+      int32_t  cur1;
+      int32_t  cur2;
+      int32_t  cur3;
+      uint32_t curmult_idx;
+      uint32_t pwm_reci;
+     #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
+      uint32_t phs;
+     #ifdef COMPUTE_PHASE_SECTOR
+      phs = pxmcc->ptphs;
+     #endif /*COMPUTE_PHASE_SECTOR*/
+     #endif /*COMPUTE_PHASE_SECTOR*/
+
+      pwm[1] = *FPGA_LX_MASTER_TX_PWM0 & 0x3fff;
+      pwm[2] = *FPGA_LX_MASTER_TX_PWM1 & 0x3fff;
+      pwm[0] = *FPGA_LX_MASTER_TX_PWM2 & 0x3fff;
+
+      cur[1] = pxmcc_curadc[0].cur_val;
+      cur[2] = pxmcc_curadc[1].cur_val;
+      cur[0] = pxmcc_curadc[2].cur_val;
+
+      pwm1 = pwm[0];
+      pwm2 = pwm[1];
+      pwm3 = pwm[2];
+
+     #ifndef SUPPRESS_CONDITIONALS
+      #ifndef COMPUTE_PHASE_SECTOR
+      if (pwm1 > pwm2)
+        if (pwm2 > pwm3)
+          phs = 0;
+        else if (pwm1 > pwm3)
+          phs = 5;
+        else
+          phs = 4;
+      else
+        if (pwm2 < pwm3)
+          phs = 3;
+        else if (pwm1 < pwm3)
+          phs = 2;
+        else
+          phs = 1;
+      #endif /*COMPUTE_PHASE_SECTOR*/
+
+      curmult_idx = (0x00102102 >> (4 * phs)) & 3;
+      pwm_reci = pxmcc->pwm_cycle - pwm[curmult_idx];
+      pwm_reci = (pxmcc->pwm_cycle << 16) / pwm_reci;
+      cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
+
+      cur1 = cur[0];
+      cur2 = cur[1];
+      cur3 = cur[2];
+
+      if ((phs == 5) || (phs == 0))
+        cur_alp = -(cur2 + cur3);
+      else
+        cur_alp = cur1;
+
+      if ((phs == 5) || (phs == 0))
+        cur_bet = cur2 - cur3;
+      else if ((phs == 3) || (phs == 4))
+        cur_bet = 2 * cur2 + cur1;
+      else /* 1 2 */
+        cur_bet = -(2 * cur3 + cur1);
+     #else /*SUPPRESS_CONDITIONALS*/
+      {
+        /*
+         *   u1>u2 u2>u3 u1>u3               cm
+         *     0     1     1 ->  1 (1, 0, 2) 2
+         *     0     1     0 ->  2 (1, 2, 0) 0
+         *     0     0     0 ->  3 (2, 1, 0) 1
+         *     1     0     0 ->  4 (2, 0, 1) 2
+         *     1     0     1 ->  5 (0, 2, 1) 0
+         *     1     1     1 ->  0 (0, 1, 2) 1
+         */
+
+        uint32_t u1gtu2 = (int32_t)(pwm2 - pwm1) >> 31;
+        uint32_t u1gtu3 = (int32_t)(pwm3 - pwm1) >> 31;
+        uint32_t u2gtu3 = (int32_t)(pwm3 - pwm2) >> 31;
+        uint32_t state50_msk = u1gtu2 & u1gtu3;
+        uint32_t pwm_reci_bits;
+
+        curmult_idx = (((u1gtu3 ^ u1gtu2) | 1) ^ u2gtu3 ^ u1gtu2) & 3;
+        pwm_reci = pxmcc->pwm_cycle - pwm[curmult_idx];
+       #if 0
+        pwm_reci_bits = __builtin_clzl(pwm_reci);
+       #else
+        asm("clz %0,%1\n":"=r"(pwm_reci_bits):"r"(pwm_reci));
+       #endif
+        pwm_reci <<= pwm_reci_bits;
+        *FPGA_FNCAPPROX_RECI = pwm_reci;
+        dummy = *FPGA_FNCAPPROX_RECI;
+        dummy = *FPGA_FNCAPPROX_RECI;
+        dummy = *FPGA_FNCAPPROX_RECI;
+        pwm_reci = *FPGA_FNCAPPROX_RECI;
+        pwm_reci >>= 16;
+        pwm_reci *= pxmcc->pwm_cycle;
+        pwm_reci >>= 30 - pwm_reci_bits;
+        cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
+
+        cur1 = cur[0];
+        cur2 = cur[1];
+        cur3 = cur[2];
+
+        cur_alp = -(cur2 + cur3);                /* 5 0 */
+        cur_alp &= state50_msk;                  /* 1 2 3 4 */
+        cur_alp |= cur1 & ~state50_msk;
+
+        cur_bet = (-2 * cur3 - cur1) & u2gtu3;   /* 1 2 */
+        cur_bet |= (2 * cur2 + cur1) & ~u2gtu3;  /* 3 4 */
+        cur_bet &= ~state50_msk;
+        cur_bet |= (cur2 - cur3) & state50_msk;  /* 5 0 */
+      }
+     #endif /*SUPPRESS_CONDITIONALS*/
+
+      cur_alp *= 3;
+      cur_alp >>= 1;
+
+      cur_bet *= CONST16_K3;
+      cur_bet >>= 16;
+
+      cur_d =  cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;
+      cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
+
+      pxmcc->cur_dq = (cur_d & 0xffff0000) | ((cur_q >> 16) & 0xffff);
+    }
   }
 }

diff --git a/hw/lx-rocon_firmware/tumbl_addr.h b/hw/lx-rocon_firmware/tumbl_addr.h
index 53a9093e40175f52312c095d39b9d1fdf5c71ab9..649858eb574225e33ed86dab7c4b5d0708b5ae40 100644 (file)
--- a/hw/lx-rocon_firmware/tumbl_addr.h
+++ b/hw/lx-rocon_firmware/tumbl_addr.h
@@ -49,7 +49,7 @@
 #define FPGA_LX_MASTER_TX_PWM14 (FPGA_LX_MASTER_TX+24)
 #define FPGA_LX_MASTER_TX_PWM15 (FPGA_LX_MASTER_TX+25)
 
-#define FPGA_LX_MASTER_RX       IO32ADDR(FPGA_LX_MASTER_BASE+0x0400)
+#define FPGA_LX_MASTER_RX       IO32ADDR(FPGA_LX_MASTER_BASE+0x0800)
 
 /* rec_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) * 3 + chan * 2; */