diff options
| -rw-r--r-- | src/configuration.cpp | 13 | ||||
| -rw-r--r-- | src/diagnostic/active-diagnostic-request.cpp | 22 | ||||
| -rw-r--r-- | src/diagnostic/active-diagnostic-request.hpp | 5 | ||||
| -rw-r--r-- | src/diagnostic/diagnostic-manager.cpp | 54 | ||||
| -rw-r--r-- | src/diagnostic/diagnostic-manager.hpp | 5 | ||||
| -rw-r--r-- | src/diagnostic/diagnostic-message.cpp | 5 | ||||
| -rw-r--r-- | src/diagnostic/diagnostic-message.hpp | 3 | ||||
| -rw-r--r-- | src/low-can-binding.cpp | 59 | ||||
| -rw-r--r-- | src/utils/timer.cpp | 7 | ||||
| -rw-r--r-- | src/utils/timer.hpp | 2 |
10 files changed, 167 insertions, 8 deletions
diff --git a/src/configuration.cpp b/src/configuration.cpp index e56dc1c4..85840f80 100644 --- a/src/configuration.cpp +++ b/src/configuration.cpp @@ -89,14 +89,19 @@ std::vector<can_signal_t>& configuration_t::get_can_signals() return can_signals_[active_message_set_]; } -const std::vector<can_message_definition_t>& configuration_t::get_can_message_definition() +std::vector<obd2_signal_t>& configuration_t::get_obd2_signals() { - return can_message_definition_[active_message_set_]; + return obd2_signals_[active_message_set_]; } -std::vector<obd2_signal_t>& configuration_t::get_obd2_signals() +const std::vector<std::string>& configuration_t::get_signals_prefix() const { - return obd2_signals_[active_message_set_]; + return signals_prefix_; +} + +const std::vector<can_message_definition_t>& 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 505d816e..7a43a0d4 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<can_bus in_flight_{false}, frequency_clock_{frequency_clock_t(frequencyHz)}, timeout_clock_{frequency_clock_t(10)} {} +uint32_t active_diagnostic_request_t::get_id() const +{ + return id_; +} + std::shared_ptr<can_bus_dev_t> 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 88d40084..fb87c839 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<can_bus_dev_t> 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 5103bf10..6e8a68cf 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_t> 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 39aae321..aa98c0b7 100644 --- a/src/diagnostic/diagnostic-manager.hpp +++ b/src/diagnostic/diagnostic-manager.hpp @@ -17,6 +17,7 @@ #pragma once +#include <systemd/sd-event.h> #include <queue> #include <vector> @@ -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 62e97b8f..cb4e691b 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 8301917f..02c6a9f6 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 52e893d9..7baf147a 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<std::string return 1; } +static int subscribe_unsubscribe_signal(struct afb_req request, bool subscribe, const std::string& sig, DiagnosticRequest* diag_req, int frequency) +{ + int ret; + sd_event_source *source; + + std::lock_guard<std::mutex> subscribed_signals_lock(get_subscribed_signals_mutex()); + std::map<std::string, struct afb_event>& 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<obd2_signal_t*> 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 966178be..1461ac4b 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 8037b85d..ce6336eb 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 |