diff options
| -rw-r--r-- | src/can-bus.cpp | 27 | ||||
| -rw-r--r-- | src/can-bus.hpp | 2 | ||||
| -rw-r--r-- | src/can-message.cpp | 116 | ||||
| -rw-r--r-- | src/can-message.hpp | 49 |
4 files changed, 143 insertions, 51 deletions
diff --git a/src/can-bus.cpp b/src/can-bus.cpp index 672e95e5..989dfa20 100644 --- a/src/can-bus.cpp +++ b/src/can-bus.cpp @@ -279,7 +279,7 @@ void can_bus_t::push_new_vehicle_message(const openxc_VehicleMessage& v_msg) has_vehicle_message_ = true; } -std::map<std::string, std::shared_ptr<can_bus_dev_t>> can_bus_t::get_can_devices(); +std::map<std::string, std::shared_ptr<can_bus_dev_t>> can_bus_t::get_can_devices() { return can_devices_m_; } @@ -372,24 +372,13 @@ canfd_frame can_bus_dev_t::read() nbytes = ::read(can_socket_, &canfd_frame, CANFD_MTU); - switch(nbytes) - { - case CANFD_MTU: - DEBUG(binder_interface, "read_can: Got an CAN FD frame with length %d", canfd_frame.len); - //maxdlen = CANFD_MAX_DLEN; - break; - case CAN_MTU: - DEBUG(binder_interface, "read_can: Got a legacy CAN frame with length %d", canfd_frame.len); - //maxdlen = CAN_MAX_DLEN; - break; - default: - if (errno == ENETDOWN) - ERROR(binder_interface, "read_can: %s binder_interface down", device_name_); - ERROR(binder_interface, "read_can: Error reading CAN bus"); - ::memset(&canfd_frame, 0, sizeof(canfd_frame)); - is_running_ = false; - break; - } + /* if we did not fit into CAN sized messages then stop_reading. */ + if (nbytes != CANFD_MTU && nbytes != CAN_MTU) + if (errno == ENETDOWN) + ERROR(binder_interface, "read: %s CAN device down", device_name_); + ERROR(binder_interface, "read: Error reading CAN bus"); + ::memset(&canfd_frame, 0, sizeof(canfd_frame)); + stop_reading(); return canfd_frame; } diff --git a/src/can-bus.hpp b/src/can-bus.hpp index 8d40ff0d..991bf62c 100644 --- a/src/can-bus.hpp +++ b/src/can-bus.hpp @@ -189,7 +189,7 @@ class can_bus_t { * * @return map can_bus_dev_m_ map */ - std::map<std::string, std::shared_ptr<can_bus_dev_t>> get_can_bus_devices(); + std::map<std::string, std::shared_ptr<can_bus_dev_t>> get_can_devices(); }; 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() diff --git a/src/can-message.hpp b/src/can-message.hpp index 6e51d928..f24c1700 100644 --- a/src/can-message.hpp +++ b/src/can-message.hpp @@ -57,10 +57,14 @@ typedef struct CanMessage CanMessage; class can_message_t { private: uint32_t id_; /*!< uint32_t id - The ID of the message. */ + bool rtr_flag_; /*!< bool rtr_flag - Telling if the frame has RTR flag positionned. Then frame hasn't data field*/ uint8_t length_; /*!< uint8_t length - the length of the data array (max 8). */ + uint8_t flags_; /*!< unint8_t flags of a CAN FD frame. Needed if we catch FD frames.*/ CanMessageFormat format_; /*!< CanMessageFormat format - the format of the message's ID.*/ uint8_t data_[CAN_MESSAGE_SIZE]; /*!< uint8_t data - The message's data field with a size of 8 which is the standard about CAN bus messages.*/ + uint8_t maxdlen_; + public: /** * @brief Class constructor @@ -77,6 +81,13 @@ class can_message_t { uint32_t get_id() const; /** + * @brief Retrieve RTR flag member. + * + * @return bool rtr_flags_ class member + */ + bool get_rtr_flag_() const; + + /** * @brief Retrieve format_ member value. * * @return CanMessageFormat format_ class member @@ -84,6 +95,13 @@ class can_message_t { int get_format() const; /** + * @brief Retrieve format_ member value. + * + * @return CanMessageFormat format_ class member + */ + uint8_t get_flags() const; + + /** * @brief Retrieve data_ member value. * * @return uint8_t data_ pointer class member @@ -96,6 +114,8 @@ class can_message_t { * @return uint8_t length_ class member */ uint8_t get_length() const; + + void set_max_data_length(const struct canfd_frame& frame); /** * @brief Control whether the object is correctly initialized @@ -113,7 +133,7 @@ class can_message_t { * * @param uint32_t id_ class member */ - void set_id(const uint32_t new_id); + void set_id_and_format(const uint32_t new_id); /** * @brief Set format_ member value. @@ -123,9 +143,30 @@ class can_message_t { * * @param CanMessageFormat format_ class member */ - void set_format(const CanMessageFormat format); + void set_format(const CanMessageFormat new_format); /** + * @brief Set format_ member value. Deducing from the can_id + * of a canfd_frame. + * + * Preferred way to initialize these members by using + * convert_from_canfd_frame method. + * + * @param uint32_t can_id integer from a canfd_frame + */ + void set_format(const uint32_t can_id); + + /** + * @brief Set format_ member value. + * + * Preferred way to initialize these members by using + * convert_from_canfd_frame method. + * + * @param CanMessageFormat format_ class member + */ + void set_flags(const uint8_t flags); + + /** * @brief Set data_ member value. * * Preferred way to initialize these members by using @@ -133,7 +174,7 @@ class can_message_t { * * @param uint8_t data_ array with a max size of 8 elements. */ - void set_data(const uint8_t new_data); + void set_data(const __u8 new_data[], size_t dlen); /** * @brief Set length_ member value. @@ -152,7 +193,7 @@ class can_message_t { * * @param canfd_frame struct read from can bus device. */ - void convert_from_canfd_frame(const canfd_frame& frame); + void convert_from_canfd_frame(const struct canfd_frame& frame); /** * @brief Take all initialized class's members and build an |