CAN dump printing out received messages

diff --git a/commands/cmd_can.c b/commands/cmd_can.c
index e23aa1a7bf9334743316574291f12a00926c67db..569044956be2df6a127c2283bca4a0168ec9b7fb 100644 (file)
--- a/commands/cmd_can.c
+++ b/commands/cmd_can.c
@@ -340,7 +340,7 @@ int cmd_do_can_dump(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
 
     uint32_t i;
 
-    if (!(sscanf(*param+8, "%u", &controller_id) == 1))
+    if (!(sscanf(param[1], "%u", &controller_id) == 1))
     {
         rpp_sci_printf("Unable to parse controller ID\n");
         return 1;
@@ -353,8 +353,27 @@ int cmd_do_can_dump(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
         rpp_can_check_rx_ind(controller_id-1, &rx_ind);
         if (rx_ind)
         {
-            rpp_can_read(controller_id-1, &pdu);
-            rpp_sci_printf("Read something\n");
+                       if (rpp_can_read(controller_id-1, &pdu) == SUCCESS)
+                       {
+                               if (pdu.id & CAN_EFF_FLAG)
+                               {
+                                       rpp_sci_printf("can%u  %08X  [%u]  ", controller_id & (~CAN_EFF_FLAG), pdu.id, pdu.dlc);
+                               }
+                               else
+                               {
+                                       rpp_sci_printf("can%u  %03X  [%u]  ", controller_id, pdu.id, pdu.dlc);
+                               }
+
+                               for (i=0; i<pdu.dlc; i++)
+                               {
+                                           rpp_sci_printf("%X ", pdu.data[i]);
+                               }
+                               rpp_sci_printf("\n");
+                       }
+                       else
+                       {
+                               rpp_sci_printf("Error rpp_can_read\n");
+                       }
         }
     }