Fix: CAN message transmitted filled of zero.

[apps/agl-service-can-low-level.git] / CAN-binder / low-can-binding / diagnostic / diagnostic-manager.cpp

diff --git a/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp b/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp
index 8a55365..eb9a344 100644 (file)
--- a/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp
+++ b/CAN-binder/low-can-binding/diagnostic/diagnostic-manager.cpp
@@ -49,6 +49,7 @@ bool diagnostic_manager_t::initialize()
        bus_ = configuration_t::instance().get_diagnostic_bus();
 
        init_diagnostic_shims();
+       event_source_ = nullptr;
        reset();
 
        initialized_ = true;
@@ -108,7 +109,7 @@ int diagnostic_manager_t::add_rx_filter(uint32_t can_id)
        bcm_msg.msg_head.ival2.tv_usec = freq.tv_usec;
 
        // If it isn't an OBD2 CAN ID then just add a simple RX_SETUP job
-       if(can_id != OBD2_FUNCTIONAL_RESPONSE_START) 
+       if(can_id != OBD2_FUNCTIONAL_BROADCAST_ID) 
        {
                bcm_msg.msg_head.can_id  = can_id;
 
@@ -167,6 +168,7 @@ bool diagnostic_manager_t::shims_send(const uint32_t arbitration_id, const uint8
        bcm_msg.msg_head.ival2.tv_sec = freq.tv_sec;
        bcm_msg.msg_head.ival2.tv_usec = freq.tv_usec;
        bcm_msg.msg_head.nframes = 1;
+       cfd.can_dlc = size;
        ::memcpy(cfd.data, data, size);
 
        bcm_msg.frames = cfd;
@@ -232,23 +234,7 @@ void diagnostic_manager_t::find_and_erase(active_diagnostic_request_t* entry, st
 // @brief TODO: implement cancel_request if needed... Don't know.
 void diagnostic_manager_t::cancel_request(active_diagnostic_request_t* entry)
 {
-
-       /* TODO: implement acceptance filters.
-       if(entry.arbitration_id_ == OBD2_FUNCTIONAL_BROADCAST_ID) {
-               for(uint32_t filter = OBD2_FUNCTIONAL_RESPONSE_START;
-                               filter < OBD2_FUNCTIONAL_RESPONSE_START +
-                                       OBD2_FUNCTIONAL_RESPONSE_COUNT;
-                               filter++) {
-                       removeAcceptanceFilter(entry.bus_, filter,
-                                       CanMessageFormat::STANDARD, getCanBuses(),
-                                       getCanBusCount());
-               }
-       } else {
-               removeAcceptanceFilter(entry.bus_,
-                               entry.arbitration_id_ +
-                                       DIAGNOSTIC_RESPONSE_ARBITRATION_ID_OFFSET,
-                               CanMessageFormat::STANDARD, getCanBuses(), getCanBusCount());
-       }*/
+       entry->get_socket().close();
 }
 
 /// @brief Cleanup a specific request if it isn't running and get complete. As it is almost
@@ -456,15 +442,28 @@ active_diagnostic_request_t* diagnostic_manager_t::add_recurring_request(Diagnos
                {
                        // TODO: implement Acceptance Filter
                        //if(updateRequiredAcceptanceFilters(bus, request)) {
-                       active_diagnostic_request_t* entry = new active_diagnostic_request_t(bus_, request, name,
+                       entry = new active_diagnostic_request_t(bus_, request, name,
                                        wait_for_multiple_responses, decoder, callback, frequencyHz);
                        recurring_requests_.push_back(entry);
 
                        entry->set_handle(shims_, request);
                        if(add_rx_filter(OBD2_FUNCTIONAL_BROADCAST_ID) < 0)
-                       { recurring_requests_.pop_back(); }
+                               { recurring_requests_.pop_back(); }
                        else
-                       { start_diagnostic_request(&shims_, entry->get_handle()); }
+                               {
+                                       start_diagnostic_request(&shims_, entry->get_handle()); 
+                                       if(event_source_ == nullptr && sd_event_add_io(afb_daemon_get_event_loop(binder_interface->daemon), 
+                                               &event_source_,
+                                               socket_.socket(),
+                                               EPOLLIN,
+                                               read_diagnostic_message,
+                                               nullptr) < 0)
+                                       {
+                                               cleanup_request(entry, true);
+                                               WARNING(binder_interface, "%s: signal: %s isn't supported. Canceling operation.", __FUNCTION__, entry->get_name().c_str());
+                                               return entry;
+                                       }
+                               }
                }
                else
                {
@@ -473,7 +472,7 @@ active_diagnostic_request_t* diagnostic_manager_t::add_recurring_request(Diagnos
                }
        }
        else
-       { DEBUG(binder_interface, "%s: Can't add request, one already exists with same key", __FUNCTION__);}
+               { DEBUG(binder_interface, "%s: Can't add request, one already exists with same key", __FUNCTION__);}
        return entry;
 }