tbf

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

diff --git a/src/can-bus.cpp b/src/can-bus.cpp
index b1dcc3a..f35319a 100644 (file)
--- a/src/can-bus.cpp
+++ b/src/can-bus.cpp
@@ -64,36 +64,34 @@ void can_bus_t::can_decode_message()
                        std::unique_lock<std::mutex> can_message_lock(can_message_mutex_);
                        new_can_message_.wait(can_message_lock);
                        can_message = next_can_message();
-               }
-
-               /* First we have to found which CanSignal it is */
-               search_key = build_DynamicField((double)can_message.get_id());
-               signals = find_can_signals(search_key);
+               
+                       /* First we have to found which CanSignal it is */
+                       search_key = build_DynamicField((double)can_message.get_id());
+                       signals = find_can_signals(search_key);
 
-               /* Decoding the message ! Don't kill the messenger ! */
-               for(auto& sig : signals)
-               {
+                       /* Decoding the message ! Don't kill the messenger ! */
+                       for(auto& sig : signals)
                        {
-                               std::lock_guard<std::mutex> subscribed_signals_lock(get_subscribed_signals_mutex());
-                               std::map<std::string, struct afb_event>& s = get_subscribed_signals();
-                               
-                               const auto& it = s.find(sig.genericName);
-                               if (it != s.end())
-                                       DEBUG(binder_interface, "Iterator key: %s, event valid? %d", it->first.c_str(), afb_event_is_valid(it->second));
-                               DEBUG(binder_interface, "Operator[] key char: %s, event valid? %d", sig.genericName, afb_event_is_valid(s[sig.genericName]));
-                               DEBUG(binder_interface, "Operator[] key string: %s, event valid? %d", sig.genericName, afb_event_is_valid(s[std::string(sig.genericName)]));
-                               DEBUG(binder_interface, "Nb elt matched char: %d", (int)s.count(sig.genericName));
-                               DEBUG(binder_interface, "Nb elt matched string: %d", (int)s.count(std::string(sig.genericName)));
-                               if( it != s.end() && afb_event_is_valid(it->second))
                                {
-                                       decoded_message = decoder.translateSignal(sig, can_message, getSignals());
-
-                                       openxc_SimpleMessage s_message = build_SimpleMessage(sig.genericName, decoded_message);
-                                       vehicle_message = build_VehicleMessage_with_SimpleMessage(openxc_DynamicField_Type::openxc_DynamicField_Type_NUM, s_message);
-
-                                       std::lock_guard<std::mutex> decoded_can_message_lock(decoded_can_message_mutex_);
-                                       push_new_vehicle_message(vehicle_message);
-                                       new_decoded_can_message_.notify_one();
+                                       std::lock_guard<std::mutex> subscribed_signals_lock(get_subscribed_signals_mutex());
+                                       std::map<std::string, struct afb_event>& s = get_subscribed_signals();
+                                       
+                                       /* DEBUG message to make easier debugger STL containers...
+                                       DEBUG(binder_interface, "Operator[] key char: %s, event valid? %d", sig.genericName, afb_event_is_valid(s[sig.genericName]));
+                                       DEBUG(binder_interface, "Operator[] key string: %s, event valid? %d", sig.genericName, afb_event_is_valid(s[std::string(sig.genericName)]));
+                                       DEBUG(binder_interface, "Nb elt matched char: %d", (int)s.count(sig.genericName));
+                                       DEBUG(binder_interface, "Nb elt matched string: %d", (int)s.count(std::string(sig.genericName))); */
+                                       if( s.find(sig.genericName) != s.end() && afb_event_is_valid(s[sig.genericName]))
+                                       {
+                                               decoded_message = decoder.translateSignal(sig, can_message, getSignals());
+
+                                               openxc_SimpleMessage s_message = build_SimpleMessage(sig.genericName, decoded_message);
+                                               vehicle_message = build_VehicleMessage_with_SimpleMessage(openxc_DynamicField_Type::openxc_DynamicField_Type_NUM, s_message);
+
+                                               std::lock_guard<std::mutex> decoded_can_message_lock(decoded_can_message_mutex_);
+                                               push_new_vehicle_message(vehicle_message);
+                                               new_decoded_can_message_.notify_one();
+                                       }
                                }
                        }
                }
@@ -112,18 +110,17 @@ void can_bus_t::can_event_push()
                        std::unique_lock<std::mutex> decoded_can_message_lock(decoded_can_message_mutex_);
                        new_decoded_can_message_.wait(decoded_can_message_lock);
                        v_message = next_vehicle_message();
-               }
 
-               s_message = get_simple_message(v_message);
-
-               {
-                       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(std::string(s_message.name)) != s.end() && afb_event_is_valid(s[std::string(s_message.name)]))
+                       s_message = get_simple_message(v_message);
                        {
-                               jo = json_object_new_object();
-                               jsonify_simple(s_message, jo);
-                               afb_event_push(s[std::string(s_message.name)], jo);
+                               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(std::string(s_message.name)) != s.end() && afb_event_is_valid(s[std::string(s_message.name)]))
+                               {
+                                       jo = json_object_new_object();
+                                       jsonify_simple(s_message, jo);
+                                       afb_event_push(s[std::string(s_message.name)], jo);
+                               }
                        }
                }
        }
@@ -168,6 +165,7 @@ int can_bus_t::init_can_dev()
                        {
                                i++;
                                DEBUG(binder_interface, "Start reading thread");
+                               NOTICE(binder_interface, "%s device opened and reading", device.c_str());
                                can_devices_m_[device]->start_reading(*this);
                        }
                        else
@@ -248,7 +246,6 @@ can_message_t can_bus_t::next_can_message()
                return can_msg;
        }
        
-       has_can_message_ = false;
        return can_msg;
 }
 
@@ -269,14 +266,12 @@ openxc_VehicleMessage can_bus_t::next_vehicle_message()
                return v_msg;
        }
        
-       has_vehicle_message_ = false;
        return v_msg;
 }
 
 void can_bus_t::push_new_vehicle_message(const openxc_VehicleMessage& v_msg)
 {
        vehicle_message_q_.push(v_msg);
-       has_vehicle_message_ = true;
 }
 
 std::map<std::string, std::shared_ptr<can_bus_dev_t>> can_bus_t::get_can_devices()
@@ -380,7 +375,9 @@ std::pair<struct canfd_frame&, size_t> can_bus_dev_t::read()
                ERROR(binder_interface, "read: Incomplete CAN(FD) frame");
                ::memset(&cfd, 0, sizeof(cfd));
        }
-
+       
+       DEBUG(binder_interface, "read: Found id: %X, length: %X, data %02X%02X%02X%02X%02X%02X%02X%02X", cfd.can_id, cfd.len,
+                                                       cfd.data[0], cfd.data[1], cfd.data[2], cfd.data[3], cfd.data[4], cfd.data[5], cfd.data[6], cfd.data[7]);
        return std::pair<struct canfd_frame&, size_t>(cfd, nbytes);
 }