motor-control: Fixed watchdog and LEDs

watchdog reset the board when long command like HELP or haltest was
issued.

diff --git a/src/motor-control/brushless.c b/src/motor-control/brushless.c
index a16e65d48aedf0aa5ac92862a9ae08790344306e..bbc5bc0b74a371f4c9a8ba5be6f1eb75875a46a0 100644 (file)
--- a/src/motor-control/brushless.c
+++ b/src/motor-control/brushless.c
@@ -25,6 +25,11 @@
 #include <canOpenDriver.h>
 #include <can_ids.h>
 
+/* Meaning of LEDs */
+#define LED_LIVE   0           /* Blinks once per second when software is alive */
+#define LED_CAN_REC 1          /* Blinks for 0.1 sec when a CAN message is received */
+#define LED_ERROR  2           /* Unhandled exception */
+#define LED_RESET  3           /* Blinks for 1 s after reset */
 
 
 int cmd_do_quit(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
@@ -69,6 +74,7 @@ int cmd_do_haltest(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
         pxmc_initialize(); // reset index mark detection
         do
         {
+            wdg_clear();
             if(turn<1)i++;
             else i--;
             int index = ((long)i*mcs->pxms_ptirc/360)%mcs->pxms_ptirc;
@@ -181,6 +187,70 @@ int cmd_do_mypwm(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
     return 0;
 }
 
+/**
+ * Implementation of help command with watchdog
+ */
+static inline char *skip_white(char *p)
+{
+  while(isspace((__u8)*p)) p++;
+  return p;
+}
+
+
+#define CMD_ARR_STACK_SIZE 4    
+int cmd_do_help_wd(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+  char *help;
+  char *filt=param[1];
+  const cmd_des_t **des_arr=*(const cmd_des_t ***)des->info[0];
+  cmd_des_t const **arr_stack[CMD_ARR_STACK_SIZE];
+  int arr_stack_sp=0;
+
+  if (cmd_io->priv.ed_line.io_stack)
+    cmd_io = cmd_io->priv.ed_line.io_stack;
+  
+  if(filt) {
+    filt=skip_white(filt);
+    if(!*filt) filt=NULL;
+  }
+  cmd_io_puts(cmd_io,"Help for commands\n");
+  while(1){
+    wdg_clear();               /* Added to avoid reseting while listing commands */
+    des=*(des_arr++);
+    if(!des){
+      if(!arr_stack_sp) break;
+      des_arr=arr_stack[--arr_stack_sp];
+      continue;
+    }
+    if(des==CMD_DES_CONTINUE_AT_ID){
+      /* list continues at new address */
+      des_arr=(const cmd_des_t **)*des_arr;
+      continue;
+    }
+    if(des==CMD_DES_INCLUDE_SUBLIST_ID){
+      /* list includes commands from sublists */
+      if(arr_stack_sp>=CMD_ARR_STACK_SIZE){
+        des_arr++;
+      }else{
+        arr_stack[arr_stack_sp++]=des_arr+1;
+        des_arr=(const cmd_des_t **)*des_arr;
+        continue;
+      }
+    }
+    if(des->name){
+      if(!filt || !strncmp(des->name,filt,strlen(filt))) {
+        help=des->help;
+        if(!help) help="?";
+        cmd_io_puts(cmd_io,des->name);
+        cmd_io_puts(cmd_io," - ");
+        cmd_io_puts(cmd_io,help);
+        cmd_io_puts(cmd_io, "\r\n");
+      }
+    }
+  }
+  return 0;
+}
+
 int runtime_display = 0;
 int slowgo = 0;
 
@@ -188,7 +258,7 @@ cmd_des_t const **cmd_list;
 
 /* command descriptions */
 cmd_des_t const cmd_des_help={0, 0,"HELP","prints help for commands",
-                              cmd_do_help,{(char*)&cmd_list}};
+                              cmd_do_help_wd,{(char*)&cmd_list}};
 cmd_des_t const cmd_des_showene={0, 0,"showene","shows ene and couple others parameters",
                                  cmd_do_showene};
 
@@ -419,6 +489,7 @@ int main()
     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");
@@ -440,7 +511,11 @@ int main()
         cmd_processor_run(&cmd_io_rs232_line, cmd_list_default);  // run command processor on serial line
         i++;
 
-        if (pxmc_msec_counter%1000 > 500) DEB_LED_ON(LED_LIVE);
+        if (pxmc_msec_counter%1000 > 500) {
+         DEB_LED_ON(LED_LIVE);
+         if (pxmc_msec_counter > 1000)
+           DEB_LED_OFF(LED_RESET);
+       }
         else DEB_LED_OFF(LED_LIVE);
 
         if (runtime_display) {