diff options
Diffstat (limited to 'src/can-message.cpp')
| -rw-r--r-- | src/can-message.cpp | 116 |
1 files changed, 89 insertions, 27 deletions
diff --git a/src/can-message.cpp b/src/can-message.cpp index d40dbc43..77fce685 100644 --- a/src/can-message.cpp +++ b/src/can-message.cpp @@ -28,7 +28,7 @@ *********************************************************************************/ can_message_t::can_message_t() - : id_{0}, length_{0}, format_{CanMessageFormat::ERROR}, data_{0,0,0,0,0,0,0,0} + : id_{0}, rtr_flag_{false}, length_{0}, flags_{0}, format_{CanMessageFormat::ERROR} {} uint32_t can_message_t::get_id() const @@ -36,6 +36,11 @@ uint32_t can_message_t::get_id() const return id_; } +bool can_message_t::get_rtr_flag_() const +{ + return rtr_flag_; +} + int can_message_t::get_format() const { if (format_ != CanMessageFormat::STANDARD || format_ != CanMessageFormat::EXTENDED) @@ -43,6 +48,11 @@ int can_message_t::get_format() const return format_; } +uint8_t can_message_t::get_flags() const +{ + return flags_; +} + const uint8_t* can_message_t::get_data() const { return data_; @@ -52,6 +62,26 @@ uint8_t can_message_t::get_length() const return length_; } +void can_message_t::set_max_data_length(const struct canfd_frame& frame) +{ + maxdlen_ = 0; + + switch(sizeof(frame)) + { + case CANFD_MTU: + DEBUG(binder_interface, "convert_from_canfd_frame: Got an CAN FD frame with length %d and flags %d", frame.len, frame.flags); + maxdlen_ = CANFD_MAX_DLEN; + break; + case CAN_MTU: + DEBUG(binder_interface, "convert_from_canfd_frame: Got a legacy CAN frame with length %d", frame.len); + maxdlen_ = CAN_MAX_DLEN; + break; + default: + ERROR(binder_interface, "convert_from_canfd_frame: unsupported CAN frame"); + break; + } +} + bool can_message_t::is_correct_to_send() { if (id_ != 0 && length_ != 0 && format_ != CanMessageFormat::ERROR) @@ -64,8 +94,9 @@ bool can_message_t::is_correct_to_send() return false; } -void can_message_t::set_id(const uint32_t new_id) +void can_message_t::set_id_and_format(const uint32_t new_id) { + set_format(new_id); switch(format_) { case CanMessageFormat::STANDARD: @@ -74,6 +105,9 @@ void can_message_t::set_id(const uint32_t new_id) case CanMessageFormat::EXTENDED: id_ = new_id & CAN_EFF_MASK; break; + case CanMessageFormat::ERROR: + id_ = new_id & (CAN_ERR_MASK|CAN_ERR_FLAG); + break; default: ERROR(binder_interface, "ERROR: Can set id, not a compatible format or format not set prior to set id."); break; @@ -82,49 +116,77 @@ void can_message_t::set_id(const uint32_t new_id) void can_message_t::set_format(const CanMessageFormat new_format) { - if(new_format == CanMessageFormat::STANDARD || new_format == CanMessageFormat::EXTENDED) + if(new_format == CanMessageFormat::STANDARD || new_format == CanMessageFormat::EXTENDED || new_format == CanMessageFormat::ERROR) format_ = new_format; else ERROR(binder_interface, "ERROR: Can set format, wrong format chosen"); } -void can_message_t::set_data(const uint8_t new_data) +void can_message_t::set_format(const uint32_t can_id) { - if ((sizeof(new_data) / sizeof(uint8_t) > CAN_MESSAGE_SIZE)) - ERROR(binder_interface, "Can set data, your data array is too big"); + if (can_id & CAN_ERR_FLAG) + format_ = CanMessageFormat::ERROR; + else if (can_id & CAN_EFF_FLAG) + format_ = CanMessageFormat::EXTENDED; else - { - ::memcpy(&data_, &new_data, sizeof(new_data)); - length_ = sizeof(new_data); - } + format_ = CanMessageFormat::STANDARD; } -void can_message_t::convert_from_canfd_frame(const canfd_frame& frame) +void can_message_t::set_flags(const uint8_t flags) { - length_ = (frame.len > CAN_MAX_DLEN) ? (uint8_t)CAN_MAX_DLEN : frame.len; - length_ = (frame.len > CANFD_MAX_DLEN) ? (uint8_t)CANFD_MAX_DLEN : frame.len; + flags_ = flags & 0xF; +} - if (frame.can_id & CAN_ERR_FLAG) +void can_message_t::set_length(const uint8_t new_length) +{ + if(rtr_flag_) + length_ = new_length & 0xF; + else { - id_ = frame.can_id & (CAN_ERR_MASK|CAN_ERR_FLAG); - format_ = CanMessageFormat::ERROR; + length_ = (new_length > maxdlen_) ? maxdlen_ : new_length; } - else if (frame.can_id & CAN_EFF_FLAG) +} + +void can_message_t::set_data(const __u8 new_data[], size_t dlen) +{ + if (sizeof(dlen)/sizeof(__u8) > maxdlen_) + ERROR(binder_interface, "Can set data, too big ! It is a CAN frame ?"); + else { - id_ = frame.can_id & CAN_EFF_MASK; - format_ = CanMessageFormat::EXTENDED; + int i; + /* Limiting to 8 bytes message for now on 64 bytes from fd frames*/ + for(i=0;i<CAN_MESSAGE_SIZE;i++) + { + data_[i] = new_data[i]; + } } - else +} + +void can_message_t::convert_from_canfd_frame(const struct canfd_frame& frame) +{ + set_max_data_length(frame); + set_length(frame.len); + set_id_and_format(frame.can_id); + + /* standard CAN frames may have RTR enabled. There are no ERR frames with RTR */ + if (frame.can_id & CAN_RTR_FLAG) { - id_ = frame.can_id & CAN_SFF_MASK; - format_ = CanMessageFormat::STANDARD; + rtr_flag_ = true; + if(frame.len && frame.len <= CAN_MAX_DLC) + set_length(frame.len); + return; } - DEBUG(binder_interface, ""); - if (sizeof(frame.data) <= sizeof(data_)) - ::memcpy(&data_, frame.data, length_); - else if (sizeof(frame.data) >= CAN_MAX_DLEN) - ERROR(binder_interface, "can_message_t: canfd_frame data too long to be stored into CanMessage object"); + /* Flags field only present for CAN FD frames*/ + if(maxdlen_ == CANFD_MAX_DLEN) + set_flags(frame.flags); + + size_t dlen = sizeof(frame.data); + memset(data_, 0, dlen); + set_data(frame.data, dlen); + + DEBUG(binder_interface, "convert_from_canfd_frame: Found id: %d, format: %d, length: %d, data %d%d%d%d%d%d%d%d", id_, format_, length_, + data_[0], data_[1], data_[2], data_[3], data_[4], data_[5], data_[6], data_[7]); } canfd_frame can_message_t::convert_to_canfd_frame() |