X-Git-Url: https://gerrit.automotivelinux.org/gerrit/gitweb?a=blobdiff_plain;f=CAN-binder%2Flow-can-binding%2Fdiagnostic%2Fdiagnostic-manager.cpp;h=f9dc9607151d06832e49b883b80b0846d24f683f;hb=a1a554bd3dc19580c9b80dc6e4beb6bed4711e53;hp=4aeaeaf3fe7b3cfbd568cd2ef930e0d85875c143;hpb=76007916b4cd6103c2843cf2247ac913598ed292;p=apps%2Fagl-service-can-low-level.git

diff --git a/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp b/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp
index 4aeaeaf3..f9dc9607 100644
--- a/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp
+++ b/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp
@@ -82,16 +82,17 @@ void diagnostic_manager_t::reset()
 ///  listens on CAN ID range 7E8 - 7EF affected to the OBD2 communications.
 ///
 /// @return -1 or negative value on error, 0 if ok.
-int diagnostic_manager_t::create_rx_filter(uint32_t can_id)
+int diagnostic_manager_t::create_rx_filter(uint32_t can_id, float frequency)
 {
 	// Make sure that socket has been opened.
 	if(! socket_)
 		socket_.open(get_bus_device_name());
 
 	struct utils::simple_bcm_msg bcm_msg;
-	memset(&bcm_msg.msg_head, 0, sizeof(bcm_msg.msg_head));
+	memset(&bcm_msg, 0, sizeof(bcm_msg));
 
-	const struct timeval freq =  recurring_requests_.back()->get_timeout_clock().get_timeval_from_period();
+	const struct timeval freq =  (frequency == recurring_requests_.back()->get_frequency_clock().get_frequency() ) ?
+		recurring_requests_.back()->get_frequency_clock().get_timeval_from_period() : frequency_clock_t(frequency).get_timeval_from_period();
 
 	bcm_msg.msg_head.opcode  = RX_SETUP;
 	bcm_msg.msg_head.flags = SETTIMER|RX_FILTER_ID;
@@ -292,7 +293,6 @@ void diagnostic_manager_t::cleanup_active_requests(bool force)
 		if (entry != nullptr)
 			cleanup_request(entry, force);
 	 }
-
 }
 
 /// @brief Will return the active_diagnostic_request_t pointer for theDiagnosticRequest or nullptr if
@@ -359,7 +359,8 @@ active_diagnostic_request_t* diagnostic_manager_t::add_request(DiagnosticRequest
 			diagnostic_request_to_string(&entry->get_handle()->request, request_string,
 					sizeof(request_string));
 
-			find_and_erase(entry, non_recurring_requests_);
+			// Erase any existing request not already cleaned.
+			cleanup_request(entry, true);
 			DEBUG(binder_interface, "%s: Added one-time diagnostic request on bus %s: %s", __FUNCTION__,
 					bus_.c_str(), request_string);
 
@@ -457,8 +458,12 @@ active_diagnostic_request_t* diagnostic_manager_t::add_recurring_request(Diagnos
 			recurring_requests_.push_back(entry);
 
 			entry->set_handle(shims_, request);
-			if(create_rx_filter(OBD2_FUNCTIONAL_BROADCAST_ID) < 0)
-				{ recurring_requests_.pop_back(); }
+			if(create_rx_filter(OBD2_FUNCTIONAL_BROADCAST_ID, frequencyHz) < 0)
+			{
+				recurring_requests_.pop_back();
+				delete entry;
+				entry = nullptr;
+			}
 			else
 				{
 					start_diagnostic_request(&shims_, entry->get_handle());