From f0627423c629b8e21592e9709f0adbe1c8c67e37 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Mon, 11 May 2015 17:15:18 +0300
Subject: [PATCH] NodeDiscoverer: Pausing querying GetNodeInfo if the node does
 not send NodeStatus

---
 .../protocol/dynamic_node_id_server/event.hpp |  2 +-
 .../node_discoverer.hpp                       | 24 ++++++++++++++-----
 2 files changed, 19 insertions(+), 7 deletions(-)

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 d3d052aee1..e28800a6dd 100644
--- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp
@@ -74,7 +74,7 @@ enum TraceCode
     TraceDiscoveryGetNodeInfoFailure,   // node ID
     TraceDiscoveryTimerStart,           // interval in microseconds
     // 45
-    TraceDiscoveryTimerStop,            // reason code (see sources for details)
+    TraceDiscoveryTimerStop,            // number of unknown nodes
     TraceDiscoveryGetNodeInfoRequest,   // target node ID
     TraceDiscoveryNodeRestartDetected,  // node ID
     Trace21,
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 7fd1474691..e1a9527101 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
@@ -74,12 +74,14 @@ class NodeDiscoverer : TimerBase
 
     struct NodeData
     {
-        uint32_t last_seen_uptime;
-        uint8_t num_get_node_info_attempts;
+        uint32_t last_seen_uptime;              ///< Used for restart detection
+        uint8_t num_get_node_info_attempts;     ///< Used to detect if the node doesn't support GetNodeInfo
+        bool updated_since_last_query;          ///< Allows to pause trying to GetNodeInfo if the node is offline
 
         NodeData()
             : last_seen_uptime(0)
             , num_get_node_info_attempts(0)
+            , updated_since_last_query(false)
         { }
     };
 
@@ -120,7 +122,10 @@ class NodeDiscoverer : TimerBase
         {
             const NodeMap::KVPair* const kv = node_map_.getByIndex(i);
             UAVCAN_ASSERT(kv != NULL);
-            lowest_number_of_attempts = min(lowest_number_of_attempts, kv->value.num_get_node_info_attempts);
+            if (kv->value.updated_since_last_query)
+            {
+                lowest_number_of_attempts = min(lowest_number_of_attempts, kv->value.num_get_node_info_attempts);
+            }
         }
 
         // Now, among nodes with this number of attempts selecting the one with highest uptime.
@@ -131,7 +136,8 @@ class NodeDiscoverer : TimerBase
             const NodeMap::KVPair* const kv = node_map_.getByIndex(i);
             UAVCAN_ASSERT(kv != NULL);
             if ((kv->value.num_get_node_info_attempts == lowest_number_of_attempts) &&
-                (kv->value.last_seen_uptime >= largest_uptime))
+                (kv->value.last_seen_uptime >= largest_uptime) &&
+                (kv->value.updated_since_last_query))
             {
                 largest_uptime = kv->value.last_seen_uptime;
                 output = kv->key;
@@ -230,7 +236,7 @@ class NodeDiscoverer : TimerBase
 
         if (!handler_.canDiscoverNewNodes())
         {
-            trace(TraceDiscoveryTimerStop, 0);
+            trace(TraceDiscoveryTimerStop, node_map_.getSize());
             stop();
             return;
         }
@@ -238,11 +244,16 @@ class NodeDiscoverer : TimerBase
         const NodeID node_id = pickNextNodeToQuery();
         if (!node_id.isUnicast())
         {
-            trace(TraceDiscoveryTimerStop, 1);
+            trace(TraceDiscoveryTimerStop, node_map_.getSize());
             stop();
             return;
         }
 
+        NodeData* const data = node_map_.access(node_id);
+        UAVCAN_ASSERT(data != NULL);
+        UAVCAN_ASSERT(data->updated_since_last_query);
+        data->updated_since_last_query = false;
+
         trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
 
         UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
@@ -281,6 +292,7 @@ class NodeDiscoverer : TimerBase
             data->num_get_node_info_attempts = 0;
         }
         data->last_seen_uptime = msg.uptime_sec;
+        data->updated_since_last_query = true;
 
         if (!isRunning() && handler_.canDiscoverNewNodes())
         {