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   {0x01, "A"}, {0x02, "B"}, {0x04, "C"}, {0x08, "D"},
  46   {0x10, "E"}, {0x20, "F"}, {0x40, "G"}, {0x80, "H"},
  47   {DBGPF_PER_TIME, "time"},
  48   {DBGPF_PER_POS,  "pos"},
  49   {DBGPF_CMD_PROC, "cmd"},
  50   {0,NULL}
  51 };
  52
  53 void run_dbg_prt(cmd_io_t *cmd_io)
  54 {
  55   char s[20];
  56   int i;
  57   pxmc_state_t *mcs;
  58
  59   if(dbg_prt_flg & DBGPF_PER_POS) {
  60     char reg_ascii_id[4];
  61     short reg_mask;
  62     reg_ascii_id[1]='!';
  63     reg_ascii_id[2]=0;
  64
  65     for(i=0,reg_mask=1;i<pxmc_main_list.pxml_cnt; i++,reg_mask<<=1){
  66       if(dbg_prt_flg&reg_mask&DBGPF_AXES){
  67         reg_ascii_id[0]='A'+i;
  68         cmd_io_puts(cmd_io,reg_ascii_id);
  69         mcs=pxmc_main_list.pxml_arr[i];
  70         i2str(s,mcs->pxms_ap>>PXMC_SUBDIV(mcs),8,0);
  71         cmd_io_puts(cmd_io,s);
  72         cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_ERR_m?" E":
  73                               mcs->pxms_flg&PXMS_BSY_m?" B":" -");
  74
  75         cmd_io_puts(cmd_io," hal");
  76         i2str(s,mcs->pxms_hal,2,0);
  77         cmd_io_puts(cmd_io,s);
  78
  79         cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_PHA_m?" A":
  80                               mcs->pxms_flg&PXMS_PTI_m?" I":" -");
  81
  82         cmd_io_puts(cmd_io," i");
  83         i2str(s,mcs->pxms_ptindx,4,0);
  84         cmd_io_puts(cmd_io,s);
  85
  86         cmd_io_puts(cmd_io," o");
  87         i2str(s,mcs->pxms_ptofs,6,0);
  88         cmd_io_puts(cmd_io,s);
  89
  90         cmd_io_puts(cmd_io," ene");
  91         i2str(s,mcs->pxms_ene,7,0);
  92         cmd_io_puts(cmd_io,s);
  93
  94         cmd_io_puts(cmd_io,"\r\n");
  95       }
  96     }
  97   }
  98 }
  99
 100 int cmd_do_switches(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 101 {
 102   unsigned val,*pval;
 103   long l;
 104   dbg_prt_des_t *pdes,*pd;
 105   char *ps;
 106   char str[20];
 107   char pm_flag;
 108   pval=(unsigned*)(des->info[0]);
 109   pdes=(dbg_prt_des_t*)(des->info[1]);
 110   ps=(des->mode&CDESM_OPCHR)?param[3]:param[1];
 111   val=*pval;
 112   if(*ps=='?'){
 113     while(pdes->name){
 114       printf(" %s",pdes->name);
 115       pdes++;
 116     }
 117     printf("\n");
 118   }
 119   while(*ps){
 120     si_skspace(&ps);
 121     if(!*ps) break;
 122     pm_flag=0;
 123     if(*ps=='+'){ps++;}
 124     else if(*ps=='-'){pm_flag=1;ps++;}
 125     else val=0;
 126     if(isdigit((uint8_t)*ps)){if(si_long(&ps,&l,0)<0) return -1;}
 127     else{
 128       si_alnumn(&ps,str,20);
 129       pd=pdes;
 130       do{
 131         if(!pd->name) return -1;
 132         if(!strcmp(pd->name,str)){
 133           l=pd->mask; break;
 134         }
 135         pd++;
 136       }while(1);
 137     }
 138     if(pm_flag) val&=~l;
 139     else val|=l;
 140   }
 141   *pval=val;
 142   return 0;
 143 }
 144
 145 cmd_des_t const cmd_des_dprint={0, 0,"dprint","enable debug messages to print, use + - to add remove, list types ?",
 146                         cmd_do_switches,
 147                         {(char*)&dbg_prt_flg,(char*)dbg_prt_des}};
 148