sw/app/rocon/appl_dprint.c

   1 /*******************************************************************
   2   Motion and Robotic System (MARS) aplication components
   3
   4   appl_dprint.c - debug print to the console
   5
   6   Copyright (C) 2001-2014 by Pavel Pisa - originator
   7                           pisa@cmp.felk.cvut.cz
   8             (C) 2001-2014 by PiKRON Ltd. - originator
   9                     http://www.pikron.com
  10
  11   This file can be used and copied according to next
  12   license alternatives
  13    - GPL - GNU Public License
  14    - other license provided by project originators
  15
  16  *******************************************************************/
  17
  18 #include <cpu_def.h>
  19 #include <system_def.h>
  20 #include <ctype.h>
  21 #include <string.h>
  22 #include <pxmc.h>
  23 #include <cmd_proc.h>
  24 #include <pxmc_cmds.h>
  25 #include <utils.h>
  26
  27 #include "appl_defs.h"
  28
  29 /* selection of debug messages */
  30
  31 #define DBGPF_AXES      0x00ff  /* mask for all axes */
  32 #define DBGPF_PER_TIME  0x0100  /* periodic time print */
  33 #define DBGPF_PER_POS   0x0800  /* periodic position info */
  34 #define DBGPF_CMD_PROC  0x1000  /* command proccessing */
  35 unsigned dbg_prt_flg=0;
  36
  37 typedef struct dbg_prt_des{
  38   unsigned mask;
  39   char *name;
  40 }dbg_prt_des_t;
  41
  42
  43 const dbg_prt_des_t dbg_prt_des[]={
  44   {~0,"all"},
  45   {1,"A"},{2,"B"},
  46   {DBGPF_PER_TIME,"time"},
  47   {DBGPF_PER_POS, "pos"},
  48   {DBGPF_CMD_PROC,"cmd"},
  49   {0,NULL}
  50 };
  51
  52 void run_dbg_prt(cmd_io_t *cmd_io)
  53 {
  54   char s[20];
  55   int i;
  56   pxmc_state_t *mcs;
  57
  58   if(dbg_prt_flg & DBGPF_PER_POS) {
  59     char reg_ascii_id[4];
  60     short reg_mask;
  61     reg_ascii_id[1]='!';
  62     reg_ascii_id[2]=0;
  63
  64     for(i=0,reg_mask=1;i<pxmc_main_list.pxml_cnt; i++,reg_mask<<=1){
  65       if(dbg_prt_flg&reg_mask&DBGPF_AXES){
  66         reg_ascii_id[0]='A'+i;
  67         cmd_io_puts(cmd_io,reg_ascii_id);
  68         mcs=pxmc_main_list.pxml_arr[i];
  69         i2str(s,mcs->pxms_ap>>PXMC_SUBDIV(mcs),8,0);
  70         cmd_io_puts(cmd_io,s);
  71         cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_ERR_m?" E":
  72                               mcs->pxms_flg&PXMS_BSY_m?" B":" -");
  73
  74         cmd_io_puts(cmd_io," hal");
  75         i2str(s,mcs->pxms_hal,2,0);
  76         cmd_io_puts(cmd_io,s);
  77
  78         cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_PHA_m?" A":
  79                               mcs->pxms_flg&PXMS_PTI_m?" I":" -");
  80
  81         cmd_io_puts(cmd_io," i");
  82         i2str(s,mcs->pxms_ptindx,4,0);
  83         cmd_io_puts(cmd_io,s);
  84
  85         cmd_io_puts(cmd_io," o");
  86         i2str(s,mcs->pxms_ptofs,6,0);
  87         cmd_io_puts(cmd_io,s);
  88
  89         cmd_io_puts(cmd_io,"\r\n");
  90       }
  91     }
  92   }
  93 }
  94
  95 int cmd_do_switches(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
  96 {
  97   unsigned val,*pval;
  98   long l;
  99   dbg_prt_des_t *pdes,*pd;
 100   char *ps;
 101   char str[20];
 102   char pm_flag;
 103   pval=(unsigned*)(des->info[0]);
 104   pdes=(dbg_prt_des_t*)(des->info[1]);
 105   ps=(des->mode&CDESM_OPCHR)?param[3]:param[1];
 106   val=*pval;
 107   if(*ps=='?'){
 108     while(pdes->name){
 109       printf(" %s",pdes->name);
 110       pdes++;
 111     }
 112     printf("\n");
 113   }
 114   while(*ps){
 115     si_skspace(&ps);
 116     if(!*ps) break;
 117     pm_flag=0;
 118     if(*ps=='+'){ps++;}
 119     else if(*ps=='-'){pm_flag=1;ps++;}
 120     else val=0;
 121     if(isdigit((uint8_t)*ps)){if(si_long(&ps,&l,0)<0) return -1;}
 122     else{
 123       si_alnumn(&ps,str,20);
 124       pd=pdes;
 125       do{
 126         if(!pd->name) return -1;
 127         if(!strcmp(pd->name,str)){
 128           l=pd->mask; break;
 129         }
 130         pd++;
 131       }while(1);
 132     }
 133     if(pm_flag) val&=~l;
 134     else val|=l;
 135   }
 136   *pval=val;
 137   return 0;
 138 }
 139
 140 cmd_des_t const cmd_des_dprint={0, 0,"dprint","enable debug messages to print, use + - to add remove, list types ?",
 141                         cmd_do_switches,
 142                         {(char*)&dbg_prt_flg,(char*)dbg_prt_des}};
 143