From 67b33a712e78bc4dc65bd8f95958afbee2e84bab Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 9 May 2015 23:52:51 +0300
Subject: [PATCH] Improved logic of allocation request manager

---
 .../dynamic_node_id/1010.Allocation.uavcan        |  5 +++++
 .../allocation_request_manager.hpp                | 15 +++++++++------
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan
index c4ecd05bf2..4793ac71f5 100644
--- a/dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan
+++ b/dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan
@@ -39,6 +39,11 @@
 #
 uint16 DEFAULT_REQUEST_PERIOD_MS = 1000
 
+#
+# Server will reset its state if there was no follow-up request in this amount of time.
+#
+uint16 FOLLOWUP_TIMEOUT_MS = 500
+
 #
 # Any request message can accommodate no more than this number of bytes of unique ID.
 # This limitation is needed to ensure that all request transfers are single-frame.
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 795bbae43f..65cd62a0fb 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
@@ -133,7 +133,6 @@ class AllocationRequestManager
             UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset");
             current_unique_id_.clear();
         }
-        last_message_timestamp_ = msg.getMonotonicTimestamp();
 
         /*
          * Checking if request stage matches the expected stage
@@ -141,26 +140,25 @@ class AllocationRequestManager
         const uint8_t request_stage = detectRequestStage(msg);
         if (request_stage == InvalidStage)
         {
-            return;         // No way
+            return;             // Malformed request - ignore without resetting
         }
 
         const uint8_t expected_stage = getExpectedStage();
         if (request_stage == InvalidStage)
         {
-            current_unique_id_.clear();
             return;
         }
 
         if (request_stage != expected_stage)
         {
-            return;         // Ignore - stage mismatch
+            return;             // Ignore - stage mismatch
         }
 
         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)
         {
-            return;         // Malformed request
+            return;             // Malformed request
         }
 
         /*
@@ -189,11 +187,16 @@ class AllocationRequestManager
         {
             broadcastIntermediateAllocationResponse();
         }
+
+        /*
+         * It is super important to update timestamp only if the request has been processed successfully.
+         */
+        last_message_timestamp_ = msg.getMonotonicTimestamp();
     }
 
 public:
     AllocationRequestManager(INode& node, IAllocationRequestHandler& handler)
-        : stage_timeout_(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS))
+        : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS))
         , active_(false)
         , handler_(handler)
         , allocation_sub_(node)