The first firmware version for DQ to 3 phase PWM computation.

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 d08cb562be4519b975d4b60c638617fb9859754d..171b2a2750d3e087fd6475f6bc21323e273cf0b9 100644 (file)
--- a/hw/lx-rocon_firmware/firmware.c
+++ b/hw/lx-rocon_firmware/firmware.c
@@ -1,25 +1,63 @@
-/* Firmware file for lx-rocon tumbl coprocessor */
+/*******************************************************************
+  Components for embedded applications builded for
+  laboratory and medical instruments firmware
+
+  firmware.c - multi axis motion controller comprocesor
+               firmware for FPGA tumble CPU of lx-rocon system
+
+  (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
+  (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com
+
+  This file can be used and copied according to next
+  license alternatives
+   - GPL - GNU Public License
+   - other license provided by project originators
+
+ *******************************************************************/
+
 
 #include <stdint.h>
 #include "tumbl_addr.h"
 
+/* k3 = math.sqrt(3) / 2 */
+
+/* 1 / k3 * 65536 */
+
+#define RECI16_K3    75674
+
+/* 1 / (2 * k3) * 65536 */
+#define RECI16_2_K3  37837
+
 typedef struct pxmcc_axis_data_t {
+  uint32_t  ccflg;
+  int32_t   pwm_d;
+  int32_t   pwm_q;
+  uint32_t  ptindx;    /* index into phase table / irc in the cycle */
   uint32_t  ptirc;     /* IRC count per phase table */
-  uint32_t  ptper;     /* number of periods per table */
-  uint32_t  ptreci;    /* number of periods per table */
+  uint32_t  ptreci;    /* Reciprocal value of ptirc * 63356  */
   uint32_t  ptofs;     /* offset between table and IRC counter */
   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;
+  uint32_t  rx_done_sqn;
 } pxmcc_axis_data_t;
 
-uint32_t sin_lat[7];
-
 pxmcc_axis_data_t pxmcc_axis[1];
 
 void init_defvals(void)
 {
 }
 
+#if 0
+
+uint32_t sin_lat[7];
+
 void find_sin_lat(void)
 {
   int i;
@@ -39,32 +77,42 @@ void find_sin_lat(void)
   a5 = *FPGA_FNCAPPROX_SIN;
   asm volatile("": : : "memory");
 
-  sin_lat[0] = 0x1234;
+  sin_lat[0] = a0;
   sin_lat[1] = a1;
   sin_lat[2] = a2;
   sin_lat[3] = a3;
   sin_lat[4] = a4;
   sin_lat[5] = a5;
   sin_lat[6] = 0x4321;
+  sin_lat[0] = 0x1234;
 }
 
+#endif
+
 void main(void)
 {
+  uint32_t last_rx_done_sqn = 0;
   pxmcc_axis_data_t *pxmcc = pxmcc_axis;
 
-  pxmcc->ptirc = *FPGA_IRC0;
-  pxmcc->ptofs = pxmcc->ptirc;
-  pxmcc->ptper = 2000;
-  pxmcc->ptreci = 2147484; /* (1LL<<32)/ptper */
+  pxmcc->ccflg = 0;
+  pxmcc->ptindx = 0;
+  pxmcc->ptofs = *FPGA_IRC0;
+  pxmcc->ptirc = 1000;
+  pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
+  pxmcc->min_idle = 0;
+  pxmcc->pwm_d = 0;
+  pxmcc->pwm_q = 0;
 
   asm volatile("": : : "memory");
 
+ #if 0
   find_sin_lat();
+ #endif
 
   while (1) {
     uint32_t irc = *FPGA_IRC0;
     uint32_t ofs = pxmcc->ptofs;
-    uint32_t per = pxmcc->ptper;
+    uint32_t per = pxmcc->ptirc;
     int32_t  pti;
     uint32_t pta;
     uint32_t dummy;
@@ -79,7 +127,7 @@ void main(void)
       pti = irc - ofs;
       pxmcc->ptofs = ofs;
     }
-    pxmcc->ptirc = pti;
+    pxmcc->ptindx = pti;
 
     pta = pti * pxmcc->ptreci;
 
@@ -92,6 +140,92 @@ void main(void)
     dummy = *FPGA_FNCAPPROX_COS;
     pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
 
+    if (pxmcc->ccflg) {
+      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;
+
+      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);
+
+      if (pwm_bet > 0)
+        if (pwm_alp > 0)
+          /* pwm_bet > 2 * k3 * pwm_alp */
+          if (pwm_bet_div_2_k3 > pwm_alp)
+            phs = 1;
+          else
+            phs = 0;
+        else
+          /* -pwm_bet > 2 * k3 * pwm_alp */
+          if (pwm_bet_div_2_k3 < -pwm_alp)
+            phs = 2;
+          else
+            phs = 1;
+      else
+        if (pwm_alp > 0)
+          /* pwm_bet > -2 * k3 * pwm_alp */
+          if (pwm_bet_div_2_k3 > -pwm_alp)
+            phs = 5;
+          else
+            phs = 4;
+        else
+          /* pwm_bet > 2 * k3 * u_alp */
+          if (pwm_bet_div_2_k3 > pwm_alp)
+            phs = 3;
+          else
+            phs = 4;
+
+      if (phs <= 1) {
+        /* pwm1 = pwm_alp + 1.0/(2.0*k3) * pwm_bet */
+        pwm1 = (pwm_alp + pwm_bet_div_2_k3) >> 16;
+        /* pwm2 = 1/k3 * pwm_bet */
+        pwm2 = pwm_bet_div_2_k3 >> 15;
+        pwm3 = 0;
+      } else if (phs <= 3) {
+        pwm1 = 0;
+        /* pwm2 = 1.0/(2.0*k3) * pwm_bet - pwm_alp */
+        pwm2 = (pwm_bet_div_2_k3 - pwm_alp) >> 16;
+        /* pwm3 = -1.0/(2.0*k3) * pwm_bet - pwm_alp */
+        pwm3 = (-pwm_bet_div_2_k3 - pwm_alp) >>16;
+      } else {
+        /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */
+        pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16;
+        pwm2 = 0;
+        /* pwm3 = -1/k3 * pwm_bet */
+        pwm3 = -pwm_bet_div_2_k3 >> 15;
+      }
+
+      pxmcc->ptphs = phs;
+
+      *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
+      *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
+      *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
+    }
+
     asm volatile("": : : "memory");
+
+    {
+      uint32_t idlecnt = 0;
+      uint32_t sqn;
+      do {
+        sqn = *FPGA_LX_MASTER_RX_DDIV;
+        idlecnt++;
+      } while (sqn == last_rx_done_sqn);
+      pxmcc->act_idle = idlecnt;
+      if (((idlecnt < pxmcc->min_idle) ||
+          (pxmcc->min_idle == 0)) &&
+          last_rx_done_sqn) {
+        pxmcc->min_idle = idlecnt;
+      }
+      last_rx_done_sqn = sqn;
+      pxmcc->rx_done_sqn = last_rx_done_sqn;
+      asm volatile("": : : "memory");
+    }
   }
 }