From: Pavel Pisa <ppisa@pikron.com>
Date: Tue, 6 Jan 2015 11:13:21 +0000 (+0100)
Subject: Extend PXMCC control and support routines to enable/disable ptofs automatic update.
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/lx-cpu1/lx-rocon.git/commitdiff_plain/3878912ee701a97c5d54e20e9bc4f0b9b859896c

Extend PXMCC control and support routines to enable/disable ptofs automatic update.

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 0d8bfec..8a21bbc 100644
--- a/hw/lx-rocon_firmware/firmware.c
+++ b/hw/lx-rocon_firmware/firmware.c
@@ -121,7 +121,13 @@ void main(void)
       irc = *(FPGA_IRC0 + (FPGA_IRC1 - FPGA_IRC0) * pxmcc->inp_info);
 
       pti = irc - ofs;
-      if ((uint32_t)pti >= per) {
+      /*
+       * the '>=' is appropriate to keep pti in <0;per-1> range,
+       * but sine and cosine computations can work over multiple
+       * periods and value of per=0xffffffff allows to control/stop
+       * updates if only '>' is used.
+       */
+      if ((uint32_t)pti > per) {
         if (pti < 0) {
           ofs -= per;
         } else {
diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index f49f5e1..6954b0b 100644
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -716,7 +716,7 @@ pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 /*******************************************************************/
 /* PXMCC - PXMC coprocessor support and communication */
 
-void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs)
+void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs, int enable_update)
 {
   volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
   int inp_chan=mcs->pxms_inp_info;
@@ -724,10 +724,15 @@ void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs)
   uint32_t irc;
 
   if (mcc_axis != NULL) {
+    if (enable_update >= 0)
+      mcc_axis->ptirc = 0xffffffff;
     ptofs = mcs->pxms_ptofs - pxmc_rocon_irc_offset[inp_chan];
     irc = fpga_irc[inp_chan]->count;
     ptofs = (int16_t)(ptofs - irc) + irc;
     mcc_axis->ptofs = ptofs;
+    if (enable_update > 0) {
+      mcc_axis->ptirc = mcs->pxms_ptirc;
+    }
   }
 }
 
@@ -754,6 +759,7 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
 
       if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m)) {
         int set_ptofs_fl = 0;
+        int ptofs_enable_update = 0;
 
         if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1) {
           short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
@@ -769,6 +775,7 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
           }
 
           set_ptofs_fl = 1;
+          ptofs_enable_update = 1;
 
           pxmc_set_flag(mcs, PXMS_PTI_b);
           pxmc_clear_flag(mcs, PXMS_PRA_b);
@@ -780,7 +787,7 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
           mcs->pxms_ptindx = ptindx;
           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 
-          pxmcc_pxmc_ptofs2mcc(mcs);
+          pxmcc_pxmc_ptofs2mcc(mcs, ptofs_enable_update);
         }
       } else {
         /* if phase table position to mask is know do fine phase table alignment */
@@ -791,7 +798,7 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
           if (res < 0) {
             pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
           } else if (res) {
-            pxmcc_pxmc_ptofs2mcc(mcs);
+            pxmcc_pxmc_ptofs2mcc(mcs, 1);
             pxmc_set_flag(mcs, PXMS_PTI_b);
             pxmc_set_flag(mcs, PXMS_PHA_b);
           }
@@ -826,6 +833,25 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
 int
 pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
 {
+  if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
+     (mcs->pxms_flg&PXMS_PRA_m)) {
+
+    {
+      /* Wait for index mark to align phases */
+      int res;
+      res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0);
+      if (res < 0) {
+        pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
+      } else if (res) {
+        pxmcc_pxmc_ptofs2mcc(mcs, 1);
+        pxmc_set_flag(mcs, PXMS_PTI_b);
+        pxmc_set_flag(mcs, PXMS_PHA_b);
+      } else {
+        pxmcc_pxmc_ptofs2mcc(mcs, 0);
+      }
+    }
+  }
+
   {
     int ene, pwm_d, pwm_q;
 
@@ -858,6 +884,9 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
   if (mcc_axis == NULL)
     return -1;
 
+  if (mcc_data->common.fwversion != PXMCC_FWVERSION)
+    return -1;
+
   mcc_axis->ccflg = 0;
   mcc_axis->mode = PXMCC_MODE_IDLE;
 
@@ -867,10 +896,9 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
   ull = (1ULL << 32) * mcs->pxms_ptper;
   ptreci = (ull + ptirc / 2) / ptirc;
 
-  mcc_axis->ptirc = ptirc;
   mcc_axis->ptreci = ptreci;
 
-  pxmcc_pxmc_ptofs2mcc(mcs);
+  pxmcc_pxmc_ptofs2mcc(mcs, 0);
 
   switch (mode) {
     case PXMCC_MODE_IDLE:
@@ -913,6 +941,8 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
   mcc_axis->ccflg = 0;
   mcc_axis->pwm_dq = 0;
 
+  pxmcc_pxmc_ptofs2mcc(mcs, 1);
+
   return 0;
 }
 
@@ -1808,6 +1838,8 @@ int pxmc_axis_rdmode(pxmc_state_t *mcs)
     return PXMC_AXIS_MODE_BLDC;
   if (mcs->pxms_do_out == pxmc_pxmcc_pwm3ph_out)
     return PXMC_AXIS_MODE_BLDC_PXMCC;
+  if (mcs->pxms_do_out == pxmc_pxmcc_pwm2ph_out)
+    return PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC;
   return -1;
 }
 
diff --git a/sw/app/rocon/pxmcc_interface.h b/sw/app/rocon/pxmcc_interface.h
index e55ade4..2ffeccf 100644
--- a/sw/app/rocon/pxmcc_interface.h
+++ b/sw/app/rocon/pxmcc_interface.h
@@ -52,7 +52,7 @@ void pxmcc_axis_enable(pxmc_state_t *mcs, int enable)
   mcc_axis->ccflg = enable? 1: 0;
 }
 
-void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs);
+void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs, int enable_update);
 
 int pxmcc_axis_setup(pxmc_state_t *mcs, int mode);