Advanced tracing

libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp

@@ -11,6 +11,7 @@
 #include <uavcan/node/publisher.hpp>
 #include <uavcan/util/method_binder.hpp>
 #include <uavcan/protocol/dynamic_node_id_server/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
 // UAVCAN types
 #include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
 
@@ -44,25 +45,27 @@ public:
 class AllocationRequestManager
 {
     typedef MethodBinder<AllocationRequestManager*,
-                         void (AllocationRequestManager::*)
-                             (const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>&)>
+                         void (AllocationRequestManager::*)(const ReceivedDataStructure<Allocation>&)>
         AllocationCallback;
 
     const MonotonicDuration stage_timeout_;
 
     MonotonicTime last_message_timestamp_;
-    protocol::dynamic_node_id::Allocation::FieldTypes::unique_id current_unique_id_;
+    Allocation::FieldTypes::unique_id current_unique_id_;
 
     IAllocationRequestHandler& handler_;
+    IEventTracer& tracer_;
 
-    Subscriber<protocol::dynamic_node_id::Allocation, AllocationCallback> allocation_sub_;
-    Publisher<protocol::dynamic_node_id::Allocation> allocation_pub_;
+    Subscriber<Allocation, AllocationCallback> allocation_sub_;
+    Publisher<Allocation> allocation_pub_;
 
     enum { InvalidStage = 0 };
 
-    static uint8_t detectRequestStage(const protocol::dynamic_node_id::Allocation& msg)
+    void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
+
+    static uint8_t detectRequestStage(const Allocation& msg)
     {
-        const uint8_t max_bytes_per_request = protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST;
+        const uint8_t max_bytes_per_request = Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST;
 
         if ((msg.unique_id.size() != max_bytes_per_request) &&
             (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) &&
@@ -74,11 +77,11 @@ class AllocationRequestManager
         {
             return 1;       // Note that CAN FD frames can deliver the unique ID in one stage!
         }
-        if (msg.unique_id.size() == protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
+        if (msg.unique_id.size() == Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
         {
             return 2;
         }
-        if (msg.unique_id.size() < protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
+        if (msg.unique_id.size() < Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
         {
             return 3;
         }
@@ -91,11 +94,11 @@ class AllocationRequestManager
         {
             return 1;
         }
-        if (current_unique_id_.size() >= (protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2))
+        if (current_unique_id_.size() >= (Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2))
         {
             return 3;
         }
-        if (current_unique_id_.size() >= protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
+        if (current_unique_id_.size() >= Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
         {
             return 2;
         }
@@ -104,21 +107,24 @@ class AllocationRequestManager
 
     void publishFollowupAllocationResponse()
     {
-        protocol::dynamic_node_id::Allocation msg;
+        Allocation msg;
         msg.unique_id = current_unique_id_;
         UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity());
 
         UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID",
                      unsigned(msg.unique_id.size()));
 
+        trace(TraceAllocationFollowupResponse, msg.unique_id.size());
+
         const int res = allocation_pub_.broadcast(msg);
         if (res < 0)
         {
+            trace(TraceError, res);
             allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast");
         }
     }
 
-    void handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg)
+    void handleAllocation(const ReceivedDataStructure<Allocation>& msg)
     {
         if (!msg.isAnonymousTransfer())
         {
@@ -132,6 +138,7 @@ class AllocationRequestManager
         {
             UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset");
             current_unique_id_.clear();
+            trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec());
         }
 
         /*
@@ -140,24 +147,29 @@ class AllocationRequestManager
         const uint8_t request_stage = detectRequestStage(msg);
         if (request_stage == InvalidStage)
         {
+            trace(TraceAllocationBadRequest, msg.unique_id.size());
             return;             // Malformed request - ignore without resetting
         }
 
         const uint8_t expected_stage = getExpectedStage();
         if (request_stage == InvalidStage)
         {
+            UAVCAN_ASSERT(0);
             return;
         }
 
         if (request_stage != expected_stage)
         {
+            trace(TraceAllocationUnexpectedStage, request_stage);
             return;             // Ignore - stage mismatch
         }
 
-        const uint8_t max_expected_bytes = static_cast<uint8_t>(current_unique_id_.capacity() - current_unique_id_.size());
+        const uint8_t max_expected_bytes =
+            static_cast<uint8_t>(current_unique_id_.capacity() - current_unique_id_.size());
         UAVCAN_ASSERT(max_expected_bytes > 0);
         if (msg.unique_id.size() > max_expected_bytes)
         {
+            trace(TraceAllocationBadRequest, msg.unique_id.size());
             return;             // Malformed request
         }
 
@@ -169,6 +181,8 @@ class AllocationRequestManager
             current_unique_id_.push_back(msg.unique_id[i]);
         }
 
+        trace(TraceAllocationRequestAccepted, current_unique_id_.size());
+
         /*
          * Proceeding with allocation if possible
          * Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader.
@@ -183,6 +197,13 @@ class AllocationRequestManager
             current_unique_id_.clear();
 
             handler_.handleAllocationRequest(unique_id, msg.node_id);
+
+            uint64_t event_agrument = 0;
+            for (uint8_t i = 0; i < 8; i++)
+            {
+                event_agrument |= static_cast<uint64_t>(unique_id[i]) << (i * 8U);
+            }
+            trace(TraceAllocationExchangeComplete, static_cast<int64_t>(event_agrument));
         }
         else
         {
@@ -192,6 +213,7 @@ class AllocationRequestManager
             }
             else
             {
+                trace(TraceAllocationFollowupDenied, 0);
                 current_unique_id_.clear();
             }
         }
@@ -203,9 +225,10 @@ class AllocationRequestManager
     }
 
 public:
-    AllocationRequestManager(INode& node, IAllocationRequestHandler& handler)
+    AllocationRequestManager(INode& node, IEventTracer& tracer, IAllocationRequestHandler& handler)
         : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS))
         , handler_(handler)
+        , tracer_(tracer)
         , allocation_sub_(node)
         , allocation_pub_(node)
     { }
@@ -231,13 +254,15 @@ public:
 
     int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id)
     {
-        protocol::dynamic_node_id::Allocation msg;
+        Allocation msg;
 
         msg.unique_id.resize(msg.unique_id.capacity());
         copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin());
 
         msg.node_id = allocated_node_id.get();
 
+        trace(TraceAllocationResponse, msg.node_id);
+
         return allocation_pub_.broadcast(msg);
     }
 };

libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp

@@ -227,8 +227,8 @@ public:
            IEventTracer& tracer)
         : node_(node)
         , raft_core_(node, storage, tracer, *this)
-        , allocation_request_manager_(node, *this)
-        , node_discoverer_(node, *this)
+        , allocation_request_manager_(node, tracer, *this)
+        , node_discoverer_(node, tracer, *this)
     { }
 
     int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown)

libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp

@@ -6,7 +6,7 @@
 #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED
 
 #include <uavcan/build_config.hpp>
-#include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
 
 namespace uavcan
 {
@@ -51,6 +51,35 @@ enum TraceCode
     TraceRaftNewEntryCommitted,         // new commit index value
     // 25
     TraceRaftAppendEntriesCallFailure,  // error code (may be negated)
+    Trace0,
+    Trace1,
+    Trace2,
+    Trace3,
+    // 30
+    TraceAllocationFollowupResponse,    // number of unique ID bytes in this response
+    TraceAllocationFollowupDenied,      // reason code (see sources for details)
+    TraceAllocationFollowupTimeout,     // timeout value in microseconds
+    TraceAllocationBadRequest,          // number of unique ID bytes in this request
+    TraceAllocationUnexpectedStage,     // stage number in the request - 1, 2, or 3
+    // 35
+    TraceAllocationRequestAccepted,     // number of bytes of unique ID after request
+    TraceAllocationExchangeComplete,    // first 8 bytes of unique ID interpreted as signed 64 bit little endian
+    TraceAllocationResponse,            // allocated node ID
+    Trace11,
+    Trace12,
+    // 40
+    TraceDiscoveryNewNodeFound,         // node ID
+    TraceDiscoveryCommitCacheUpdated,   // node ID marked as committed
+    TraceDiscoveryNodeFinalized,        // node ID in lower 7 bits, bit 8 (256, 0x100) is set if unique ID is known
+    TraceDiscoveryGetNodeInfoFailure,   // node ID
+    TraceDiscoveryTimerStart,           // interval in microseconds
+    // 45
+    TraceDiscoveryTimerStop,            // reason code (see sources for details)
+    TraceDiscoveryGetNodeInfoRequest,   // target node ID
+    Trace20,
+    Trace21,
+    Trace22,
+    // 50
 
     NumTraceCodes
 };
@@ -68,9 +97,8 @@ public:
      */
     static const char* getEventName(TraceCode code)
     {
-        // import re
-        // make_strings = lambda s: ',\n'.join('"%s"' % x for x in re.findall(r'\ \ \ \ Trace([A-Za-z0-9]+),', s))
-        static const char* const Strings[NumTraceCodes] =
+        // import re;m = lambda s:',\n'.join('"%s"' % x for x in re.findall(r'\ {4}Trace[0-9]*([A-Za-z0-9]*),',s))
+        static const char* const Strings[] =
         {
             "Error",
             "LogLastIndexRestored",
@@ -97,7 +125,31 @@ public:
             "RaftCommitIndexUpdate",
             "RaftNewerTermInResponse",
             "RaftNewEntryCommitted",
-            "RaftAppendEntriesCallFailure"
+            "RaftAppendEntriesCallFailure",
+            "",
+            "",
+            "",
+            "",
+            "AllocationFollowupResponse",
+            "AllocationFollowupDenied",
+            "AllocationFollowupTimeout",
+            "AllocationBadRequest",
+            "AllocationUnexpectedStage",
+            "AllocationRequestAccepted",
+            "AllocationExchangeComplete",
+            "AllocationResponse",
+            "",
+            "",
+            "DiscoveryNewNodeFound",
+            "DiscoveryCommitCacheUpdated",
+            "DiscoveryNodeFinalized",
+            "DiscoveryGetNodeInfoFailure",
+            "DiscoveryTimerStart",
+            "DiscoveryTimerStop",
+            "DiscoveryGetNodeInfoRequest",
+            "",
+            "",
+            ""
         };
         uavcan::StaticAssert<sizeof(Strings) / sizeof(Strings[0]) == NumTraceCodes>::check();
         UAVCAN_ASSERT(code < NumTraceCodes);

libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp

@@ -12,6 +12,7 @@
 #include <uavcan/node/subscriber.hpp>
 #include <uavcan/node/service_client.hpp>
 #include <uavcan/protocol/dynamic_node_id_server/types.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
 #include <cassert>
 // UAVCAN types
 #include <uavcan/protocol/NodeStatus.hpp>
@@ -94,6 +95,7 @@ class NodeDiscoverer : TimerBase
      * States
      */
     INodeDiscoveryHandler& handler_;
+    IEventTracer& tracer_;
 
     BitSet<NodeID::Max + 1> committed_node_mask_;       ///< Nodes that are marked will not be queried
     NodeMap node_map_;                                  ///< Will not work in UAVCAN_TINY
@@ -104,6 +106,8 @@ class NodeDiscoverer : TimerBase
     /*
      * Methods
      */
+    void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
+
     INode& getNode() { return node_status_sub_.getNode(); }
 
     NodeID pickNextNodeToQuery() const
@@ -166,6 +170,7 @@ class NodeDiscoverer : TimerBase
         }
         else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted)
         {
+            trace(TraceDiscoveryCommitCacheUpdated, node_id.get());
             committed_node_mask_[node_id.get()] = true;
             node_map_.remove(node_id);
             return false;
@@ -179,6 +184,7 @@ class NodeDiscoverer : TimerBase
 
     void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
     {
+        trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == NULL) ? 0U : 0x100U));
         node_map_.remove(node_id);
         if (needToQuery(node_id))     // Making sure the server is still interested
         {
@@ -196,6 +202,8 @@ class NodeDiscoverer : TimerBase
         }
         else
         {
+            trace(TraceDiscoveryGetNodeInfoFailure, result.server_node_id.get());
+
             NodeData* const data = node_map_.access(result.server_node_id);
             if (data == NULL)
             {
@@ -222,7 +230,7 @@ class NodeDiscoverer : TimerBase
 
         if (!handler_.canDiscoverNewNodes())
         {
-            UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer stopped - server disallows discovery");
+            trace(TraceDiscoveryTimerStop, 0);
             stop();
             return;
         }
@@ -230,11 +238,13 @@ class NodeDiscoverer : TimerBase
         const NodeID node_id = pickNextNodeToQuery();
         if (!node_id.isUnicast())
         {
-            UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer stopped - no unknown nodes left");
+            trace(TraceDiscoveryTimerStop, 1);
             stop();
             return;
         }
 
+        trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
+
         UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
                      int(node_id.get()));
         const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request());
@@ -254,7 +264,8 @@ class NodeDiscoverer : TimerBase
         NodeData* data = node_map_.access(msg.getSrcNodeID());
         if (data == NULL)
         {
-            UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Found new node %d", int(msg.getSrcNodeID().get()));
+            trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get());
+
             data = node_map_.insert(msg.getSrcNodeID(), NodeData());
             if (data == NULL)
             {
@@ -273,15 +284,16 @@ class NodeDiscoverer : TimerBase
 
         if (!isRunning() && handler_.canDiscoverNewNodes())
         {
-            UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer started");
+            trace(TraceDiscoveryTimerStart, get_node_info_client_.getRequestTimeout().toUSec());
             startPeriodic(get_node_info_client_.getRequestTimeout());
         }
     }
 
 public:
-    NodeDiscoverer(INode& node, INodeDiscoveryHandler& handler)
+    NodeDiscoverer(INode& node, IEventTracer& tracer, INodeDiscoveryHandler& handler)
         : TimerBase(node)
         , handler_(handler)
+        , tracer_(tracer)
         , node_map_(node.getAllocator())
         , get_node_info_client_(node)
         , node_status_sub_(node)

libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp

@@ -5,6 +5,7 @@
 #include <gtest/gtest.h>
 #include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
 #include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include "event_tracer.hpp"
 #include "../helpers.hpp"
 
 
@@ -88,10 +89,11 @@ TEST(dynamic_node_id_server_AllocationRequestManager, Basic)
     /*
      * Request manager initialization
      */
+    EventTracer tracer;
     AllocationRequestHandler handler;
     handler.can_followup = true;
 
-    AllocationRequestManager manager(nodes.a, handler);
+    AllocationRequestManager manager(nodes.a, tracer, handler);
 
     ASSERT_LE(0, manager.init());
 

libuavcan/test/protocol/dynamic_node_id_server/event.cpp

@@ -8,7 +8,6 @@
 
 TEST(DynamicNodeIDServer, EventCodeToString)
 {
-    using namespace uavcan::dynamic_node_id_server::distributed;
     using namespace uavcan::dynamic_node_id_server;
 
     // Simply checking some error codes