From e4a50a3e96aa39346ad7212cae984524276973dd Mon Sep 17 00:00:00 2001 From: Romain Forlot Date: Fri, 19 May 2017 12:15:07 +0200 Subject: [PATCH] Added timestamp to received CAN messages and store into frequency_clock_t. Change-Id: If209070298bd9df49297fdcbed554770e1bc0e4a Signed-off-by: Romain Forlot --- CAN-binder/low-can-binding/can/can-decoder.cpp | 1 + CAN-binder/low-can-binding/can/can-message.cpp | 26 ++++++++++++++++------ CAN-binder/low-can-binding/can/can-message.hpp | 9 +++++--- CAN-binder/low-can-binding/can/can-signals.cpp | 5 +++++ CAN-binder/low-can-binding/can/can-signals.hpp | 1 + .../diagnostic/diagnostic-manager.cpp | 3 ++- CAN-binder/low-can-binding/utils/socketcan-bcm.cpp | 7 +++++- CAN-binder/low-can-binding/utils/socketcan-raw.cpp | 5 ++++- CAN-binder/low-can-binding/utils/timer.cpp | 10 +++++++-- CAN-binder/low-can-binding/utils/timer.hpp | 7 +++--- 10 files changed, 56 insertions(+), 18 deletions(-) diff --git a/CAN-binder/low-can-binding/can/can-decoder.cpp b/CAN-binder/low-can-binding/can/can-decoder.cpp index b0d7bb8a..aa1f57ed 100644 --- a/CAN-binder/low-can-binding/can/can-decoder.cpp +++ b/CAN-binder/low-can-binding/can/can-decoder.cpp @@ -172,6 +172,7 @@ openxc_DynamicField decoder_t::translateSignal(can_signal_t& signal, const can_m *send = false; } signal.set_last_value(value); + signal.set_timestamp(message.get_timestamp()); signal.get_message()->set_last_value(message); return decoded_value; } diff --git a/CAN-binder/low-can-binding/can/can-message.cpp b/CAN-binder/low-can-binding/can/can-message.cpp index 66a4f4fe..772734d7 100644 --- a/CAN-binder/low-can-binding/can/can-message.cpp +++ b/CAN-binder/low-can-binding/can/can-message.cpp @@ -27,7 +27,7 @@ /// Constructor about can_message_t class. /// can_message_t::can_message_t() - : maxdlen_{0}, id_{0}, length_{0}, format_{can_message_format_t::INVALID}, rtr_flag_{false}, flags_{0} + : maxdlen_{0}, id_{0}, length_{0}, format_{can_message_format_t::INVALID}, rtr_flag_{false}, flags_{0}, timestamp_{0} {} can_message_t::can_message_t(uint8_t maxdlen, @@ -36,14 +36,16 @@ can_message_t::can_message_t(uint8_t maxdlen, can_message_format_t format, bool rtr_flag, uint8_t flags, - std::vector data) + std::vector data, + uint64_t timestamp) : maxdlen_{maxdlen}, id_{id}, length_{length}, format_{format}, rtr_flag_{rtr_flag}, flags_{flags}, - data_{data} + data_{data}, + timestamp_{timestamp} {} /// @@ -119,6 +121,16 @@ uint8_t can_message_t::get_length() const return length_; } +uint64_t can_message_t::get_timestamp() const +{ + return timestamp_; +} + +void can_message_t::set_timestamp(uint64_t timestamp) +{ + timestamp_ = timestamp; +} + /// /// @brief Control whether the object is correctly initialized /// to be sent over the CAN bus @@ -162,7 +174,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_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, uint64_t timestamp) { uint8_t maxdlen, length, flags = (uint8_t)NULL; uint32_t id; @@ -239,10 +251,10 @@ can_message_t can_message_t::convert_from_frame(const struct canfd_frame& frame, 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); + return can_message_t(maxdlen, id, length, format, rtr_flag, flags, data, timestamp); } -can_message_t can_message_t::convert_from_frame(const struct can_frame& frame, size_t nbytes) +can_message_t can_message_t::convert_from_frame(const struct can_frame& frame, size_t nbytes, uint64_t timestamp) { uint8_t maxdlen, length, flags = (uint8_t)NULL; uint32_t id; @@ -310,7 +322,7 @@ can_message_t can_message_t::convert_from_frame(const struct can_frame& frame, s // 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); + return can_message_t(maxdlen, id, length, format, rtr_flag, flags, data, timestamp); } /// diff --git a/CAN-binder/low-can-binding/can/can-message.hpp b/CAN-binder/low-can-binding/can/can-message.hpp index 3fde5f1a..910b3e9f 100644 --- a/CAN-binder/low-can-binding/can/can-message.hpp +++ b/CAN-binder/low-can-binding/can/can-message.hpp @@ -51,10 +51,11 @@ private: bool rtr_flag_; /*!< rtr_flag_ - Telling if the frame has RTR flag positionned. Then frame hasn't data field*/ uint8_t flags_; /*!< flags_ - flags of a CAN FD frame. Needed if we catch FD frames.*/ std::vector data_; /*!< data_ - The message's data field with a size of 8 which is the standard about CAN bus messages.*/ + uint64_t timestamp_; /*!< timestamp_ - timestamp of the received message*/ public: can_message_t(); - can_message_t(uint8_t maxdlen, uint32_t id, uint8_t length, can_message_format_t format, bool rtr_flag_, uint8_t flags, std::vector data); + can_message_t(uint8_t maxdlen, uint32_t id, uint8_t length, can_message_format_t format, bool rtr_flag_, uint8_t flags, std::vector data, uint64_t timestamp); uint32_t get_id() const; bool get_rtr_flag_() const; @@ -63,13 +64,15 @@ public: const uint8_t* get_data() const; const std::vector get_data_vector() const; uint8_t get_length() const; + uint64_t get_timestamp() const; + void set_timestamp(uint64_t timestamp); void set_format(const can_message_format_t new_format); bool is_correct_to_send(); - 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); + static can_message_t convert_from_frame(const canfd_frame& frame, size_t nbytes, uint64_t timestamp); + static can_message_t convert_from_frame(const can_frame& frame, size_t nbytes, uint64_t timestamp); struct canfd_frame convert_to_canfd_frame(); struct can_frame convert_to_can_frame(); diff --git a/CAN-binder/low-can-binding/can/can-signals.cpp b/CAN-binder/low-can-binding/can/can-signals.cpp index 024855c0..9a876136 100644 --- a/CAN-binder/low-can-binding/can/can-signals.cpp +++ b/CAN-binder/low-can-binding/can/can-signals.cpp @@ -215,6 +215,11 @@ void can_signal_t::set_last_value(float val) last_value_ = val; } +void can_signal_t::set_timestamp(uint64_t timestamp) +{ + frequency_.tick(timestamp); +} + /// @brief Create a RX_SETUP receive job used by the BCM socket. /// /// @return 0 if ok else -1 diff --git a/CAN-binder/low-can-binding/can/can-signals.hpp b/CAN-binder/low-can-binding/can/can-signals.hpp index e4077c2b..cc4b6da3 100644 --- a/CAN-binder/low-can-binding/can/can-signals.hpp +++ b/CAN-binder/low-can-binding/can/can-signals.hpp @@ -154,6 +154,7 @@ public: void set_prefix(std::string val); void set_received(bool r); void set_last_value(float val); + void set_timestamp(uint64_t timestamp); int create_rx_filter(); }; diff --git a/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp b/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp index 4b254262..1a382af9 100644 --- a/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp +++ b/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp @@ -534,7 +534,8 @@ openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_handle(active_diagn else if(!response.completed && response.multi_frame) { // Reset the timeout clock while completing the multi-frame receive - entry->get_timeout_clock().tick(); + entry->get_timeout_clock().tick( + entry->get_timeout_clock().get_time_function()()); } return build_VehicleMessage(); diff --git a/CAN-binder/low-can-binding/utils/socketcan-bcm.cpp b/CAN-binder/low-can-binding/utils/socketcan-bcm.cpp index 7323565c..38f48522 100644 --- a/CAN-binder/low-can-binding/utils/socketcan-bcm.cpp +++ b/CAN-binder/low-can-binding/utils/socketcan-bcm.cpp @@ -84,7 +84,12 @@ namespace utils DEBUG(binder_interface, "read: Found on bus %s:\n id: %X, length: %X, data %02X%02X%02X%02X%02X%02X%02X%02X", ifr.ifr_name, msg.msg_head.can_id, msg.frames.can_dlc, msg.frames.data[0], msg.frames.data[1], msg.frames.data[2], msg.frames.data[3], msg.frames.data[4], msg.frames.data[5], msg.frames.data[6], msg.frames.data[7]); - cm = ::can_message_t::convert_from_frame(msg.frames , nbytes-sizeof(struct bcm_msg_head)); + struct timeval tv; + ioctl(s.socket(), SIOCGSTAMP, &tv); + uint64_t timestamp = 1000000 * tv.tv_sec + tv.tv_usec; + cm = ::can_message_t::convert_from_frame(msg.frames , + nbytes-sizeof(struct bcm_msg_head), + timestamp); return s; } diff --git a/CAN-binder/low-can-binding/utils/socketcan-raw.cpp b/CAN-binder/low-can-binding/utils/socketcan-raw.cpp index d8cd906f..a35a99d4 100644 --- a/CAN-binder/low-can-binding/utils/socketcan-raw.cpp +++ b/CAN-binder/low-can-binding/utils/socketcan-raw.cpp @@ -78,7 +78,10 @@ namespace utils DEBUG(binder_interface, "read: Found on bus %s:\n id: %X, length: %X, data %02X%02X%02X%02X%02X%02X%02X%02X", ifr.ifr_name, frame.can_id, frame.len, frame.data[0], frame.data[1], frame.data[2], frame.data[3], frame.data[4], frame.data[5], frame.data[6], frame.data[7]); - cm = ::can_message_t::convert_from_frame(frame , nbytes); + struct timeval tv; + ioctl(s.socket(), SIOCGSTAMP, &tv); + uint64_t timestamp = 1000000 * tv.tv_sec + tv.tv_usec; + cm = ::can_message_t::convert_from_frame(frame , nbytes, timestamp); return s; } diff --git a/CAN-binder/low-can-binding/utils/timer.cpp b/CAN-binder/low-can-binding/utils/timer.cpp index 3d90a007..9c22bd0d 100644 --- a/CAN-binder/low-can-binding/utils/timer.cpp +++ b/CAN-binder/low-can-binding/utils/timer.cpp @@ -62,6 +62,12 @@ frequency_clock_t::frequency_clock_t(float frequency) frequency_ = 1; } +frequency_clock_t::frequency_clock_t(float frequency, uint64_t last_tick, time_function_t time_function) + : unit_{1000000}, frequency_{frequency}, last_tick_{0}, time_function_{nullptr} +{ + if(frequency_ <= 0) + frequency_ = 1; +} /// @brief Return the period in ms given the frequency in hertz. /// @param[in] frequency - Frequency to convert, in hertz float frequency_clock_t::frequency_to_period() const @@ -109,7 +115,7 @@ float frequency_clock_t::get_frequency() const /// @brief Force the clock to tick, regardless of it its time has actually /// elapsed. -void frequency_clock_t::tick() +void frequency_clock_t::tick(uint64_t timestamp) { - last_tick_ = get_time_function()(); + last_tick_ = timestamp; } \ No newline at end of file diff --git a/CAN-binder/low-can-binding/utils/timer.hpp b/CAN-binder/low-can-binding/utils/timer.hpp index 8ea74768..432d513d 100644 --- a/CAN-binder/low-can-binding/utils/timer.hpp +++ b/CAN-binder/low-can-binding/utils/timer.hpp @@ -18,6 +18,7 @@ #pragma once #include +#include /// @brief return epoch in milliseconds /// @@ -36,13 +37,13 @@ class frequency_clock_t private: float unit_; ///< unit_ - multiplicator to make operation to be in the right unit (milli, micro, nano, etc) float frequency_; ///< the clock frequency in Hz. - unsigned long last_tick_; ///< the last time (in milliseconds since startup) that the clock ticked. + uint64_t last_tick_; ///< the last time (in microseconds since startup) that the clock ticked. time_function_t time_function_; ///< a function returning current time public: frequency_clock_t(); frequency_clock_t(float frequency); - frequency_clock_t(float frequency, unsigned long last_tick, time_function_t time_function); + frequency_clock_t(float frequency, uint64_t last_tick, time_function_t time_function); float get_frequency() const; const struct timeval get_timeval_from_period() const; @@ -52,5 +53,5 @@ public: time_function_t get_time_function(); bool elapsed(bool stagger); - void tick(); + void tick(uint64_t timestamp); }; \ No newline at end of file -- 2.16.6