From 7617950b0ab386949b43c6f221b02826e18548d6 Mon Sep 17 00:00:00 2001 From: Romain Forlot Date: Sun, 12 Mar 2017 19:48:21 +0100 Subject: Implemente way to send diagnostic request when subscribed. When subscribed, the signal is added to recurring request list of diagnostic manager and an event is added to the systemd event loop with timer set using frequency parameter from the requested signal. Change-Id: I4d604c498047d7744c090b7f03fce0f2b427fd01 Signed-off-by: Romain Forlot --- src/configuration.cpp | 13 ++++-- src/diagnostic/active-diagnostic-request.cpp | 22 +++++++++++ src/diagnostic/active-diagnostic-request.hpp | 5 +++ src/diagnostic/diagnostic-manager.cpp | 54 +++++++++++++++++++++++++ src/diagnostic/diagnostic-manager.hpp | 5 +++ src/diagnostic/diagnostic-message.cpp | 5 +++ src/diagnostic/diagnostic-message.hpp | 3 +- src/low-can-binding.cpp | 59 ++++++++++++++++++++++++++-- src/utils/timer.cpp | 7 ++++ src/utils/timer.hpp | 2 + 10 files changed, 167 insertions(+), 8 deletions(-) diff --git a/src/configuration.cpp b/src/configuration.cpp index e56dc1c..85840f8 100644 --- a/src/configuration.cpp +++ b/src/configuration.cpp @@ -89,14 +89,19 @@ std::vector& configuration_t::get_can_signals() return can_signals_[active_message_set_]; } -const std::vector& configuration_t::get_can_message_definition() +std::vector& configuration_t::get_obd2_signals() { - return can_message_definition_[active_message_set_]; + return obd2_signals_[active_message_set_]; } -std::vector& configuration_t::get_obd2_signals() +const std::vector& configuration_t::get_signals_prefix() const { - return obd2_signals_[active_message_set_]; + return signals_prefix_; +} + +const std::vector& configuration_t::get_can_message_definition() +{ + return can_message_definition_[active_message_set_]; } uint32_t configuration_t::get_signal_id(obd2_signal_t& sig) const diff --git a/src/diagnostic/active-diagnostic-request.cpp b/src/diagnostic/active-diagnostic-request.cpp index 505d816..7a43a0d 100644 --- a/src/diagnostic/active-diagnostic-request.cpp +++ b/src/diagnostic/active-diagnostic-request.cpp @@ -57,6 +57,11 @@ active_diagnostic_request_t::active_diagnostic_request_t(std::shared_ptr active_diagnostic_request_t::get_can_bus_dev() { return bus_; @@ -77,6 +82,16 @@ bool active_diagnostic_request_t::get_in_flight() const return in_flight_; } +frequency_clock_t& active_diagnostic_request_t::get_frequency_clock() +{ + return frequency_clock_; +} + +frequency_clock_t& active_diagnostic_request_t::get_timeout_clock() +{ + return timeout_clock_; +} + void active_diagnostic_request_t::set_handle(DiagnosticShims& shims, DiagnosticRequest* request) { handle_ = new DiagnosticRequestHandle(generate_diagnostic_request(&shims, request, nullptr)); @@ -87,6 +102,13 @@ void active_diagnostic_request_t::set_in_flight(bool val) in_flight_ = val; } +bool active_diagnostic_request_t::should_send() +{ + return !get_in_flight() && ( + (!get_recurring() && !request_completed()) || + (get_recurring() && get_frequency_clock().elapsed(true))); +} + bool active_diagnostic_request_t::timed_out() { // don't use staggered start with the timeout clock diff --git a/src/diagnostic/active-diagnostic-request.hpp b/src/diagnostic/active-diagnostic-request.hpp index 88d4008..fb87c83 100644 --- a/src/diagnostic/active-diagnostic-request.hpp +++ b/src/diagnostic/active-diagnostic-request.hpp @@ -88,14 +88,19 @@ public: const DiagnosticResponseDecoder decoder, const DiagnosticResponseCallback callback, float frequencyHz); + uint32_t get_id() const; std::shared_ptr get_can_bus_dev(); DiagnosticRequestHandle* get_handle(); bool get_recurring() const; bool get_in_flight() const; + frequency_clock_t& get_frequency_clock(); + frequency_clock_t& get_timeout_clock(); void set_handle(DiagnosticShims& shims, DiagnosticRequest* request); void set_in_flight(bool val); + bool should_send(); + bool timed_out(); bool response_received() const; bool request_completed(); diff --git a/src/diagnostic/diagnostic-manager.cpp b/src/diagnostic/diagnostic-manager.cpp index 5103bf1..6e8a68c 100644 --- a/src/diagnostic/diagnostic-manager.cpp +++ b/src/diagnostic/diagnostic-manager.cpp @@ -261,6 +261,60 @@ bool diagnostic_manager_t::add_recurring_request(DiagnosticRequest* request, con return added; } +bool diagnostic_manager_t::conflicting(active_diagnostic_request_t* request, active_diagnostic_request_t* candidate) const +{ + return (candidate->get_in_flight() && candidate != request && + candidate->get_can_bus_dev() == request->get_can_bus_dev() && + candidate->get_id() == request->get_id()); +} + + +/// @brief Returns true if there are no other active requests to the same arbitration ID. +bool diagnostic_manager_t::clear_to_send(active_diagnostic_request_t* request) const +{ + for ( auto entry : non_recurring_requests_) + { + if(conflicting(request, entry)) + return false; + } + + for ( auto entry : recurring_requests_) + { + if(conflicting(request, entry)) + return false; + } + return true; +} + +int diagnostic_manager_t::send_request(sd_event_source *s, uint64_t usec, void *userdata) +{ + diagnostic_manager_t& dm = configuration_t::instance().get_diagnostic_manager(); + DiagnosticRequest* request = (DiagnosticRequest*)userdata; + active_diagnostic_request_t* adr = dm.lookup_recurring_request(request); + + if(adr != nullptr && adr->get_can_bus_dev() == dm.get_can_bus_dev() && adr->should_send() && + dm.clear_to_send(adr)) + { + DEBUG(binder_interface, "Got active_diagnostic_request from recurring_requests_ queue."); + adr->get_frequency_clock().tick(); + start_diagnostic_request(&dm.get_shims(), adr->get_handle()); + if(adr->get_handle()->completed && !adr->get_handle()->success) + { + DEBUG(binder_interface, "Fatal error sending diagnostic request"); + return 0; + } + adr->get_timeout_clock().tick(); + adr->set_in_flight(true); + return 1; + } + return -1; +} + +DiagnosticShims& diagnostic_manager_t::get_shims() +{ + return shims_; +} + bool diagnostic_manager_t::shims_send(const uint32_t arbitration_id, const uint8_t* data, const uint8_t size) { std::shared_ptr can_bus_dev = configuration_t::instance().get_diagnostic_manager().get_can_bus_dev(); diff --git a/src/diagnostic/diagnostic-manager.hpp b/src/diagnostic/diagnostic-manager.hpp index 39aae32..aa98c0b 100644 --- a/src/diagnostic/diagnostic-manager.hpp +++ b/src/diagnostic/diagnostic-manager.hpp @@ -17,6 +17,7 @@ #pragma once +#include #include #include @@ -94,4 +95,8 @@ public: bool add_recurring_request(DiagnosticRequest* request, const char* name, bool waitForMultipleResponses, const DiagnosticResponseDecoder decoder, const DiagnosticResponseCallback callback, float frequencyHz); + + bool conflicting(active_diagnostic_request_t* request, active_diagnostic_request_t* candidate) const; + bool clear_to_send(active_diagnostic_request_t* request) const; + static int send_request(sd_event_source *s, uint64_t usec, void *userdata); }; diff --git a/src/diagnostic/diagnostic-message.cpp b/src/diagnostic/diagnostic-message.cpp index 62e97b8..cb4e691 100644 --- a/src/diagnostic/diagnostic-message.cpp +++ b/src/diagnostic/diagnostic-message.cpp @@ -58,6 +58,11 @@ const std::string& obd2_signal_t::get_prefix() const return prefix_; } +int obd2_signal_t::get_frequency() const +{ + return frequency_; +} + void obd2_signal_t::set_prefix(std::string val) { prefix_ = val; diff --git a/src/diagnostic/diagnostic-message.hpp b/src/diagnostic/diagnostic-message.hpp index 8301917..02c6a9f 100644 --- a/src/diagnostic/diagnostic-message.hpp +++ b/src/diagnostic/diagnostic-message.hpp @@ -62,6 +62,7 @@ class obd2_signal_t { const std::string& get_generic_name() const; const std::string get_name() const; const std::string& get_prefix() const; + int get_frequency() const; void set_prefix(std::string val); @@ -71,5 +72,5 @@ class obd2_signal_t { bool is_obd2_request(DiagnosticRequest *request); bool is_obd2_signal(const char *name); - float decode_obd2_response(const DiagnosticResponse* response, float parsedPayload); + static float decode_obd2_response(const DiagnosticResponse* response, float parsedPayload); }; \ No newline at end of file diff --git a/src/low-can-binding.cpp b/src/low-can-binding.cpp index 52e893d..7baf147 100644 --- a/src/low-can-binding.cpp +++ b/src/low-can-binding.cpp @@ -72,6 +72,44 @@ static int create_event_handle(const std::string& sig_name, std::map subscribed_signals_lock(get_subscribed_signals_mutex()); + std::map& s = get_subscribed_signals(); + if (s.find(sig) != s.end() && !afb_event_is_valid(s[sig])) + { + if(!subscribe) + { + NOTICE(binder_interface, "Event isn't valid, it can't be unsubscribed."); + ret = -1; + } + else + { + /* Event it isn't valid anymore, recreate it */ + sd_event_add_time(afb_daemon_get_event_loop(binder_interface->daemon), &source, CLOCK_MONOTONIC, frequency, 0, + configuration_t::instance().get_diagnostic_manager().send_request, diag_req); + ret = create_event_handle(sig, s); + } + } + else + { + /* Event doesn't exist , so let's create it */ + struct afb_event empty_event = {nullptr, nullptr}; + subscribed_signals[sig] = empty_event; + ret = create_event_handle(sig, s); + } + + /* Check whether or not the event handler has been correctly created and + * make the subscription/unsubscription operation is so. + */ + if (ret <= 0) + return ret; + return make_subscription_unsubscription(request, sig, s, subscribe); +} + static int subscribe_unsubscribe_signal(struct afb_req request, bool subscribe, const std::string& sig) { int ret; @@ -125,10 +163,25 @@ static int subscribe_unsubscribe_signals(struct afb_req request, bool subscribe, const std::string& can_prefix = configuration_t::instance().get_can_signals().front().get_prefix(); const std::string& obd2_prefix = configuration_t::instance().get_obd2_signals().front().get_prefix(); - for(auto& sig : signals) + for(const std::string& sig : signals) { - //if (sig.find_first_of()) - int ret = subscribe_unsubscribe_signal(request, subscribe, sig); + int ret; + if (sig.find_first_of(can_prefix.c_str(), 0, can_prefix.size())) + ret = subscribe_unsubscribe_signal(request, subscribe, sig); + else if (sig.find_first_of(obd2_prefix.c_str(), 0, obd2_prefix.size())) + { + std::vector found; + configuration_t::instance().find_obd2_signals(build_DynamicField(sig), found); + int frequency = found.front()->get_frequency(); + DiagnosticRequest* diag_req = new DiagnosticRequest(found.front()->build_diagnostic_request()); + configuration_t::instance().get_diagnostic_manager().add_recurring_request( + diag_req, sig.c_str(), false, obd2_signal_t::decode_obd2_response, nullptr, (float)frequency); + //TODO: Adding callback requesting ignition status: diag_req, sig.c_str(), false, obd2_signal_t::decode_obd2_response, obd2_signal_t::check_ignition_status, frequency); + ret = subscribe_unsubscribe_signal(request, subscribe, sig, diag_req,frequency); + } + else + ret = -1; + if(ret <= 0) return ret; rets++; diff --git a/src/utils/timer.cpp b/src/utils/timer.cpp index 966178b..1461ac4 100644 --- a/src/utils/timer.cpp +++ b/src/utils/timer.cpp @@ -71,3 +71,10 @@ bool frequency_clock_t::elapsed(bool stagger) return frequency_ == 0 || elapsed_time >= period; } + +/// @brief Force the clock to tick, regardless of it its time has actually +/// elapsed. +void frequency_clock_t::tick() +{ + last_tick_ = get_time_function()(); +} \ No newline at end of file diff --git a/src/utils/timer.hpp b/src/utils/timer.hpp index 8037b85..ce6336e 100644 --- a/src/utils/timer.hpp +++ b/src/utils/timer.hpp @@ -56,4 +56,6 @@ public: bool started(); time_function_t get_time_function(); bool elapsed(bool stagger); + + void tick(); }; \ No newline at end of file -- cgit 1.2.3-korg