sw/app/rocon/appl_cmds.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components
   3
   4   appl_cmds.c - application specific commands - mainly global
   5                 PXMC state manipulation for RoCoN
   6
   7   Copyright (C) 2001-2014 by Pavel Pisa - originator
   8                           pisa@cmp.felk.cvut.cz
   9             (C) 2001-2014 by PiKRON Ltd. - originator
  10                     http://www.pikron.com
  11
  12   This file can be used and copied according to next
  13   license alternatives
  14    - GPL - GNU Public License
  15    - other license provided by project originators
  16
  17  *******************************************************************/
  18
  19 #include <cpu_def.h>
  20 #include <system_def.h>
  21 #include <stdlib.h>
  22 #include <string.h>
  23 #include <pxmc.h>
  24 #include <pxmc_coordmv.h>
  25 #include <cmd_proc.h>
  26 #include <pxmc_cmds.h>
  27 #include <utils.h>
  28
  29 #include "appl_defs.h"
  30
  31 #ifndef __STRINGIFY
  32 #define __STRINGIFY(x)     #x              /* stringify without expanding x */
  33 #endif
  34 #ifndef STRINGIFY
  35 #define STRINGIFY(x)    __STRINGIFY(x)        /* expand x, then stringify */
  36 #endif
  37
  38 int sqrtll_main(int argc, char *argv[]);
  39 int cmd_do_stop_all(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]);
  40 int cmd_do_release_all(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]);
  41 int cmd_do_status_bsybits(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]);
  42 int cmd_do_axst_all(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]);
  43 int cmd_do_cer_all(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]);
  44 int cmd_do_clr_all(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[]);
  45
  46 unsigned short statchk_power_stop = 0;
  47 unsigned short statchk_power_off = 0;
  48 int appl_errstop_mode = 0;
  49 int appl_idlerel_time = 0;
  50
  51 int cmd_do_status_all(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  52 {
  53   int i;
  54   int val;
  55   int opchar;
  56   pxmc_state_t *mcs;
  57
  58   if((opchar=cmd_opchar_check(cmd_io,des,param))<0) return opchar;
  59   if(opchar=='?'){
  60     val=0;
  61     pxmc_for_each_mcs(i, mcs) {
  62       if(mcs)
  63         val|=mcs->pxms_flg;
  64     }
  65     if(pxmc_coordmv_checkst(&pxmc_coordmv_state)>=2)
  66       val|=PXMS_CQF_m;  /* coordinator coomand queue full */
  67     /*if(trap_pc_addr)
  68       val|=0x1000000;*/
  69     if(statchk_power_stop)
  70       val|=0x20000;
  71     if(statchk_power_off)
  72       val|=0x10000;
  73     return cmd_opchar_replong(cmd_io, param, val, 0, 0);
  74   }
  75   return 0;
  76 }
  77
  78 int cmd_do_pthalign(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  79 {
  80     pxmc_state_t *mcs;
  81     int res;
  82
  83     if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
  84
  85     if(*param[2]=='?') {
  86       return cmd_opchar_replong(cmd_io, param, (long)mcs->pxms_ptmark, 0, 0);
  87     }
  88
  89     if(*param[2]!=':') return -CMDERR_OPCHAR;
  90
  91     res = pxmc_rocon_pthalalign(mcs, 20);
  92
  93     if(res < 0)
  94        return -CMDERR_BADDIO;
  95
  96     return 0;
  97 }
  98
  99 /**
 100  * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
 101  *
 102  * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
 103  */
 104 int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 105 {
 106   int val;
 107   pxmc_state_t *mcs;
 108
 109   if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 110
 111   if(*param[2]=='?') {
 112     return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0);
 113   }
 114
 115   if(*param[2]!=':') return -CMDERR_OPCHAR;
 116
 117   if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
 118
 119   val=atol(param[3]);
 120   val=pxmc_axis_mode(mcs,val);
 121   if(val<0)
 122     return val;
 123
 124   return 0;
 125 }
 126
 127 int cmd_do_axes_outmap(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 128 {
 129   long val;
 130   char *ps;
 131   ps = param[3];
 132   int mcs_idx;
 133   int out_chan = 0;
 134   pxmc_state_t *mcs;
 135   int exes_cnt = pxmc_main_list.pxml_cnt > 0? pxmc_main_list.pxml_cnt : 0;
 136   pxmc_info_t outmap[exes_cnt];
 137
 138   if (exes_cnt == 0)
 139     return 0;
 140
 141   if(*param[2]=='?') {
 142     char str[20];
 143     cmd_io_write(cmd_io,param[0],param[2]-param[0]);
 144
 145     pxmc_for_each_mcs(mcs_idx, mcs)
 146     {
 147       cmd_io_putc(cmd_io, mcs_idx? ',' : '=');
 148       if (mcs != NULL) {
 149         val = mcs->pxms_out_info;
 150         i2str(str, val, 0, 0);
 151         cmd_io_write(cmd_io,str,strlen(str));
 152       } else {
 153         cmd_io_putc(cmd_io, 'X');
 154       }
 155     }
 156
 157     return 0;
 158   }
 159
 160   if(*param[2]!=':') return -CMDERR_OPCHAR;
 161
 162   mcs_idx = -1;
 163   si_skspace(&ps);
 164   do {
 165     if (!*ps)
 166       break;
 167     if (mcs_idx >= exes_cnt - 1)
 168       return -CMDERR_BADREG;
 169
 170     if (si_long(&ps, &val, 0) < 0)
 171       return -CMDERR_BADPAR;
 172
 173     if (val < 0)
 174       return -CMDERR_BADPAR;
 175
 176     outmap[++mcs_idx] = val;
 177
 178     si_skspace(&ps);
 179
 180     if (*ps)
 181       if(*(ps++) != ',')
 182         return -CMDERR_BADSEP;
 183   } while(1);
 184
 185   while (mcs_idx < exes_cnt - 1) {
 186     if (mcs_idx >= 0) {
 187       mcs = pxmc_main_list.pxml_arr[mcs_idx];
 188       if (mcs != NULL) {
 189         val = pxmc_axis_rdmode(mcs);
 190         val = pxmc_axis_out_chans4mode(val);
 191         if (val > 0)
 192           out_chan = outmap[mcs_idx] + val;
 193       }
 194     }
 195     outmap[++mcs_idx] = out_chan;
 196   }
 197
 198   pxmc_for_each_mcs(mcs_idx, mcs)
 199   {
 200     if (mcs == NULL)
 201       continue;
 202     if(mcs->pxms_flg & PXMS_BSY_m)
 203       return -CMDERR_BSYREG;
 204     pxmc_axis_release(mcs);
 205     val = pxmc_axis_mode(mcs, PXMC_AXIS_MODE_NOCHANGE);
 206     if (val < 0)
 207       return -CMDERR_EIO;
 208   }
 209
 210   pxmc_for_each_mcs(mcs_idx, mcs)
 211   {
 212     if (mcs == NULL)
 213       continue;
 214     mcs->pxms_out_info = outmap[mcs_idx];
 215     val = pxmc_axis_mode(mcs, PXMC_AXIS_MODE_NOCHANGE);
 216     if (val < 0)
 217       return -CMDERR_EIO;
 218   }
 219
 220   return 0;
 221 }
 222
 223
 224 int cmd_do_ioecho(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 225 {
 226   int val;
 227   int opchar;
 228
 229   if((opchar=cmd_opchar_check(cmd_io, des, param))<0) return opchar;
 230   if(opchar == ':') {
 231     val = *param[3];
 232     if (val == '0')
 233       cmd_io->priv.ed_line.in->flg &= ~FL_ELB_ECHO;
 234     else if(val == '1')
 235       cmd_io->priv.ed_line.in->flg |= FL_ELB_ECHO;
 236     else return -CMDERR_BADPAR;
 237   }else{
 238     cmd_io_write(cmd_io, param[0], param[2]-param[0]);
 239     cmd_io_putc(cmd_io,'=');
 240     cmd_io_putc(cmd_io,cmd_io->priv.ed_line.in->flg & FL_ELB_ECHO?'1':'0');
 241   }
 242   return 0;
 243 }
 244
 245 const char software_ver[]="LX_ROCON v"STRINGIFY(SW_VER_MAJOR)"."STRINGIFY(SW_VER_MINOR)
 246            " build Pi "__DATE__" "__TIME__;
 247
 248 int cmd_do_ver(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 249 {
 250   if(*param[2]!='?') return -CMDERR_OPCHAR;
 251   cmd_io_write(cmd_io,param[0],param[2]-param[0]);
 252   cmd_io_putc(cmd_io,'=');
 253   cmd_io_write(cmd_io,software_ver,strlen(software_ver));
 254   return 0;
 255 }
 256
 257 int cmd_do_spmax(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 258 {
 259   if(*param[2]!='?') return -CMDERR_OPCHAR;
 260   return cmd_opchar_replong(cmd_io, param, pxmc_sfi_spent_time_max, 0, 0);
 261   pxmc_sfi_spent_time_max = 0;
 262   return 0;
 263 }
 264
 265 int cmd_do_sqrtll(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 266 {
 267   char *ps;
 268   ps = param[1];
 269   sqrtll_main(!ps?1: !*ps? 1: 2, param);
 270   return 0;
 271 }
 272
 273 cmd_des_t const cmd_des_cer_all={0, CDESM_OPCHR,"PURGE","clear error flags and release failded axes",
 274                         cmd_do_cer_all,{}};
 275 cmd_des_t const cmd_des_clr_all={0, CDESM_OPCHR,
 276                         "CLEAR",0,cmd_do_clr_all,{}};
 277 cmd_des_t const cmd_des_stop_all={0, CDESM_OPCHR,
 278                         "STOP","stop all motions",cmd_do_stop_all,{}};
 279 cmd_des_t const cmd_des_release_all={0, CDESM_OPCHR,
 280                         "RELEASE","releases all axes closed loop control",cmd_do_release_all,{}};
 281 cmd_des_t const cmd_des_status_all={0, CDESM_OPCHR|CDESM_RD,
 282                         "ST","system status bits encoded in number",
 283                         cmd_do_status_all,{}};
 284 cmd_des_t const cmd_des_status_bsybits={0, CDESM_OPCHR|CDESM_RD,
 285                         "STBSYBITS","busy bits of all axes in one number",
 286                         cmd_do_status_bsybits,{}};
 287 cmd_des_t const cmd_des_stamp={0, CDESM_OPCHR,
 288                         "STAMP","host communication stamp",
 289                         cmd_do_stamp,{}};
 290 cmd_des_t const cmd_des_pthalign={0, CDESM_OPCHR|CDESM_RW,"PTHALIGN?","run alignement of phase according to HAL",
 291                         cmd_do_pthalign,
 292                         {0,0}};
 293 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
 294                         "REGMODE?","axis working mode",cmd_do_axis_mode,
 295                         {}};
 296 cmd_des_t const cmd_des_axes_outmap={0, CDESM_OPCHR|CDESM_WR,
 297                         "REGOUTMAP","map outputs to axes",cmd_do_axes_outmap,
 298                         {}};
 299 cmd_des_t const cmd_des_errstop={0, CDESM_OPCHR|CDESM_RW,
 300                         "ERRSTOP","stop all axes at error",cmd_do_rw_int,
 301                         {(char*)&appl_errstop_mode,
 302                          0}};
 303 cmd_des_t const cmd_des_idlerel={0, CDESM_OPCHR|CDESM_RW,
 304                         "IDLEREL","release controllers after ? seconds",cmd_do_rw_int,
 305                         {(char*)&appl_idlerel_time,
 306                          0}};
 307 cmd_des_t const cmd_des_echo={0, CDESM_OPCHR|CDESM_RW,
 308                         "ECHO","enable echoing of received character",cmd_do_ioecho,{}};
 309 cmd_des_t const cmd_des_ver={0, CDESM_OPCHR|CDESM_RD,
 310                         "VER","software version",cmd_do_ver,{}};
 311 cmd_des_t const cmd_des_spmax={0, CDESM_OPCHR|CDESM_RD,
 312                         "SPMAX","read and reset maximal time spent in isr",
 313                         cmd_do_spmax,{}};
 314 cmd_des_t const cmd_des_sqrtll={0, 0,"sqrtll","test 64-bit square root computation",
 315                         cmd_do_sqrtll,
 316                         {0,0}};
 317
 318 cmd_des_t const *const cmd_appl_specific[]={
 319   &cmd_des_cer_all,
 320   &cmd_des_clr_all,
 321   &cmd_des_stop_all,
 322   &cmd_des_release_all,
 323   &cmd_des_status_all,
 324   &cmd_des_status_bsybits,
 325   &cmd_des_stamp,
 326   &cmd_des_pthalign,
 327   &cmd_des_axis_mode,
 328   &cmd_des_axes_outmap,
 329   &cmd_des_errstop,
 330   &cmd_des_idlerel,
 331   &cmd_des_echo,
 332   &cmd_des_ver,
 333   &cmd_des_spmax,
 334   &cmd_des_sqrtll,
 335   NULL
 336 };