RoCoN: Add command to select motor driver type/axis mode at runtime.

[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 bdc2f061b962598a250bffa9d664cd6b14ee8b4c..d9a8408c4a8f01260ede0b842516eccc2d112bd8 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -1351,19 +1351,57 @@ int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
   return res;
 }
 
+int pxmc_axis_rdmode(pxmc_state_t *mcs)
+{
+  if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
+    return PXMC_AXIS_MODE_STEPPER_WITH_IRC;
+  if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out)
+    return PXMC_AXIS_MODE_DC;
+  if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
+    return PXMC_AXIS_MODE_BLDC;
+  return -1;
+}
+
+
 int
 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
 {
-  int res;
+  static const typeof(*mcs->pxms_ptptr1) dummy0 = 0;
+  int res = 0;
 
-  mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
+  if (mode == PXMC_AXIS_MODE_NOCHANGE)
+    mode = pxmc_axis_rdmode(mcs);
+  if (mode < 0)
+    return -1;
 
-  /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
-#ifndef PXMC_WITH_PT_ZIC
-  res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
-#else /*PXMC_WITH_PT_ZIC*/
-  res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
-#endif /*PXMC_WITH_PT_ZIC*/
+  switch (mode) {
+    /*case PXMC_AXIS_MODE_STEPPER:*/
+    case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
+      res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
+      break;
+    /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
+    case PXMC_AXIS_MODE_DC:
+      /*profive some sane dummy values*/
+      mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0;
+      mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0;
+      mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0;
+
+      mcs->pxms_ptscale_mult=1;
+      mcs->pxms_ptscale_shift=15;
+      break;
+    case PXMC_AXIS_MODE_BLDC:
+      /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
+     #ifndef PXMC_WITH_PT_ZIC
+      res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
+     #else /*PXMC_WITH_PT_ZIC*/
+      res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
+     #endif /*PXMC_WITH_PT_ZIC*/
+      break;
+    default:
+      return -1;
+  }
+
+  mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
 
   return res;
 }
@@ -1387,14 +1425,32 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode)
   pxmc_clear_flag(mcs, PXMS_PHA_b);
   pxmc_clear_flag(mcs, PXMS_PTI_b);
 
+  if (mode == PXMC_AXIS_MODE_NOCHANGE)
+    mode = pxmc_axis_rdmode(mcs);
+  if (mode < 0)
+    return -1;
   if (!mode)
-  {
-    /*mode=pxmc_axis_rdmode(mcs);*/
-    if (!mode)
-      mode = PXMC_AXIS_MODE_BLDC;
-  }
+    mode = PXMC_AXIS_MODE_DC;
 
   res = pxmc_axis_pt4mode(mcs, mode);
+  if (res < 0)
+    return -1;
+
+  switch (mode) {
+    /*case PXMC_AXIS_MODE_STEPPER:*/
+    case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
+      mcs->pxms_do_out = pxmc_rocon_pwm2ph_out;
+      break;
+    /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
+    case PXMC_AXIS_MODE_DC:
+      mcs->pxms_do_out = pxmc_rocon_pwm_dc_out;
+      break;
+    case PXMC_AXIS_MODE_BLDC:
+      mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
+      break;
+    default:
+      return -1;
+  }
 
   pxmc_set_flag(mcs, PXMS_ENI_b);
   return res;