From d623eee54af33a8ba5fca68aac04a8f4d1d4e1f0 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Mon, 11 May 2015 13:26:53 +0300
Subject: [PATCH] Raft logic fix: forcing active mode when allocation activity
 is detected

---
 .../allocation_request_manager.hpp                 |  8 ++++++++
 .../distributed/raft_core.hpp                      | 14 ++++++++++++--
 .../dynamic_node_id_server/distributed/server.hpp  |  5 +++++
 .../protocol/dynamic_node_id_server/event.hpp      |  2 +-
 .../allocation_request_manager.cpp                 |  6 ++++++
 5 files changed, 32 insertions(+), 3 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 a1c2f05b2f..8c63132adc 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
@@ -35,6 +35,11 @@ public:
      */
     virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0;
 
+    /**
+     * This method will be invoked when there's an Allocation message detected on the bus.
+     */
+    virtual void handleAllocationActivityDetection(const ReceivedDataStructure<Allocation>& msg) = 0;
+
     virtual ~IAllocationRequestHandler() { }
 };
 
@@ -126,6 +131,9 @@ class AllocationRequestManager
 
     void handleAllocation(const ReceivedDataStructure<Allocation>& msg)
     {
+        trace(TraceAllocationActivity, msg.getSrcNodeID().get());
+        handler_.handleAllocationActivityDetection(msg);
+
         if (!msg.isAnonymousTransfer())
         {
             return;         // This is a response from another allocator, ignore
diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp
index 5d0641fbc0..5323024c29 100644
--- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp
@@ -763,7 +763,8 @@ public:
         {
             return res;
         }
-        append_entries_client_.setCallback(AppendEntriesResponseCallback(this, &RaftCore::handleAppendEntriesResponse));
+        append_entries_client_.setCallback(AppendEntriesResponseCallback(this,
+                                                                         &RaftCore::handleAppendEntriesResponse));
         append_entries_client_.setRequestTimeout(update_interval_);
 
         for (uint8_t i = 0; i < NumRequestVoteClients; i++)
@@ -773,7 +774,8 @@ public:
             {
                 return res;
             }
-            request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse));
+            request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this,
+                                                                              &RaftCore::handleRequestVoteResponse));
             request_vote_clients_[i]->setRequestTimeout(update_interval_);
         }
 
@@ -785,6 +787,14 @@ public:
         return 0;
     }
 
+    /**
+     * Normally should be called when there's allocation activity on the bus.
+     */
+    void forceActiveMode()
+    {
+        setActiveMode(true); // If the current state was Follower, active mode may be toggling quickly for some time
+    }
+
     /**
      * This function is mostly needed for testing.
      */
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 eedb2eb46f..2c20fcb11d 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
@@ -135,6 +135,11 @@ class Server : IAllocationRequestHandler
          }
     }
 
+    virtual void handleAllocationActivityDetection(const ReceivedDataStructure<Allocation>&)
+    {
+        raft_core_.forceActiveMode();
+    }
+
     /*
      * Methods of INodeDiscoveryHandler
      */
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 6a61b8dfdb..97f58e4493 100644
--- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp
+++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp
@@ -65,7 +65,7 @@ enum TraceCode
     TraceAllocationRequestAccepted,     // number of bytes of unique ID after request
     TraceAllocationExchangeComplete,    // first 8 bytes of unique ID interpreted as signed 64 bit big endian
     TraceAllocationResponse,            // allocated node ID
-    Trace11,
+    TraceAllocationActivity,            // source node ID of the message
     Trace12,
     // 40
     TraceDiscoveryNewNodeFound,         // node ID
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 dfecf86b6a..037d73bc3a 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
@@ -30,6 +30,12 @@ public:
         return can_followup;
     }
 
+    virtual void handleAllocationActivityDetection(
+        const uavcan::ReceivedDataStructure<uavcan::protocol::dynamic_node_id::Allocation>& msg)
+    {
+        std::cout << "ALLOCATION ACTIVITY\n" << msg << std::endl;
+    }
+
     bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id)
     {
         if (requests_.empty())