4 files changed, 100 insertions, 12 deletions

diff --git a/low-can-binding/diagnostic/diagnostic-manager.cpp b/low-can-binding/diagnostic/diagnostic-manager.cpp
index 448133ef..86ff25b5 100644
--- a/low-can-binding/diagnostic/diagnostic-manager.cpp
+++ b/low-can-binding/diagnostic/diagnostic-manager.cpp
@@ -396,10 +396,14 @@ active_diagnostic_request_t* diagnostic_manager_t::add_recurring_request(Diagnos
 /// @param[in] response - The response to decode from which the Vehicle message will be built and returned
 ///
 /// @return A filled openxc_VehicleMessage or a zeroed struct if there is an error.
-openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response)
+openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response, const uint64_t timestamp)
 {
 	openxc_VehicleMessage message = build_VehicleMessage();
 	float value = (float)diagnostic_payload_to_integer(&response);
+
+	struct utils::signals_found found_signals;
+	found_signals = utils::signals_manager_t::instance().find_signals(build_DynamicField((double) adr->get_pid()));
+
 	if(adr->get_decoder() != nullptr)
 	{
 		value = adr->get_decoder()(&response, value);
@@ -425,8 +429,6 @@ openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_dia
 	// If not success but completed then the pid isn't supported
 	if(!response.success)
 	{
-		struct utils::signals_found found_signals;
-		found_signals = utils::signals_manager_t::instance().find_signals(build_DynamicField(adr->get_name()));
 		found_signals.diagnostic_messages.front()->set_supported(false);
 		cleanup_request(adr, true);
 		AFB_NOTICE("PID not supported or ill formed. Please unsubscribe from it. Error code : %d", response.negative_response_code);
@@ -440,6 +442,20 @@ openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_dia
 
 	// Reset the completed flag handle to make sure that it will be reprocessed the next time.
 	adr->get_handle()->success = false;
+
+	// Save value and timestamp of diagnostic message
+	if(!found_signals.diagnostic_messages.empty())
+	{
+		// Then, for each diag_message found
+		for(const auto& diag_mess: found_signals.diagnostic_messages)
+		{
+			// Save value and timestamp for this message
+			diag_mess->set_received(true);
+			diag_mess->set_last_value(value);
+			diag_mess->set_timestamp(timestamp);
+		}
+	}
+
 	return message;
 }
 
@@ -457,7 +473,7 @@ openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_handle(active_diagn
 	if(response.completed && entry->get_handle()->completed)
 	{
 		if(entry->get_handle()->success)
-			return relay_diagnostic_response(entry, response);
+			return relay_diagnostic_response(entry, response, cm.get_timestamp());
 	}
 	else if(!response.completed && response.multi_frame)
 	{
diff --git a/low-can-binding/diagnostic/diagnostic-manager.hpp b/low-can-binding/diagnostic/diagnostic-manager.hpp
index ddbc97a2..1124ff95 100644
--- a/low-can-binding/diagnostic/diagnostic-manager.hpp
+++ b/low-can-binding/diagnostic/diagnostic-manager.hpp
@@ -83,7 +83,7 @@ public:
 		const DiagnosticResponseCallback callback, float frequencyHz);
 
 	// Decoding part
-	openxc_VehicleMessage relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response);
+	openxc_VehicleMessage relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response, const uint64_t timestamp);
 	openxc_VehicleMessage relay_diagnostic_handle(active_diagnostic_request_t* entry, const can_message_t& cm);
 	openxc_VehicleMessage find_and_decode_adr(const can_message_t& cm);
 	bool is_diagnostic_response(const can_message_t& cm);
diff --git a/low-can-binding/diagnostic/diagnostic-message.cpp b/low-can-binding/diagnostic/diagnostic-message.cpp
index 69ca4e34..701248dc 100644
--- a/low-can-binding/diagnostic/diagnostic-message.cpp
+++ b/low-can-binding/diagnostic/diagnostic-message.cpp
@@ -34,11 +34,30 @@ const char *UNIT_NAMES[10] = {
 	"NM"
 };
 
-diagnostic_message_t::diagnostic_message_t(uint8_t pid, const std::string& generic_name, const int min,
-	const int max, enum UNIT unit, float frequency,	DiagnosticResponseDecoder decoder,
-	DiagnosticResponseCallback callback, bool supported)
-	: parent_{nullptr}, pid_{pid}, generic_name_{generic_name}, min_{min}, max_{max}, unit_{unit},
-	frequency_{frequency}, decoder_{decoder}, callback_{callback}, supported_{supported}
+diagnostic_message_t::diagnostic_message_t(
+		uint8_t pid,
+		const std::string& generic_name,
+		const int min,
+		const int max,
+		enum UNIT unit,
+		float frequency,
+		DiagnosticResponseDecoder decoder,
+		DiagnosticResponseCallback callback,
+		bool supported,
+		bool received)
+		: parent_{nullptr},
+		pid_{pid},
+		generic_name_{generic_name},
+		min_{min},
+		max_{max},
+		unit_{unit},
+		frequency_{frequency},
+		decoder_{decoder},
+		callback_{callback},
+		supported_{supported},
+		last_timestamp_{0},
+		received_{received},
+		last_value_{.0f}
 {}
 
 uint32_t diagnostic_message_t::get_pid()
@@ -75,6 +94,21 @@ bool diagnostic_message_t::get_supported() const
 	return supported_;
 }
 
+bool diagnostic_message_t::get_received() const
+{
+	return received_;
+}
+
+float diagnostic_message_t::get_last_value() const
+{
+	return last_value_;
+}
+
+std::pair<float, uint64_t> diagnostic_message_t::get_last_value_with_timestamp() const
+{
+	return std::make_pair(last_value_, last_timestamp_);
+}
+
 void diagnostic_message_t::set_supported(bool value)
 {
 	supported_ = value;
@@ -85,6 +119,21 @@ void diagnostic_message_t::set_parent(can_message_set_t* parent)
 	parent_ = parent;
 }
 
+void diagnostic_message_t::set_received(bool r)
+{
+	received_ = r;
+}
+
+void diagnostic_message_t::set_last_value(float val)
+{
+	last_value_ = val;
+}
+
+void diagnostic_message_t::set_timestamp(uint64_t timestamp)
+{
+	last_timestamp_ = timestamp;
+}
+
 ///
 /// @brief Build a DiagnosticRequest struct to be passed
 ///  to diagnostic manager instance.
diff --git a/low-can-binding/diagnostic/diagnostic-message.hpp b/low-can-binding/diagnostic/diagnostic-message.hpp
index 873c7ff2..c2f31e35 100644
--- a/low-can-binding/diagnostic/diagnostic-message.hpp
+++ b/low-can-binding/diagnostic/diagnostic-message.hpp
@@ -60,10 +60,25 @@ class diagnostic_message_t
 
 		bool supported_; /*!< supported_ - boolean indicating whether this pid is supported by the vehicle or not.*/
 
+		uint64_t last_timestamp_; /*!< last_timestamp_ - the last time (in microseconds since epoch)
+								* that the message has been received. */
+
+		bool received_; /*!< received_ - True if this signal has ever been received. */
+		float last_value_; /*!< last_value_ - The last received value of the diagnostic message.
+								* If 'received_' is false, this value is undefined. */
+
 	public:
 		const char* generic_name = generic_name_.c_str();
-		diagnostic_message_t(uint8_t pid, const std::string& generic_name, const int min, const int max, enum UNIT unit, float frequency,
-											DiagnosticResponseDecoder decoder, DiagnosticResponseCallback callback, bool supported);
+		diagnostic_message_t(uint8_t pid,
+				     const std::string& generic_name,
+				     const int min,
+				     const int max,
+				     enum UNIT unit,
+				     float frequency,
+				     DiagnosticResponseDecoder decoder,
+				     DiagnosticResponseCallback callback,
+				     bool supported,
+				     bool received);
 
 		uint32_t get_pid();
 		const std::string get_generic_name() const;
@@ -73,6 +88,14 @@ class diagnostic_message_t
 		DiagnosticResponseCallback get_callback() const;
 		bool get_supported() const;
 
+		bool get_received() const;
+		float get_last_value() const;
+		std::pair<float, uint64_t> get_last_value_with_timestamp() const;
+
+		void set_received(bool r);
+		void set_last_value(float val);
+		void set_timestamp(uint64_t timestamp);
+
 		void set_supported(bool value);
 		void set_parent(can_message_set_t* parent);
 		const DiagnosticRequest build_diagnostic_request() const;