Can FD support

Breaks compat with can_msgs/Frame due to changed array type

can_msgs/msg/Frame.msg

@@ -4,4 +4,6 @@ bool is_rtr
 bool is_extended
 bool is_error
 uint8 dlc
-uint8[8] data
+uint8[] data
+
+uint8 DLC_FD_BRS_FLAG = 128

socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h

@@ -64,9 +64,11 @@ void convertSocketCANToMessage(const can::Frame& f, can_msgs::Frame& m)
   m.is_rtr = f.is_rtr;
   m.is_extended = f.is_extended;
 
-  for (int i = 0; i < 8; i++)  // always copy all data, regardless of dlc.
+  m.data.reserve(f.dlc);
+
+  for (int i = 0; i < f.dlc; i++)
   {
-    m.data[i] = f.data[i];
+    m.data.push_back(f.data[i]);
   }
 };
 

socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h

@@ -32,6 +32,16 @@
 #include <can_msgs/Frame.h>
 #include <ros/ros.h>
 
+#define _MASK(offset, len) (((1u << ((offset) + (len))) - 1u) ^ ((1u << (offset))-1u))
+
+#define CANMSG_DLC_DLC_OFFSET (0u)
+#define CANMSG_DLC_DLC_LEN (7u)
+#define CANMSG_DLC_DLC_MASK _MASK(CANMSG_DLC_DLC_OFFSET, CANMSG_DLC_DLC_LEN)
+
+#define CANMSG_DLC_FDFLAGS_OFFSET (CANMSG_DLC_DLC_LEN)
+#define CANMSG_DLC_FDFLAGS_LEN (1u)
+#define CANMSG_DLC_FDFLAGS_MASK _MASK(CANMSG_DLC_FDFLAGS_OFFSET, CANMSG_DLC_FDFLAGS_LEN)
+
 namespace socketcan_bridge
 {
 class TopicToSocketCAN
@@ -53,12 +63,16 @@ class TopicToSocketCAN
 void convertMessageToSocketCAN(const can_msgs::Frame& m, can::Frame& f)
 {
   f.id = m.id;
-  f.dlc = m.dlc;
+  f.dlc = (m.dlc & CANMSG_DLC_DLC_MASK) >> CANMSG_DLC_DLC_OFFSET;
+  f.is_fd = ((m.dlc & CANMSG_DLC_FDFLAGS_MASK) != 0) || (f.dlc > 8);
+  f.flags = (m.dlc & CANMSG_DLC_FDFLAGS_MASK) >> CANMSG_DLC_FDFLAGS_OFFSET;
   f.is_error = m.is_error;
   f.is_rtr = m.is_rtr;
   f.is_extended = m.is_extended;
 
-  for (int i = 0; i < 8; i++)  // always copy all data, regardless of dlc.
+  int copy_len = m.data.size() > f.data.size() ? f.data.size() : m.data.size();
+
+  for (int i = 0; i < copy_len; i++)
   {
     f.data[i] = m.data[i];
   }

socketcan_bridge/test/test_conversion.cpp

@@ -95,7 +95,7 @@ TEST(ConversionTest, topicToSocketCANStandard)
   m.is_extended = false;
   for (uint8_t i = 0; i < m.dlc; ++i)
   {
-    m.data[i] = i;
+    m.data.push_back(i);
   }
   socketcan_bridge::convertMessageToSocketCAN(m, f);
   EXPECT_EQ(127, f.id);

socketcan_bridge/test/to_socketcan_test.cpp

@@ -83,7 +83,7 @@ TEST(TopicToSocketCANTest, checkCorrectData)
   msg.dlc = 8;
   for (uint8_t i=0; i < msg.dlc; i++)
   {
-    msg.data[i] = i;
+    msg.data.push_back(i);
   }
 
   msg.header.frame_id = "0";  // "0" for no frame.
@@ -108,6 +108,65 @@ TEST(TopicToSocketCANTest, checkCorrectData)
   EXPECT_EQ(received.data, msg.data);
 }
 
+TEST(TopicToSocketCANTest, checkCorrectFdData)
+{
+  ros::NodeHandle nh(""), nh_param("~");
+
+  // create the dummy interface
+  can::DummyInterfaceSharedPtr driver_ = std::make_shared<can::DummyInterface>(true);
+
+  // start the to topic bridge.
+  socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver_);
+  to_socketcan_bridge.setup();
+
+  // init the driver to test stateListener (not checked automatically).
+  driver_->init("string_not_used", true);
+
+  // register for messages on received_messages.
+  ros::Publisher publisher_ = nh.advertise<can_msgs::Frame>("sent_messages", 10);
+
+  // create a frame collector.
+  frameCollector frame_collector_;
+
+  //  driver->createMsgListener(&frameCallback);
+  can::FrameListenerConstSharedPtr frame_listener_ = driver_->createMsgListener(
+
+            std::bind(&frameCollector::frameCallback, &frame_collector_, std::placeholders::_1));
+
+  // create a message
+  can_msgs::Frame msg;
+  msg.is_extended = true;
+  msg.is_rtr = false;
+  msg.is_error = false;
+  msg.id = 0x123;
+  msg.dlc = 64 | can_msgs::Frame::DLC_FD_BRS_FLAG;
+  for (uint8_t i=0; i < 64; i++)
+  {
+    msg.data.push_back(i);
+  }
+
+  msg.header.frame_id = "0";  // "0" for no frame.
+  msg.header.stamp = ros::Time::now();
+
+  // send the can_frame::Frame message to the sent_messages topic.
+  publisher_.publish(msg);
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+
+  can_msgs::Frame received;
+  can::Frame f = frame_collector_.frames.back();
+  socketcan_bridge::convertSocketCANToMessage(f, received);
+
+  EXPECT_EQ(received.id, msg.id);
+  EXPECT_EQ(received.dlc, 64);
+  EXPECT_EQ(received.is_extended, msg.is_extended);
+  EXPECT_EQ(received.is_rtr, msg.is_rtr);
+  EXPECT_EQ(received.is_error, msg.is_error);
+  EXPECT_EQ(received.data, msg.data);
+}
+
 TEST(TopicToSocketCANTest, checkInvalidFrameHandling)
 {
   // - tries to send a non-extended frame with an id larger than 11 bits.
@@ -162,8 +221,8 @@ TEST(TopicToSocketCANTest, checkInvalidFrameHandling)
   frame_collector_.frames.clear();
 
 
-  // finally, check if frames with a dlc > 8 are discarded.
-  msg.dlc = 10;
+  // finally, check if frames with a dlc > 64 are discarded.
+  msg.dlc = 65;
   publisher_.publish(msg);
   ros::WallDuration(1.0).sleep();
   ros::spinOnce();

socketcan_bridge/test/to_topic_test.cpp

@@ -109,7 +109,69 @@ TEST(SocketCANToTopicTest, checkCorrectData)
   EXPECT_EQ(received.is_extended, f.is_extended);
   EXPECT_EQ(received.is_rtr, f.is_rtr);
   EXPECT_EQ(received.is_error, f.is_error);
-  EXPECT_EQ(received.data, f.data);
+  for (int i = 0; i < f.dlc; ++i)
+  {
+    EXPECT_EQ(received.data[i], f.data[i]);
+  }
+}
+
+TEST(SocketCANToTopicTest, checkCorrectFdData)
+{
+  ros::NodeHandle nh(""), nh_param("~");
+
+  // create the dummy interface
+  can::DummyInterfaceSharedPtr driver_ = std::make_shared<can::DummyInterface>(true);
+
+  // start the to topic bridge.
+  socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver_);
+  to_topic_bridge.setup();  // initiate the message callbacks
+
+  // init the driver to test stateListener (not checked automatically).
+  driver_->init("string_not_used", true);
+
+  // create a frame collector.
+  msgCollector message_collector_;
+
+  // register for messages on received_messages.
+  ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_);
+
+  // create a can frame
+  can::Frame f;
+  f.is_extended = true;
+  f.is_rtr = false;
+  f.is_error = false;
+  f.id = 0x123;
+  f.dlc = 64;
+  f.is_fd = true;
+  for (uint8_t i=0; i < f.dlc; i++)
+  {
+    f.data[i] = i;
+  }
+
+  // send the can frame to the driver
+  driver_->send(f);
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+
+  ASSERT_EQ(1, message_collector_.messages.size());
+
+  // compare the received can_msgs::Frame message to the sent can::Frame.
+  can::Frame received;
+  can_msgs::Frame msg = message_collector_.messages.back();
+  socketcan_bridge::convertMessageToSocketCAN(msg, received);
+
+  EXPECT_EQ(received.id, f.id);
+  EXPECT_EQ(received.dlc, f.dlc);
+  EXPECT_EQ(received.is_extended, f.is_extended);
+  EXPECT_EQ(received.is_rtr, f.is_rtr);
+  EXPECT_EQ(received.is_error, f.is_error);
+  EXPECT_EQ(received.is_fd, f.is_fd);
+  for (int i = 0; i < f.dlc; ++i)
+  {
+    EXPECT_EQ(received.data[i], f.data[i]);
+  }
 }
 
 TEST(SocketCANToTopicTest, checkInvalidFrameHandling)
@@ -118,7 +180,7 @@ TEST(SocketCANToTopicTest, checkInvalidFrameHandling)
   //   that should not be sent.
   // - verifies that sending one larger than 11 bits actually works.
 
-  // sending a message with a dlc > 8 is not possible as the DummyInterface
+  // sending a message with a dlc > 64 is not possible as the DummyInterface
   // causes a crash then.
 
   ros::NodeHandle nh(""), nh_param("~");
@@ -213,7 +275,10 @@ TEST(SocketCANToTopicTest, checkCorrectCanIdFilter)
   EXPECT_EQ(received.is_extended, f.is_extended);
   EXPECT_EQ(received.is_rtr, f.is_rtr);
   EXPECT_EQ(received.is_error, f.is_error);
-  EXPECT_EQ(received.data, f.data);
+  for (int i = 0; i < f.dlc; ++i)
+  {
+    EXPECT_EQ(received.data[i], f.data[i]);
+  }
 }
 
 TEST(SocketCANToTopicTest, checkInvalidCanIdFilter)

socketcan_interface/include/socketcan_interface/bcm.h

@@ -18,28 +18,33 @@
 
 namespace can {
 
+typedef struct {
+    struct bcm_msg_head msg_head;
+    struct canfd_frame frames[0];
+} bcm_fdmsg_head;
+
 class BCMsocket{
     int s_;
     struct Message {
         size_t size;
         uint8_t *data;
-        Message(size_t n)
-        : size(sizeof(bcm_msg_head) + sizeof(can_frame)*n), data(new uint8_t[size])
+        Message(size_t n, bool is_fd)
+        : size(sizeof(bcm_msg_head) + (is_fd?sizeof(canfd_frame):sizeof(can_frame))*n), data(new uint8_t[size])
         {
             assert(n<=256);
             std::memset(data, 0, size);
-            head().nframes = n;
+            head()->nframes = n;
         }
-        bcm_msg_head& head() {
-            return *(bcm_msg_head*)data;
+        bcm_msg_head* head() {
+            return (bcm_msg_head*)data;
         }
         template<typename T> void setIVal2(T period){
             long long usec = boost::chrono::duration_cast<boost::chrono::microseconds>(period).count();
-            head().ival2.tv_sec = usec / 1000000;
-            head().ival2.tv_usec = usec % 1000000;
+            head()->ival2.tv_sec = usec / 1000000;
+            head()->ival2.tv_usec = usec % 1000000;
         }
         void setHeader(Header header){
-            head().can_id = header.id | (header.is_extended?CAN_EFF_FLAG:0);
+            head()->can_id = header.id | (header.is_extended?CAN_EFF_FLAG:0);
         }
         bool write(int s){
             return ::write(s, data, size) > 0;
@@ -79,27 +84,44 @@ class BCMsocket{
         return true;
     }
     template<typename DurationType> bool startTX(DurationType period, Header header, size_t num, Frame *frames) {
-        Message msg(num);
+        Message msg(num, header.is_fd);
         msg.setHeader(header);
         msg.setIVal2(period);
 
-        bcm_msg_head &head = msg.head();
+        if (header.is_fd){
+            bcm_fdmsg_head* head = (bcm_fdmsg_head*)msg.head();
 
-        head.opcode = TX_SETUP;
-        head.flags |= SETTIMER | STARTTIMER;
+            head->msg_head.opcode = TX_SETUP;
+            head->msg_head.flags |= SETTIMER | STARTTIMER | CAN_FD_FRAME;
 
-        for(size_t i=0; i < num; ++i){ // msg nr
-            head.frames[i].can_dlc = frames[i].dlc;
-            head.frames[i].can_id = head.can_id;
-            for(size_t j = 0; j < head.frames[i].can_dlc; ++j){ // byte nr
-                head.frames[i].data[j] = frames[i].data[j];
+            for(size_t i=0; i < num; ++i){ // msg nr
+                head->frames[i].len = frames[i].dlc;
+                head->frames[i].can_id = head->msg_head.can_id;
+                head->frames[i].flags = header.flags;
+                for(size_t j = 0; j < head->frames[i].len; ++j){ // byte nr
+                    head->frames[i].data[j] = frames[i].data[j];
+                }
+            }
+        } else {
+            bcm_msg_head* head = msg.head();
+
+            head->opcode = TX_SETUP;
+            head->flags |= SETTIMER | STARTTIMER;
+
+            for(size_t i=0; i < num; ++i){ // msg nr
+                head->frames[i].can_dlc = frames[i].dlc;
+                head->frames[i].can_id = head->can_id;
+                for(size_t j = 0; j < head->frames[i].can_dlc; ++j){ // byte nr
+                    head->frames[i].data[j] = frames[i].data[j];
+                }
             }
         }
+
         return msg.write(s_);
     }
     bool stopTX(Header header){
-        Message msg(0);
-        msg.head().opcode = TX_DELETE;
+        Message msg(0, false);
+        msg.head()->opcode = TX_DELETE;
         msg.setHeader(header);
         return msg.write(s_);
     }

socketcan_interface/include/socketcan_interface/interface.h

@@ -24,6 +24,8 @@ struct Header{
     unsigned int is_error:1; ///< marks an error frame (only used internally)
     unsigned int is_rtr:1; ///< frame is a remote transfer request
     unsigned int is_extended:1; ///< frame uses 29 bit CAN identifier
+    bool is_fd; ///< frame is a CAN FD frame
+    unsigned char flags; ///< additional flags for CAN FD
     /** check if frame header is valid*/
     bool isValid() const{
         return id < (is_extended?(1<<29):(1<<11));
@@ -39,10 +41,10 @@ struct Header{
         */
 
     Header()
-    : id(0),is_error(0),is_rtr(0), is_extended(0) {}
+    : id(0),is_error(0),is_rtr(0), is_extended(0), is_fd(false), flags(0) {}
 
-    Header(unsigned int i, bool extended, bool rtr, bool error)
-    : id(i),is_error(error?1:0),is_rtr(rtr?1:0), is_extended(extended?1:0) {}
+    Header(unsigned int i, bool extended, bool rtr, bool error, bool is_fd=false, unsigned char flags=0)
+    : id(i),is_error(error?1:0),is_rtr(rtr?1:0), is_extended(extended?1:0), is_fd(is_fd), flags(flags) {}
 };
 
 
@@ -61,12 +63,12 @@ struct ErrorHeader : public Header{
 /** representation of a CAN frame */
 struct Frame: public Header{
     using value_type = unsigned char;
-    std::array<value_type, 8> data; ///< array for 8 data bytes with bounds checking
+    std::array<value_type, 64> data; ///< array for 64 data bytes with bounds checking
     unsigned char dlc; ///< len of data
 
     /** check if frame header and length are valid*/
     bool isValid() const{
-        return (dlc <= 8)  &&  Header::isValid();
+        return (dlc <= data.size())  &&  Header::isValid();
     }
     /**
      * constructor with default parameters