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 int cmd_do_reg_type(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 100 {
 101   int val;
 102   pxmc_state_t *mcs;
 103
 104   if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 105
 106   if(*param[2]=='?') {
 107     return cmd_opchar_replong(cmd_io, param, pxmc_get_reg_type(mcs), 0, 0);
 108   }
 109
 110   if(*param[2]!=':') return -CMDERR_OPCHAR;
 111
 112   if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
 113
 114   val=atol(param[3]);
 115   val=pxmc_set_reg_type(mcs, val);
 116
 117   if(val<0)
 118     return val;
 119
 120   return 0;
 121 }
 122
 123 /**
 124  * cmd_do_axis_mode - checks the command format and busy flag validity, calls pxmc_axis_mode
 125  *
 126  * if pxmc_axis_mode returns -1, cmd_do_axis_mode returns -1.
 127  */
 128 int cmd_do_axis_mode(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 129 {
 130   int val;
 131   pxmc_state_t *mcs;
 132
 133   if((mcs=cmd_opchar_getreg(cmd_io,des,param))==NULL) return -CMDERR_BADREG;
 134
 135   if(*param[2]=='?') {
 136     return cmd_opchar_replong(cmd_io, param, pxmc_axis_rdmode(mcs), 0, 0);
 137   }
 138
 139   if(*param[2]!=':') return -CMDERR_OPCHAR;
 140
 141   if(mcs->pxms_flg&PXMS_BSY_m) return -CMDERR_BSYREG;
 142
 143   val=atol(param[3]);
 144   val=pxmc_axis_mode(mcs,val);
 145   if(val<0)
 146     return val;
 147
 148   return 0;
 149 }
 150
 151 int cmd_do_axes_outmap(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 152 {
 153   long val;
 154   char *ps;
 155   ps = param[3];
 156   int mcs_idx;
 157   int out_chan = 0;
 158   pxmc_state_t *mcs;
 159   int exes_cnt = pxmc_main_list.pxml_cnt > 0? pxmc_main_list.pxml_cnt : 0;
 160   pxmc_info_t outmap[exes_cnt];
 161
 162   if (exes_cnt == 0)
 163     return 0;
 164
 165   if(*param[2]=='?') {
 166     char str[20];
 167     cmd_io_write(cmd_io,param[0],param[2]-param[0]);
 168
 169     pxmc_for_each_mcs(mcs_idx, mcs)
 170     {
 171       cmd_io_putc(cmd_io, mcs_idx? ',' : '=');
 172       if (mcs != NULL) {
 173         val = mcs->pxms_out_info;
 174         i2str(str, val, 0, 0);
 175         cmd_io_write(cmd_io,str,strlen(str));
 176       } else {
 177         cmd_io_putc(cmd_io, 'X');
 178       }
 179     }
 180
 181     return 0;
 182   }
 183
 184   if(*param[2]!=':') return -CMDERR_OPCHAR;
 185
 186   mcs_idx = -1;
 187   si_skspace(&ps);
 188   do {
 189     if (!*ps)
 190       break;
 191     if (mcs_idx >= exes_cnt - 1)
 192       return -CMDERR_BADREG;
 193
 194     if (si_long(&ps, &val, 0) < 0)
 195       return -CMDERR_BADPAR;
 196
 197     if (val < 0)
 198       return -CMDERR_BADPAR;
 199
 200     outmap[++mcs_idx] = val;
 201
 202     si_skspace(&ps);
 203
 204     if (*ps)
 205       if(*(ps++) != ',')
 206         return -CMDERR_BADSEP;
 207   } while(1);
 208
 209   while (mcs_idx < exes_cnt - 1) {
 210     if (mcs_idx >= 0) {
 211       mcs = pxmc_main_list.pxml_arr[mcs_idx];
 212       if (mcs != NULL) {
 213         val = pxmc_axis_rdmode(mcs);
 214         val = pxmc_axis_out_chans4mode(val);
 215         if (val > 0)
 216           out_chan = outmap[mcs_idx] + val;
 217       }
 218     }
 219     outmap[++mcs_idx] = out_chan;
 220   }
 221
 222   pxmc_for_each_mcs(mcs_idx, mcs)
 223   {
 224     if (mcs == NULL)
 225       continue;
 226     if(mcs->pxms_flg & PXMS_BSY_m)
 227       return -CMDERR_BSYREG;
 228     pxmc_axis_release(mcs);
 229     val = pxmc_axis_mode(mcs, PXMC_AXIS_MODE_NOCHANGE);
 230     if (val < 0)
 231       return -CMDERR_EIO;
 232   }
 233
 234   pxmc_for_each_mcs(mcs_idx, mcs)
 235   {
 236     if (mcs == NULL)
 237       continue;
 238     mcs->pxms_out_info = outmap[mcs_idx];
 239     val = pxmc_axis_mode(mcs, PXMC_AXIS_MODE_NOCHANGE);
 240     if (val < 0)
 241       return -CMDERR_EIO;
 242   }
 243
 244   return 0;
 245 }
 246
 247
 248 int cmd_do_ioecho(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 249 {
 250   int val;
 251   int opchar;
 252
 253   if((opchar=cmd_opchar_check(cmd_io, des, param))<0) return opchar;
 254   if(opchar == ':') {
 255     val = *param[3];
 256     if (val == '0')
 257       cmd_io->priv.ed_line.in->flg &= ~FL_ELB_ECHO;
 258     else if(val == '1')
 259       cmd_io->priv.ed_line.in->flg |= FL_ELB_ECHO;
 260     else return -CMDERR_BADPAR;
 261   }else{
 262     cmd_io_write(cmd_io, param[0], param[2]-param[0]);
 263     cmd_io_putc(cmd_io,'=');
 264     cmd_io_putc(cmd_io,cmd_io->priv.ed_line.in->flg & FL_ELB_ECHO?'1':'0');
 265   }
 266   return 0;
 267 }
 268
 269 const char software_ver[]="LX_ROCON v"STRINGIFY(SW_VER_MAJOR)"."STRINGIFY(SW_VER_MINOR)
 270            " build Pi "__DATE__" "__TIME__;
 271
 272 int cmd_do_ver(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 273 {
 274   if(*param[2]!='?') return -CMDERR_OPCHAR;
 275   cmd_io_write(cmd_io,param[0],param[2]-param[0]);
 276   cmd_io_putc(cmd_io,'=');
 277   cmd_io_write(cmd_io,software_ver,strlen(software_ver));
 278   return 0;
 279 }
 280
 281 int cmd_do_spmax(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 282 {
 283   if(*param[2]!='?') return -CMDERR_OPCHAR;
 284   return cmd_opchar_replong(cmd_io, param, pxmc_sfi_spent_time_max, 0, 0);
 285   pxmc_sfi_spent_time_max = 0;
 286   return 0;
 287 }
 288
 289 int cmd_do_sqrtll(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 290 {
 291   char *ps;
 292   ps = param[1];
 293   sqrtll_main(!ps?1: !*ps? 1: 2, param);
 294   return 0;
 295 }
 296
 297 cmd_des_t const cmd_des_cer_all={0, CDESM_OPCHR,"PURGE","clear error flags and release failded axes",
 298                         cmd_do_cer_all,{}};
 299 cmd_des_t const cmd_des_clr_all={0, CDESM_OPCHR,
 300                         "CLEAR",0,cmd_do_clr_all,{}};
 301 cmd_des_t const cmd_des_stop_all={0, CDESM_OPCHR,
 302                         "STOP","stop all motions",cmd_do_stop_all,{}};
 303 cmd_des_t const cmd_des_release_all={0, CDESM_OPCHR,
 304                         "RELEASE","releases all axes closed loop control",cmd_do_release_all,{}};
 305 cmd_des_t const cmd_des_status_all={0, CDESM_OPCHR|CDESM_RD,
 306                         "ST","system status bits encoded in number",
 307                         cmd_do_status_all,{}};
 308 cmd_des_t const cmd_des_status_bsybits={0, CDESM_OPCHR|CDESM_RD,
 309                         "STBSYBITS","busy bits of all axes in one number",
 310                         cmd_do_status_bsybits,{}};
 311 cmd_des_t const cmd_des_stamp={0, CDESM_OPCHR,
 312                         "STAMP","host communication stamp",
 313                         cmd_do_stamp,{}};
 314 cmd_des_t const cmd_des_pthalign={0, CDESM_OPCHR|CDESM_RW,"PTHALIGN?","run alignement of phase according to HAL",
 315                         cmd_do_pthalign,
 316                         {0,0}};
 317 cmd_des_t const cmd_des_reg_type={0, CDESM_OPCHR|CDESM_RW,
 318                         "REGTYPE?","select controller structure and type",cmd_do_reg_type,
 319                         {}};
 320 cmd_des_t const cmd_des_axis_mode={0, CDESM_OPCHR|CDESM_WR,
 321                         "REGMODE?","axis working mode",cmd_do_axis_mode,
 322                         {}};
 323 cmd_des_t const cmd_des_axes_outmap={0, CDESM_OPCHR|CDESM_WR,
 324                         "REGOUTMAP","map outputs to axes",cmd_do_axes_outmap,
 325                         {}};
 326 cmd_des_t const cmd_des_errstop={0, CDESM_OPCHR|CDESM_RW,
 327                         "ERRSTOP","stop all axes at error",cmd_do_rw_int,
 328                         {(char*)&appl_errstop_mode,
 329                          0}};
 330 cmd_des_t const cmd_des_idlerel={0, CDESM_OPCHR|CDESM_RW,
 331                         "IDLEREL","release controllers after ? seconds",cmd_do_rw_int,
 332                         {(char*)&appl_idlerel_time,
 333                          0}};
 334 cmd_des_t const cmd_des_echo={0, CDESM_OPCHR|CDESM_RW,
 335                         "ECHO","enable echoing of received character",cmd_do_ioecho,{}};
 336 cmd_des_t const cmd_des_ver={0, CDESM_OPCHR|CDESM_RD,
 337                         "VER","software version",cmd_do_ver,{}};
 338 cmd_des_t const cmd_des_spmax={0, CDESM_OPCHR|CDESM_RD,
 339                         "SPMAX","read and reset maximal time spent in isr",
 340                         cmd_do_spmax,{}};
 341 cmd_des_t const cmd_des_sqrtll={0, 0,"sqrtll","test 64-bit square root computation",
 342                         cmd_do_sqrtll,
 343                         {0,0}};
 344
 345 cmd_des_t const *const cmd_appl_specific[]={
 346   &cmd_des_cer_all,
 347   &cmd_des_clr_all,
 348   &cmd_des_stop_all,
 349   &cmd_des_release_all,
 350   &cmd_des_status_all,
 351   &cmd_des_status_bsybits,
 352   &cmd_des_stamp,
 353   &cmd_des_pthalign,
 354   &cmd_des_reg_type,
 355   &cmd_des_axis_mode,
 356   &cmd_des_axes_outmap,
 357   &cmd_des_errstop,
 358   &cmd_des_idlerel,
 359   &cmd_des_echo,
 360   &cmd_des_ver,
 361   &cmd_des_spmax,
 362   &cmd_des_sqrtll,
 363   NULL
 364 };