Variant of 3-phase DQ firmware which replaces branches by arithmetic.

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 171b2a2750d3e087fd6475f6bc21323e273cf0b9..f6ffdb14879763318eba914c1abbd75e1e1e717d 100644 (file)
--- a/hw/lx-rocon_firmware/firmware.c
+++ b/hw/lx-rocon_firmware/firmware.c
@@ -15,6 +15,8 @@
 
  *******************************************************************/
 
+#define SUPPRESS_CONDITIONALS 1
+#undef COMPUTE_PHASE_SECTOR
 
 #include <stdint.h>
 #include "tumbl_addr.h"
@@ -32,6 +34,8 @@ typedef struct pxmcc_axis_data_t {
   uint32_t  ccflg;
   int32_t   pwm_d;
   int32_t   pwm_q;
+  int32_t   cur_d;
+  int32_t   cur_q;
   uint32_t  ptindx;    /* index into phase table / irc in the cycle */
   uint32_t  ptirc;     /* IRC count per phase table */
   uint32_t  ptreci;    /* Reciprocal value of ptirc * 63356  */
@@ -39,9 +43,6 @@ typedef struct pxmcc_axis_data_t {
   int32_t   ptsin;
   int32_t   ptcos;
   uint32_t  ptphs;
-  int32_t   pwm_alp;
-  int32_t   pwm_bet;
-  int32_t   pwm_bet2;
 
   uint32_t  act_idle;
   uint32_t  min_idle;
@@ -96,7 +97,7 @@ void main(void)
 
   pxmcc->ccflg = 0;
   pxmcc->ptindx = 0;
-  pxmcc->ptofs = *FPGA_IRC0;
+  /*pxmcc->ptofs = *FPGA_IRC0;*/
   pxmcc->ptirc = 1000;
   pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
   pxmcc->min_idle = 0;
@@ -144,16 +145,19 @@ void main(void)
       int32_t pwm_alp;
       int32_t pwm_bet;
       int32_t pwm_bet_div_2_k3;
-      uint32_t phs;
       uint32_t pwm1;
       uint32_t pwm2;
       uint32_t pwm3;
+     #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
+      uint32_t phs;
+     #endif /*COMPUTE_PHASE_SECTOR*/
 
-      pwm_alp = pxmcc->pwm_d * pxmcc->ptcos + pxmcc->pwm_q * pxmcc->ptsin;
-      pwm_bet = pxmcc->pwm_d * pxmcc->ptsin - pxmcc->pwm_q * pxmcc->ptcos;
+      pwm_alp = pxmcc->pwm_d * pxmcc->ptcos - pxmcc->pwm_q * pxmcc->ptsin;
+      pwm_bet = pxmcc->pwm_d * pxmcc->ptsin + pxmcc->pwm_q * pxmcc->ptcos;
 
       pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
 
+     #ifndef SUPPRESS_CONDITIONALS
       if (pwm_bet > 0)
         if (pwm_alp > 0)
           /* pwm_bet > 2 * k3 * pwm_alp */
@@ -200,8 +204,56 @@ void main(void)
         /* pwm3 = -1/k3 * pwm_bet */
         pwm3 = -pwm_bet_div_2_k3 >> 15;
       }
+     #else /*SUPPRESS_CONDITIONALS*/
+      {
+        int32_t alp_m_bet_d2k3;
+        uint32_t state23_msk;
+        uint32_t pwm23_shift;
+        uint32_t bet_sgn = pwm_bet >> 31;
+        uint32_t alp_sgn = pwm_alp >> 31;
+        alp_m_bet_d2k3 = (alp_sgn ^ pwm_alp) - (bet_sgn ^ pwm_bet_div_2_k3);
+        alp_m_bet_d2k3 = alp_m_bet_d2k3 >> 31;
+       #ifdef COMPUTE_PHASE_SECTOR
+        uint32_t bet_sgn_cpl = ~bet_sgn;
+       #endif /*COMPUTE_PHASE_SECTOR*/
+
+        state23_msk = alp_sgn & ~alp_m_bet_d2k3;
+
+        /*
+         *   bet alp amb s23
+         *    0   0   0   0 -> 0 (000)
+         *    0   0  -1   0 -> 1 (001)
+         *   -1   0  -1   0 -> 1 (001)
+         *   -1   0   0  -1 -> 2 (010)
+         *   -1  -1   0  -1 -> 3 (011)
+         *   -1  -1  -1   0 -> 4 (100)
+         *    0  -1  -1   0 -> 4 (100)
+         *    0  -1   0   0 -> 5 (101)
+         */
+
+       #ifdef COMPUTE_PHASE_SECTOR
+        phs = (bet_sgn & 5) + (bet_sgn_cpl ^ ((alp_m_bet_d2k3 + 2 * state23_msk) + bet_sgn_cpl));
+       #endif /*COMPUTE_PHASE_SECTOR*/
+
+        pwm1 = pwm_alp & state23_msk;
+        pwm2 = pwm_bet_div_2_k3 - pwm1;
+        pwm3 = -pwm_bet_div_2_k3 - pwm1;
+        pwm2 &= (~bet_sgn | state23_msk);
+        pwm3 &= (bet_sgn | state23_msk);
+        pwm1 = pwm_alp + pwm2 + pwm3;
+        pwm1 &= ~state23_msk;
+        pwm1 >>= 16;
+        pwm23_shift = 15 - state23_msk;
+        pwm2 >>= pwm23_shift;
+        pwm3 >>= pwm23_shift;
+      }
+     #endif /*SUPPRESS_CONDITIONALS*/
 
+     #ifdef COMPUTE_PHASE_SECTOR
       pxmcc->ptphs = phs;
+     #endif /*COMPUTE_PHASE_SECTOR*/
+
+      asm volatile("": : : "memory");
 
       *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
       *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;