Merge pull request #709 from arduino/fix-support-extended-and-standard

[Arduino_CAN] support both 11-Bit (standard) and 29-Bit (extended) IDs

Author: aentinger

Diff for: libraries/Arduino_CAN/examples/CANWrite/CANWrite.ino

@@ -35,7 +35,7 @@ void loop()
    */
   uint8_t const msg_data[] = {0xCA,0xFE,0,0,0,0,0,0};
   memcpy((void *)(msg_data + 4), &msg_cnt, sizeof(msg_cnt));
-  CanMsg msg(CAN_ID, sizeof(msg_data), msg_data);
+  CanMsg msg(CanStandardId(CAN_ID), sizeof(msg_data), msg_data);
 
   /* Transmit the CAN message, capture and display an
    * error core in case of failure.

Diff for: libraries/Arduino_CAN/src/Arduino_CAN.cpp

@@ -48,12 +48,14 @@ void Arduino_CAN::end()
 
 int Arduino_CAN::write(CanMsg const & msg)
 {
+  bool const is_standard_id = msg.isStandardId();
+
   mbed::CANMessage const can_msg(
-    msg.id,
+    is_standard_id ? msg.getStandardId()  : msg.getExtendedId(),
     msg.data,
     msg.data_length,
     CANData,
-    CANStandard);
+    is_standard_id ? CANStandard : CANExtended);
 
   return _can.write(can_msg);
 }
@@ -65,8 +67,10 @@ size_t Arduino_CAN::available()
 
   if (msg_read)
   {
+    bool const is_standard_id = (can_msg.format == CANStandard);
+
     CanMsg const msg(
-      can_msg.id,
+      is_standard_id ? CanStandardId(can_msg.id) : CanExtendedId(can_msg.id),
       can_msg.len,
       can_msg.data);