RoCoN and TUMBL firmware for control of stepper motor with IRC sensor.

[fpga/lx-cpu1/lx-rocon.git] / sw / app / rocon / appl_pxmc.c

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index 1f9287f3109db6c9ecd2eb1e26b456a9991acde2..f49f5e1c4049449cc921074c9d4bac2bd699b06c 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -35,7 +35,8 @@
 #include "pxmcc_types.h"
 #include "pxmcc_interface.h"
 
-#define PXMC_AXIS_MODE_BLDC_PXMCC (PXMC_AXIS_MODE_BLDC + 1)
+#define PXMC_AXIS_MODE_BLDC_PXMCC             (PXMC_AXIS_MODE_BLDC + 1)
+#define PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC (PXMC_AXIS_MODE_BLDC + 2)
 
 int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
                            unsigned long index_irc, int diff2err);
@@ -715,6 +716,21 @@ pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
 /*******************************************************************/
 /* PXMCC - PXMC coprocessor support and communication */
 
+void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs)
+{
+  volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+  int inp_chan=mcs->pxms_inp_info;
+  uint32_t ptofs;
+  uint32_t irc;
+
+  if (mcc_axis != NULL) {
+    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;
+  }
+}
+
 int
 pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
 {
@@ -761,16 +777,10 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
             set_ptofs_fl = 1;
         }
         if (set_ptofs_fl) {
-          volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
-          uint32_t ptofs;
-          uint32_t irc;
-
           mcs->pxms_ptindx = ptindx;
           mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
 
-          irc = fpga_irc[mcs->pxms_inp_info]->count;
-          ptofs = (int16_t)(mcs->pxms_ptofs - irc) + irc;
-          mcc_axis->ptofs = ptofs;
+          pxmcc_pxmc_ptofs2mcc(mcs);
         }
       } else {
         /* if phase table position to mask is know do fine phase table alignment */
@@ -781,6 +791,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);
             pxmc_set_flag(mcs, PXMS_PTI_b);
             pxmc_set_flag(mcs, PXMS_PHA_b);
           }
@@ -812,13 +823,29 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
   return 0;
 }
 
+int
+pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
+{
+  {
+    int ene, pwm_d, pwm_q;
+
+    ene = mcs->pxms_ene;
+    pwm_d = 0;
+    pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
+
+    pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
+
+    if (mcs->pxms_flg & PXMS_ERR_m)
+      pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
+  }
+
+  return 0;
+}
 
 int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
 {
   volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
   volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
-  uint32_t ptofs;
-  uint32_t irc;
   uint32_t ptirc;
   uint32_t ptreci;
   uint32_t pwmtx_info;
@@ -836,22 +863,20 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode)
 
   mcc_data->common.pwm_cycle = PXMC_LXPWR_PWM_CYCLE;
 
-  irc = fpga_irc[mcs->pxms_inp_info]->count;
-  ptofs = (int16_t)(mcs->pxms_ptofs - irc) + irc;
-
   ptirc = mcs->pxms_ptirc;
   ull = (1ULL << 32) * mcs->pxms_ptper;
   ptreci = (ull + ptirc / 2) / ptirc;
 
   mcc_axis->ptirc = ptirc;
   mcc_axis->ptreci = ptreci;
-  mcc_axis->ptofs = ptofs;
+
+  pxmcc_pxmc_ptofs2mcc(mcs);
 
   switch (mode) {
     case PXMCC_MODE_IDLE:
       phcnt = 0;
       break;
-    case PXMCC_MODE_MODE_BLDC:
+    case PXMCC_MODE_BLDC:
       phcnt = 3;
       break;
     case PXMCC_MODE_STEPPER_WITH_IRC:
@@ -1801,6 +1826,7 @@ pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
   switch (mode) {
     /*case PXMC_AXIS_MODE_STEPPER:*/
     case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
+    case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
       res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
       break;
     /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
@@ -1844,6 +1870,7 @@ int
 pxmc_axis_mode(pxmc_state_t *mcs, int mode)
 {
   int res;
+  int prev_mode;
 
   pxmc_set_const_out(mcs, 0);
   pxmc_clear_flag(mcs, PXMS_ENI_b);
@@ -1854,13 +1881,19 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
   pxmc_clear_flag(mcs, PXMS_PTI_b);
 
 
+  prev_mode = pxmc_axis_rdmode(mcs);
+
   if (mode == PXMC_AXIS_MODE_NOCHANGE)
-    mode = pxmc_axis_rdmode(mcs);
+    mode = prev_mode;
   if (mode < 0)
     return -1;
   if (!mode)
     mode = PXMC_AXIS_MODE_DC;
 
+  if ((prev_mode == PXMC_AXIS_MODE_BLDC_PXMCC) ||
+      (prev_mode == PXMCC_MODE_STEPPER_WITH_IRC))
+    pxmcc_axis_setup(mcs, PXMCC_MODE_IDLE);
+
   res = pxmc_axis_pt4mode(mcs, mode);
   if (res < 0)
     return -1;
@@ -1878,11 +1911,17 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
       mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
       break;
     case PXMC_AXIS_MODE_BLDC_PXMCC:
-      if (pxmcc_axis_setup(mcs, PXMCC_MODE_MODE_BLDC) < 0)
+      if (pxmcc_axis_setup(mcs, PXMCC_MODE_BLDC) < 0)
         return -1;
       pxmcc_axis_enable(mcs, 1);
       mcs->pxms_do_out = pxmc_pxmcc_pwm3ph_out;
       break;
+    case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
+      if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER_WITH_IRC) < 0)
+        return -1;
+      pxmcc_axis_enable(mcs, 1);
+      mcs->pxms_do_out = pxmc_pxmcc_pwm2ph_out;
+      break;
     default:
       return -1;
   }