From abd0190c2ba3a624272ed1f6a3a9c8fe8c4dd140 Mon Sep 17 00:00:00 2001 From: Romain Forlot Date: Wed, 26 Apr 2017 18:25:33 +0200 Subject: Create method to handle both CAN frames. Change-Id: Ie876614266fd2e7089754a8f917d25d316de4c98 Signed-off-by: Romain Forlot Fix: wrong call since renaming of function to more generic name Change-Id: I2adebb855bc815ba6ab14a1002b17d9c9fb293fd Signed-off-by: Romain Forlot --- CAN-binder/low-can-binding/can/can-bus-dev.cpp | 2 +- CAN-binder/low-can-binding/can/can-message.cpp | 115 +++++++++++++++++++++---- CAN-binder/low-can-binding/can/can-message.hpp | 7 +- 3 files changed, 103 insertions(+), 21 deletions(-) diff --git a/CAN-binder/low-can-binding/can/can-bus-dev.cpp b/CAN-binder/low-can-binding/can/can-bus-dev.cpp index 1837f49..4737d11 100644 --- a/CAN-binder/low-can-binding/can/can-bus-dev.cpp +++ b/CAN-binder/low-can-binding/can/can-bus-dev.cpp @@ -116,7 +116,7 @@ can_message_t can_bus_dev_t::read() DEBUG(binder_interface, "%s: Found id: %X, length: %X, data %02X%02X%02X%02X%02X%02X%02X%02X", __FUNCTION__, cfd.can_id, cfd.len, cfd.data[0], cfd.data[1], cfd.data[2], cfd.data[3], cfd.data[4], cfd.data[5], cfd.data[6], cfd.data[7]); - return can_message_t::convert_from_canfd_frame(cfd, nbytes); + return can_message_t::convert_from_frame(cfd, nbytes); } /// @brief Create a RX_SETUP receive job using the BCM socket. diff --git a/CAN-binder/low-can-binding/can/can-message.cpp b/CAN-binder/low-can-binding/can/can-message.cpp index f27dce3..3575348 100644 --- a/CAN-binder/low-can-binding/can/can-message.cpp +++ b/CAN-binder/low-can-binding/can/can-message.cpp @@ -143,7 +143,6 @@ void can_message_t::set_format(const can_message_format_t new_format) ERROR(binder_interface, "%s: Can set format, wrong format chosen", __FUNCTION__); } -/// /// @brief Take a canfd_frame struct to initialize class members /// /// This is the preferred way to initialize class members. @@ -153,7 +152,7 @@ void can_message_t::set_format(const can_message_format_t new_format) /// /// @return A can_message_t object fully initialized with canfd_frame values. /// -can_message_t can_message_t::convert_from_canfd_frame(const struct canfd_frame& frame, size_t nbytes) +can_message_t can_message_t::convert_from_frame(const struct canfd_frame& frame, size_t nbytes) { uint8_t maxdlen, length, flags = (uint8_t)NULL; uint32_t id; @@ -177,26 +176,19 @@ can_message_t can_message_t::convert_from_canfd_frame(const struct canfd_frame& } if (frame.can_id & CAN_ERR_FLAG) + { format = can_message_format_t::ERROR; + id = frame.can_id & (CAN_ERR_MASK|CAN_ERR_FLAG); + } else if (frame.can_id & CAN_EFF_FLAG) + { format = can_message_format_t::EXTENDED; + id = frame.can_id & CAN_EFF_MASK; + } else - format = can_message_format_t::STANDARD; - - switch(format) { - case can_message_format_t::STANDARD: - id = frame.can_id & CAN_SFF_MASK; - break; - case can_message_format_t::EXTENDED: - id = frame.can_id & CAN_EFF_MASK; - break; - case can_message_format_t::ERROR: - id = frame.can_id & (CAN_ERR_MASK|CAN_ERR_FLAG); - break; - default: - ERROR(binder_interface, "%s: Can set id, not a compatible format or format not set prior to set id.", __FUNCTION__); - break; + format = can_message_format_t::STANDARD; + id = frame.can_id & CAN_SFF_MASK; } /* Overwrite length_ if RTR flags is detected. @@ -240,6 +232,77 @@ can_message_t can_message_t::convert_from_canfd_frame(const struct canfd_frame& return can_message_t(maxdlen, id, length, format, rtr_flag, flags, data); } +can_message_t can_message_t::convert_from_frame(const struct can_frame& frame, size_t nbytes) +{ + uint8_t maxdlen, length, flags = (uint8_t)NULL; + uint32_t id; + can_message_format_t format; + bool rtr_flag; + std::vector data; + + if(nbytes <= CAN_MTU) + { + DEBUG(binder_interface, "%s: Got a legacy CAN frame", __FUNCTION__); + maxdlen = CAN_MAX_DLEN; + } + else + { + ERROR(binder_interface, "%s: unsupported CAN frame", __FUNCTION__); + } + + if (frame.can_id & CAN_ERR_FLAG) + { + format = can_message_format_t::ERROR; + id = frame.can_id & (CAN_ERR_MASK|CAN_ERR_FLAG); + } + else if (frame.can_id & CAN_EFF_FLAG) + { + format = can_message_format_t::EXTENDED; + id = frame.can_id & CAN_EFF_MASK; + } + else + { + format = can_message_format_t::STANDARD; + id = frame.can_id & CAN_SFF_MASK; + } + + /* Overwrite length_ if RTR flags is detected. + * standard CAN frames may have RTR enabled. There are no ERR frames with RTR */ + if (frame.can_id & CAN_RTR_FLAG) + { + rtr_flag = true; + if(frame.can_dlc && frame.can_dlc <= CAN_MAX_DLC) + { + if(rtr_flag) + length = frame.can_dlc& 0xF; + else + { + length = (frame.can_dlc > maxdlen) ? maxdlen : frame.can_dlc; + } + } + } + else + { + length = (frame.can_dlc > maxdlen) ? maxdlen : frame.can_dlc; + + if (data.capacity() < maxdlen) + data.reserve(maxdlen); + int i; + + data.clear(); + /* maxdlen_ is now set at CAN_MAX_DLEN or CANFD_MAX_DLEN, respectively 8 and 64 bytes*/ + for(i=0;i