From ad12760d7f3df47099dc2b46b24f5de5de5683e6 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Thu, 23 Jul 2015 14:47:57 +0300
Subject: [PATCH] NodeStatusMonitor: Fix to premature node timeout

---
 .../include/uavcan/protocol/node_status_monitor.hpp   | 11 ++++-------
 1 file changed, 4 insertions(+), 7 deletions(-)

diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp
index 92cba78d37..2c3c1256fa 100644
--- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp
+++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp
@@ -67,7 +67,7 @@ public:
     };
 
 private:
-    enum { TimerPeriodMs100 = 4 };
+    enum { TimerPeriodMs100 = 2 };
 
     typedef MethodBinder<NodeStatusMonitor*,
                          void (NodeStatusMonitor::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
@@ -138,22 +138,19 @@ private:
 
         for (uint8_t i = 1; i <= NodeID::Max; i++)
         {
-            const NodeID nid(i);
-            UAVCAN_ASSERT(nid.isUnicast());
-
-            Entry& entry = getEntry(nid);
+            Entry& entry = getEntry(i);
             if (entry.time_since_last_update_ms100 >= 0 &&
                 entry.status.mode != protocol::NodeStatus::MODE_OFFLINE)
             {
                 entry.time_since_last_update_ms100 =
                     int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
 
-                if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100)
+                if (entry.time_since_last_update_ms100 > OfflineTimeoutMs100)
                 {
                     Entry new_entry_value = entry;
                     new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100;
                     new_entry_value.status.mode = protocol::NodeStatus::MODE_OFFLINE;
-                    changeNodeStatus(nid, new_entry_value);
+                    changeNodeStatus(i, new_entry_value);
                 }
             }
         }