From 10b5661da41aeb30fa040757c3386f9dbbda4222 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 19:22:03 +0300 Subject: [PATCH] Advanced tracing --- .../allocation_request_manager.hpp | 57 ++++++++++++----- .../distributed/server.hpp | 4 +- .../protocol/dynamic_node_id_server/event.hpp | 62 +++++++++++++++++-- .../node_discoverer.hpp | 22 +++++-- .../allocation_request_manager.cpp | 4 +- .../protocol/dynamic_node_id_server/event.cpp | 1 - 6 files changed, 120 insertions(+), 30 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index 30fc0915e4..bb5a6fb3bd 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -11,6 +11,7 @@ #include #include #include +#include // UAVCAN types #include @@ -44,25 +45,27 @@ public: class AllocationRequestManager { typedef MethodBinder&)> + void (AllocationRequestManager::*)(const ReceivedDataStructure&)> 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 allocation_sub_; - Publisher allocation_pub_; + Subscriber allocation_sub_; + Publisher 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& msg) + void handleAllocation(const ReceivedDataStructure& 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(current_unique_id_.capacity() - current_unique_id_.size()); + const uint8_t max_expected_bytes = + static_cast(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(unique_id[i]) << (i * 8U); + } + trace(TraceAllocationExchangeComplete, static_cast(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); } }; diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index e98838bf2e..274045a6f3 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/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) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index 566c9a7ed7..d3ab3949c8 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/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 -#include +#include 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::check(); UAVCAN_ASSERT(code < NumTraceCodes); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 5483795308..c1074c3a8e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include // UAVCAN types #include @@ -94,6 +95,7 @@ class NodeDiscoverer : TimerBase * States */ INodeDiscoveryHandler& handler_; + IEventTracer& tracer_; BitSet 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) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp index edf82906e4..dfecf86b6a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -5,6 +5,7 @@ #include #include #include +#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()); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp index 8aea0e56c0..b62abff9de 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp +++ b/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