From ca99abeec539d8618bb079f309c63dc8231d10eb Mon Sep 17 00:00:00 2001 From: Simon Tegelid Date: Thu, 19 Dec 2019 17:07:39 +0100 Subject: [PATCH] Can FD support Breaks compat with can_msgs/Frame due to changed array type --- can_msgs/msg/Frame.msg | 4 +- .../socketcan_bridge/socketcan_to_topic.h | 6 +- .../socketcan_bridge/topic_to_socketcan.h | 18 ++++- socketcan_bridge/test/test_conversion.cpp | 2 +- socketcan_bridge/test/to_socketcan_test.cpp | 65 +++++++++++++++- socketcan_bridge/test/to_topic_test.cpp | 71 ++++++++++++++++- .../include/socketcan_interface/bcm.h | 60 +++++++++----- .../include/socketcan_interface/interface.h | 12 +-- .../include/socketcan_interface/socketcan.h | 78 ++++++++++++++++--- socketcan_interface/src/candump.cpp | 2 + socketcan_interface/src/string.cpp | 26 ++++++- socketcan_interface/test/test_string.cpp | 24 +++++- 12 files changed, 318 insertions(+), 50 deletions(-) diff --git a/can_msgs/msg/Frame.msg b/can_msgs/msg/Frame.msg index 9a878be1c..6daa9dc5f 100644 --- a/can_msgs/msg/Frame.msg +++ b/can_msgs/msg/Frame.msg @@ -4,4 +4,6 @@ bool is_rtr bool is_extended bool is_error uint8 dlc -uint8[8] data \ No newline at end of file +uint8[] data + +uint8 DLC_FD_BRS_FLAG = 128 diff --git a/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h b/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h index 21ef242aa..c7bfc25bd 100644 --- a/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h +++ b/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]); } }; diff --git a/socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h b/socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h index 0474ee3a5..bb3159a64 100644 --- a/socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h +++ b/socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h @@ -32,6 +32,16 @@ #include #include +#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]; } diff --git a/socketcan_bridge/test/test_conversion.cpp b/socketcan_bridge/test/test_conversion.cpp index fcab66b37..cdbf6e794 100644 --- a/socketcan_bridge/test/test_conversion.cpp +++ b/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); diff --git a/socketcan_bridge/test/to_socketcan_test.cpp b/socketcan_bridge/test/to_socketcan_test.cpp index 791ef8230..2c04906d0 100644 --- a/socketcan_bridge/test/to_socketcan_test.cpp +++ b/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(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("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(); diff --git a/socketcan_bridge/test/to_topic_test.cpp b/socketcan_bridge/test/to_topic_test.cpp index 9a385f58c..92b2684f6 100644 --- a/socketcan_bridge/test/to_topic_test.cpp +++ b/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(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) diff --git a/socketcan_interface/include/socketcan_interface/bcm.h b/socketcan_interface/include/socketcan_interface/bcm.h index cbf456f9c..65556f6af 100644 --- a/socketcan_interface/include/socketcan_interface/bcm.h +++ b/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 void setIVal2(T period){ long long usec = boost::chrono::duration_cast(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 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_); } diff --git a/socketcan_interface/include/socketcan_interface/interface.h b/socketcan_interface/include/socketcan_interface/interface.h index 6ac79f1d0..71a0e2eb8 100644 --- a/socketcan_interface/include/socketcan_interface/interface.h +++ b/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 data; ///< array for 8 data bytes with bounds checking + std::array 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 diff --git a/socketcan_interface/include/socketcan_interface/socketcan.h b/socketcan_interface/include/socketcan_interface/socketcan.h index f73f689d4..b243f943b 100644 --- a/socketcan_interface/include/socketcan_interface/socketcan.h +++ b/socketcan_interface/include/socketcan_interface/socketcan.h @@ -18,12 +18,21 @@ #include #include +#ifdef CANFD_MAX_DLC + #define CAN_FRAME_TYPE canfd_frame + #define CAN_FRAME_TYPE_DLC_FIELD len +#else + #define CAN_FRAME_TYPE can_frame + #define CAN_FRAME_TYPE_DLC_FIELD can_dlc +#endif + namespace can { class SocketCANInterface : public AsioDriver { bool loopback_; int sc_; can_err_mask_t error_mask_, fatal_error_mask_; + bool use_canfd_; static can_err_mask_t parse_error_mask(SettingsConstSharedPtr settings, const std::string &entry, can_err_mask_t defaults) { can_err_mask_t mask = 0; @@ -44,13 +53,17 @@ class SocketCANInterface : public AsioDriver sep + 1) && (s[sep + 1] == '#')) { + // CAN FD frame. First nibble after ## is FD flags. + is_fd = true; + if (!hex2dec(fd_flags, s[sep + 2])) + return MsgHeader(0xfff); + sep += 2; + } + Header header = toheader(s.substr(0, sep)); + header.flags = fd_flags; + header.is_fd = is_fd; Frame frame(header); std::string buffer; if (header.isValid() && hex2buffer(buffer, s.substr(sep + 1), false)) { - if (buffer.size() > 8) + if (buffer.size() > frame.data.size()) return MsgHeader(0xfff); for (size_t i = 0; i < buffer.size(); ++i) { diff --git a/socketcan_interface/test/test_string.cpp b/socketcan_interface/test/test_string.cpp index 4e3a339b2..591a27442 100644 --- a/socketcan_interface/test/test_string.cpp +++ b/socketcan_interface/test/test_string.cpp @@ -5,16 +5,22 @@ #include -TEST(StringTest, stringconversion) +TEST(StringTest, stringConversion1) { const std::string s1("123#1234567812345678"); can::Frame f1 = can::toframe(s1); EXPECT_EQ(s1, can::tostring(f1, true)); +} +TEST(StringTest, stringConversion2) +{ const std::string s2("1337#1234567812345678"); can::Frame f2 = can::toframe(s2); EXPECT_FALSE(f2.isValid()); +} +TEST(StringTest, stringConversion3) +{ const std::string s3("80001337#1234567812345678"); const std::string s4("00001337#1234567812345678"); @@ -29,21 +35,37 @@ TEST(StringTest, stringconversion) EXPECT_TRUE(f4.isValid()); EXPECT_TRUE(f4.is_extended); EXPECT_EQ(s4, can::tostring(f4, true)); +} +TEST(StringTest, stringConversion4) +{ const std::string s5("20001337#1234567812345678"); can::Frame f5 = can::toframe(s5); EXPECT_EQ(f5.fullid(), 0xA0001337); EXPECT_TRUE(f5.isValid()); EXPECT_TRUE(f5.is_error); EXPECT_EQ(s5, can::tostring(f5, true)); +} +TEST(StringTest, stringConversion5) +{ const std::string s6("40001337#1234567812345678"); can::Frame f6 = can::toframe(s6); EXPECT_EQ(f6.fullid(), 0xC0001337); EXPECT_TRUE(f6.isValid()); EXPECT_TRUE(f6.is_rtr); EXPECT_EQ(s6, can::tostring(f6, true)); +} +TEST(StringTest, stringConversion6) +{ + const std::string s1fd("123##1123456781234567811223344"); + can::Frame f1fd = can::toframe(s1fd); + EXPECT_EQ(f1fd.id, 0x123); + EXPECT_EQ(f1fd.dlc, 12); + EXPECT_EQ(f1fd.flags, 1); + EXPECT_EQ(f1fd.is_fd, true); + EXPECT_EQ(s1fd, can::tostring(f1fd, true)); } // Run all the tests that were declared with TEST()