Implement check of supported diagnostic PID.

[apps/agl-service-can-low-level.git] / src / diagnostic / diagnostic-manager.cpp

diff --git a/src/diagnostic/diagnostic-manager.cpp b/src/diagnostic/diagnostic-manager.cpp
index 31fbf0b..2c0a4d9 100644 (file)
--- a/src/diagnostic/diagnostic-manager.cpp
+++ b/src/diagnostic/diagnostic-manager.cpp
@@ -111,6 +111,7 @@ void diagnostic_manager_t::cleanup_request(active_diagnostic_request_t* entry, b
                        find_and_erase(entry, recurring_requests_);
                        if(force)
                                cancel_request(entry);
+                       DEBUG(binder_interface, "cleanup_request: Cancelling completed, recurring request: %s", request_string);
                }
                else
                {
@@ -314,7 +315,7 @@ int diagnostic_manager_t::send_request(sd_event_source *s, uint64_t usec, void *
        return -1;
 }
 
-openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response) const
+openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_diagnostic_request_t* adr, const DiagnosticResponse& response)
 {
        openxc_VehicleMessage message = build_VehicleMessage();
        float value = (float)diagnostic_payload_to_integer(&response);
@@ -338,6 +339,16 @@ openxc_VehicleMessage diagnostic_manager_t::relay_diagnostic_response(active_dia
                message = build_VehicleMessage(adr, response, value);
        }
 
+       // If not success but completed then the pid isn't supported
+       if(!response.success)
+       {
+               std::vector<diagnostic_message_t*> found_signals;
+               configuration_t::instance().find_diagnostic_messages( build_DynamicField(adr->get_name()), found_signals );
+               found_signals.front()->set_supported(false);
+               cleanup_request(adr, true);
+               NOTICE(binder_interface, "relay_diagnostic_response: PID not supported or ill formed. Please unsubscribe from it. Error code : %d", response.negative_response_code);
+       }
+
        if(adr->get_callback() != nullptr)
        {
                adr->get_callback()(adr, &response, value);