diff options
| -rw-r--r-- | CAN-binder/low-can-binding/can/can-bus-dev.cpp | 2 | ||||
| -rw-r--r-- | CAN-binder/low-can-binding/can/can-message.cpp | 115 | ||||
| -rw-r--r-- | 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 1837f491..4737d113 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 f27dce3f..3575348f 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<uint8_t> 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<maxdlen;i++) + { + data.push_back(frame.data[i]); + }; + +// DEBUG(binder_interface, "%s: Found id: %X, format: %X, length: %X, data %02X%02X%02X%02X%02X%02X%02X%02X", __FUNCTION__, +// id, (uint8_t)format, length, data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7]); + } + + return can_message_t(maxdlen, id, length, format, rtr_flag, flags, data); +} + /// /// @brief Take all initialized class's members and build an /// canfd_frame struct that can be use to send a CAN message over @@ -247,7 +310,7 @@ can_message_t can_message_t::convert_from_canfd_frame(const struct canfd_frame& /// /// @return canfd_frame struct built from class members. /// -canfd_frame can_message_t::convert_to_canfd_frame() +struct canfd_frame can_message_t::convert_to_canfd_frame() { canfd_frame frame; @@ -262,3 +325,19 @@ canfd_frame can_message_t::convert_to_canfd_frame() return frame; } + +struct can_frame can_message_t::convert_to_can_frame() +{ + can_frame frame; + + if(is_correct_to_send()) + { + frame.can_id = get_id(); + frame.can_dlc = get_length(); + ::memcpy(frame.data, get_data(), length_); + } + else + ERROR(binder_interface, "%s: can_message_t not correctly initialized to be sent", __FUNCTION__); + + return frame; +} diff --git a/CAN-binder/low-can-binding/can/can-message.hpp b/CAN-binder/low-can-binding/can/can-message.hpp index 5a6e6eb3..d38f7ba2 100644 --- a/CAN-binder/low-can-binding/can/can-message.hpp +++ b/CAN-binder/low-can-binding/can/can-message.hpp @@ -70,6 +70,9 @@ public: bool is_correct_to_send(); - static can_message_t convert_from_canfd_frame(const struct canfd_frame& frame, size_t nbytes); - canfd_frame convert_to_canfd_frame(); + static can_message_t convert_from_frame(const canfd_frame& frame, size_t nbytes); + static can_message_t convert_from_frame(const can_frame& frame, size_t nbytes); + + struct canfd_frame convert_to_canfd_frame(); + struct can_frame convert_to_can_frame(); }; |