RoCoN: new command REGOUTMAP to setup power outputs connection to axes.

Signed-off-by: Pavel Pisa <ppisa@pikron.com>

diff --git a/sw/app/rocon/appl_cmds.c b/sw/app/rocon/appl_cmds.c
index f85182eb5b6958d833c508dbe282cf531254252f..c944c11802e570e19644ca7f8830a5d479ae4756 100644 (file)
--- a/sw/app/rocon/appl_cmds.c
+++ b/sw/app/rocon/appl_cmds.c
@@ -124,6 +124,103 @@ int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
   return 0;
 }
 
+int cmd_do_axes_outmap(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+  long val;
+  char *ps;
+  ps = param[3];
+  int mcs_idx;
+  int out_chan = 0;
+  pxmc_state_t *mcs;
+  int exes_cnt = pxmc_main_list.pxml_cnt > 0? pxmc_main_list.pxml_cnt : 0;
+  pxmc_info_t outmap[exes_cnt];
+
+  if (exes_cnt == 0)
+    return 0;
+
+  if(*param[2]=='?') {
+    char str[20];
+    cmd_io_write(cmd_io,param[0],param[2]-param[0]);
+
+    pxmc_for_each_mcs(mcs_idx, mcs)
+    {
+      cmd_io_putc(cmd_io, mcs_idx? ',' : '=');
+      if (mcs != NULL) {
+        val = mcs->pxms_out_info;
+        i2str(str, val, 0, 0);
+        cmd_io_write(cmd_io,str,strlen(str));
+      } else {
+        cmd_io_putc(cmd_io, 'X');
+      }
+    }
+
+    return 0;
+  }
+
+  if(*param[2]!=':') return -CMDERR_OPCHAR;
+
+  mcs_idx = -1;
+  si_skspace(&ps);
+  do {
+    if (!*ps)
+      break;
+    if (mcs_idx >= exes_cnt - 1)
+      return -CMDERR_BADREG;
+
+    if (si_long(&ps, &val, 0) < 0)
+      return -CMDERR_BADPAR;
+
+    if (val < 0)
+      return -CMDERR_BADPAR;
+
+    outmap[++mcs_idx] = val;
+
+    si_skspace(&ps);
+
+    if (*ps)
+      if(*(ps++) != ',')
+        return -CMDERR_BADSEP;
+  } while(1);
+
+  while (mcs_idx < exes_cnt - 1) {
+    if (mcs_idx >= 0) {
+      mcs = pxmc_main_list.pxml_arr[mcs_idx];
+      if (mcs != NULL) {
+        val = pxmc_axis_rdmode(mcs);
+        val = pxmc_axis_out_chans4mode(val);
+        if (val > 0)
+          out_chan = outmap[mcs_idx] + val;
+      }
+    }
+    outmap[++mcs_idx] = out_chan;
+  }
+
+  pxmc_for_each_mcs(mcs_idx, mcs)
+  {
+    if (mcs == NULL)
+      continue;
+    if(mcs->pxms_flg & PXMS_BSY_m)
+      return -CMDERR_BSYREG;
+    pxmc_set_const_out(mcs,0);
+    val = pxmc_axis_mode(mcs, PXMC_AXIS_MODE_NOCHANGE);
+    if (val < 0)
+      return -CMDERR_EIO;
+  }
+
+  pxmc_for_each_mcs(mcs_idx, mcs)
+  {
+    if (mcs == NULL)
+      continue;
+    mcs->pxms_out_info = outmap[mcs_idx];
+    val = pxmc_axis_mode(mcs, PXMC_AXIS_MODE_NOCHANGE);
+    if (val < 0)
+      return -CMDERR_EIO;
+  }
+
+  return 0;
+}
+
+
 int cmd_do_ioecho(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 {
   int val;
@@ -196,6 +293,9 @@ cmd_des_t const cmd_des_pthalign={0, CDESM_OPCHR|CDESM_RW,"PTHALIGN?","run align
 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
                        "REGMODE?","axis working mode",cmd_do_axis_mode,
                        {}};
+cmd_des_t const cmd_des_axes_outmap={0, CDESM_OPCHR|CDESM_WR,
+                       "REGOUTMAP","map outputs to axes",cmd_do_axes_outmap,
+                       {}};
 cmd_des_t const cmd_des_errstop={0, CDESM_OPCHR|CDESM_RW,
                        "ERRSTOP","stop all axes at error",cmd_do_rw_int,
                        {(char*)&appl_errstop_mode,
@@ -225,6 +325,7 @@ cmd_des_t const *const cmd_appl_specific[]={
   &cmd_des_stamp,
   &cmd_des_pthalign,
   &cmd_des_axis_mode,
+  &cmd_des_axes_outmap,
   &cmd_des_errstop,
   &cmd_des_idlerel,
   &cmd_des_echo,

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index ec520dce6b422cba53830e2da86ffd00db342278..a7abedc1e86080da736250433f776388674a59d3 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -2154,6 +2154,22 @@ int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
   return res;
 }
 
+int pxmc_axis_out_chans4mode(int mode)
+{
+  switch (mode) {
+    case PXMC_AXIS_MODE_DC:
+      return 2;
+    case PXMC_AXIS_MODE_BLDC:
+    case PXMC_AXIS_MODE_BLDC_PXMCC:
+      return 3;
+    case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
+    case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
+    case PXMC_AXIS_MODE_STEPPER_PXMCC:
+      return 4;
+  }
+  return -1;
+}
+
 int pxmc_axis_rdmode(pxmc_state_t *mcs)
 {
   if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
@@ -2171,7 +2187,6 @@ int pxmc_axis_rdmode(pxmc_state_t *mcs)
   return -1;
 }
 
-
 int
 pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
 {