motor-control: add debugging command to run periodic state print.

Signed-off-by: Pavel Pisa <pisa@cmp.felk.cvut.cz>

diff --git a/src/motor-control/brushless.c b/src/motor-control/brushless.c
index 842357bed64c9016ba58c96b4e99094e2b7f87ff..91789643dca298b60a7f8003cb4cda00b161f6ee 100644 (file)
--- a/src/motor-control/brushless.c
+++ b/src/motor-control/brushless.c
@@ -336,6 +336,102 @@ int cmd_do_help_wd(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
   return 0;
 }
 
+/* selection of debug messages */
+
+#define        DBGPF_AXES      0x00ff  /* mask for all axes */
+#define        DBGPF_PER_TIME  0x0100  /* periodic time print */
+#define        DBGPF_PER_POS   0x0800  /* periodic position info */
+#define        DBGPF_CMD_PROC  0x1000  /* command proccessing */
+unsigned dbg_prt_flg=0;
+
+typedef struct dbg_prt_des{
+  unsigned mask;
+  char *name;
+}dbg_prt_des_t;
+
+const dbg_prt_des_t dbg_prt_des[]={
+  {~0,"all"},
+  {1,"A"},{2,"B"},
+  {DBGPF_PER_TIME,"time"},
+  {DBGPF_PER_POS, "pos"},
+  {DBGPF_CMD_PROC,"cmd"},
+  {0,NULL}
+};
+
+
+void run_dbg_prt(cmd_io_t *cmd_io)
+{
+  char s[20];
+  int i;
+  pxmc_state_t *mcs;
+ 
+  if(dbg_prt_flg & DBGPF_PER_POS) {
+    char reg_ascii_id[4];
+    short reg_mask;
+    reg_ascii_id[1]='!';
+    reg_ascii_id[2]=0;
+       
+    for(i=0,reg_mask=1;i<pxmc_main_list.pxml_cnt; i++,reg_mask<<=1){
+      if(dbg_prt_flg&reg_mask&DBGPF_AXES){
+       reg_ascii_id[0]='A'+i;
+        cmd_io_puts(cmd_io,reg_ascii_id);
+        mcs=pxmc_main_list.pxml_arr[i];
+        i2str(s,mcs->pxms_ap>>PXMC_SUBDIV(mcs),8,0);
+        cmd_io_puts(cmd_io,s);
+        cmd_io_puts(cmd_io,mcs->pxms_flg&PXMS_ERR_m?" E":
+                             mcs->pxms_flg&PXMS_BSY_m?" B":" -");
+        cmd_io_puts(cmd_io,"\r\n");
+      }
+    }
+  }
+}
+
+int cmd_do_switches(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+  unsigned val,*pval;
+  long l;
+  dbg_prt_des_t *pdes,*pd;
+  char *ps;
+  char str[20];
+  char pm_flag;
+  pval=(unsigned*)(des->info[0]);
+  pdes=(dbg_prt_des_t*)(des->info[1]);
+  ps=(des->mode&CDESM_OPCHR)?param[3]:param[1];
+  val=*pval;
+  if(*ps=='?'){
+    while(pdes->name){
+      printf(" %s",pdes->name);
+      pdes++;
+    }
+    printf("\n");
+  }
+  while(*ps){
+    si_skspace(&ps);
+    if(!*ps) break;
+    pm_flag=0;
+    if(*ps=='+'){ps++;}
+    else if(*ps=='-'){pm_flag=1;ps++;}
+    else val=0;
+    if(isdigit((uint8_t)*ps)){if(si_long(&ps,&l,0)<0) return -1;}
+    else{
+      si_alnumn(&ps,str,20);
+      pd=pdes;
+      do{
+        if(!pd->name) return -1;
+       if(!strcmp(pd->name,str)){
+         l=pd->mask; break;
+       }
+       pd++;
+      }while(1);
+    }
+    if(pm_flag) val&=~l;
+    else val|=l;
+  }
+  *pval=val;
+  return 0;
+}
+
+
 int runtime_display = 0;
 int slowgo = 0;
 
@@ -368,6 +464,10 @@ cmd_des_t const cmd_des_quit={0, 0,"QUIT","exit",
 cmd_des_t const cmd_des_canst={0, 0,"CANST","Print CAN controller status",
                               cmd_do_canst};
 
+cmd_des_t const cmd_des_dprint={0, 0,"dprint","enable debug messages to print, use + - to add remove, list types ?",
+                       cmd_do_switches,
+                       {(char*)&dbg_prt_flg,(char*)dbg_prt_des}};
+
 cmd_des_t const cmd_des_setflags={0, 0,"setflags","set some flags",
                                   cmd_do_setflags};
 
@@ -388,6 +488,7 @@ cmd_des_t const *cmd_list_default[]={
     &cmd_des_disp,
     &cmd_des_slowgo,
     &cmd_des_canst,
+    &cmd_des_dprint,
     (cmd_des_t*)1,
     (cmd_des_t*)cmd_stm_default,
     NULL
@@ -700,12 +801,16 @@ static INLINE void handle_leds()
 void _print(char *ptr);
 int main()
 {
+    int dbg_prt_last_msec;
+
     cli();
     excptvec_initfill(unhandled_exception, 0);
     sci_rs232_setmode(19200, 0, 0, sci_rs232_chan_default); //PC
     DEB_LED_ON(LED_RESET);
     wdg_enable(6);              /* 420 ms */
     sti();
+
+
     _print("CPU initialized\r\n\r\n");
     
     printf("Eurobot motor control application.\n"
@@ -722,14 +827,28 @@ int main()
     canInit(0, 1000000, receive_id);
     printf("CAN initialized\n");
 
+    dbg_prt_last_msec = pxmc_msec_counter;
+
     do {
+        int res;
+
         wdg_clear();
         handle_can_receive();
         handle_odometry_send();
         handle_status_send();
         handle_motor_errors();
 
-        cmd_processor_run(&cmd_io_rs232_line, cmd_list_default);  // run command processor on serial line
+        res = cmd_processor_run(&cmd_io_rs232_line, cmd_list_default);  // run command processor on serial line
+
+        if(!res && ((int)(pxmc_msec_counter - dbg_prt_last_msec) > 2000) &&
+          !cmd_io_rs232_line.priv.ed_line.in->lastch) {
+          cmd_io_t *cmd_io = &cmd_io_rs232_line;
+          dbg_prt_last_msec = pxmc_msec_counter;
+          
+           if (cmd_io->priv.ed_line.io_stack)
+             cmd_io = cmd_io->priv.ed_line.io_stack;
+           run_dbg_prt(cmd_io);
+        }
 
        handle_leds();