From 00ec7186b09e61ea2fbe3d6ea13044b816a348be Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:24:31 +0300 Subject: [PATCH] Node status monitor moved to header --- .../uavcan/protocol/node_status_monitor.hpp | 133 ++++++++++++++-- .../src/protocol/uc_node_status_monitor.cpp | 143 ------------------ 2 files changed, 124 insertions(+), 152 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_node_status_monitor.cpp diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index e9028e37b9..cf0dd7fd9b 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -5,10 +5,13 @@ #ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED #define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED +#include #include #include #include #include +#include +#include namespace uavcan { @@ -40,7 +43,7 @@ public: }; private: - static const unsigned TimerPeriodMs100 = 5; + enum { TimerPeriodMs100 = 5 }; typedef MethodBinder&)> @@ -65,13 +68,71 @@ private: mutable Entry entries_[NodeID::Max]; // [1, NodeID::Max] - Entry& getEntry(NodeID node_id) const; + Entry& getEntry(NodeID node_id) const + { + if (node_id.get() < 1 || node_id.get() > NodeID::Max) + { + handleFatalError("NodeStatusMonitor NodeID"); + } + return entries_[node_id.get() - 1]; + } - void changeNodeStatus(const NodeID node_id, const Entry new_entry_value); + void changeNodeStatus(const NodeID node_id, const Entry new_entry_value) + { + Entry& entry = getEntry(node_id); + if (entry.status_code != new_entry_value.status_code) + { + NodeStatusChangeEvent event; + event.node_id = node_id; - void handleNodeStatus(const ReceivedDataStructure& msg); + event.old_status.known = entry.time_since_last_update_ms100 >= 0; + event.old_status.status_code = entry.status_code; - virtual void handleTimerEvent(const TimerEvent&); + event.status.known = true; + event.status.status_code = new_entry_value.status_code; + + UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: %i --> %i", int(node_id.get()), + (event.old_status.known ? "known" : "new"), + int(event.old_status.status_code), int(event.status.status_code)); + handleNodeStatusChange(event); + } + entry = new_entry_value; + } + + void handleNodeStatus(const ReceivedDataStructure& msg) + { + Entry new_entry_value; + new_entry_value.time_since_last_update_ms100 = 0; + new_entry_value.status_code = msg.status_code; + + changeNodeStatus(msg.getSrcNodeID(), new_entry_value); + + handleNodeStatusMessage(msg); + } + + virtual void handleTimerEvent(const TimerEvent&) + { + const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; + + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + UAVCAN_ASSERT(nid.isUnicast()); + Entry& entry = getEntry(nid); + if (entry.time_since_last_update_ms100 >= 0 && + entry.status_code != protocol::NodeStatus::STATUS_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) + { + Entry new_entry_value; + new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100; + new_entry_value.status_code = protocol::NodeStatus::STATUS_OFFLINE; + changeNodeStatus(nid, new_entry_value); + } + } + } + } protected: /** @@ -107,25 +168,79 @@ public: * Destroy the object to stop it. * Returns negative error code. */ - int start(); + int start() + { + const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus)); + if (res >= 0) + { + TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); + } + return res; + } /** * Make the node unknown. */ - void forgetNode(NodeID node_id); + void forgetNode(NodeID node_id) + { + if (node_id.isValid()) + { + Entry& entry = getEntry(node_id); + entry = Entry(); + } + else + { + UAVCAN_ASSERT(0); + } + } /** * Returns status of a given node. * Unknown nodes are considered offline. */ - NodeStatus getNodeStatus(NodeID node_id) const; + NodeStatus getNodeStatus(NodeID node_id) const + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return NodeStatus(); + } + NodeStatus status; + const Entry& entry = getEntry(node_id); + if (entry.time_since_last_update_ms100 >= 0) + { + status.known = true; + status.status_code = entry.status_code; + } + return status; + } /** * This helper method allows to quickly estimate the overall network health. * Status of the local node is not considered. * Returns an invalid Node ID value if there's no known nodes in the network. */ - NodeID findNodeWithWorstStatus() const; + NodeID findNodeWithWorstStatus() const + { + NodeID nid_with_worst_status; + NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK; + + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + UAVCAN_ASSERT(nid.isUnicast()); + const Entry& entry = getEntry(nid); + if (entry.time_since_last_update_ms100 >= 0) + { + if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid()) + { + nid_with_worst_status = nid; + worst_status_code = entry.status_code; + } + } + } + return nid_with_worst_status; + } }; } diff --git a/libuavcan/src/protocol/uc_node_status_monitor.cpp b/libuavcan/src/protocol/uc_node_status_monitor.cpp deleted file mode 100644 index 7904b8ecc4..0000000000 --- a/libuavcan/src/protocol/uc_node_status_monitor.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include -#include -#include - -namespace uavcan -{ - -const unsigned NodeStatusMonitor::TimerPeriodMs100; - -NodeStatusMonitor::Entry& NodeStatusMonitor::getEntry(NodeID node_id) const -{ - if (node_id.get() < 1 || node_id.get() > NodeID::Max) - { - handleFatalError("NodeStatusMonitor NodeID"); - } - return entries_[node_id.get() - 1]; -} - -void NodeStatusMonitor::changeNodeStatus(const NodeID node_id, const Entry new_entry_value) -{ - Entry& entry = getEntry(node_id); - if (entry.status_code != new_entry_value.status_code) - { - NodeStatusChangeEvent event; - event.node_id = node_id; - - event.old_status.known = entry.time_since_last_update_ms100 >= 0; - event.old_status.status_code = entry.status_code; - - event.status.known = true; - event.status.status_code = new_entry_value.status_code; - - UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: %i --> %i", int(node_id.get()), - (event.old_status.known ? "known" : "new"), - int(event.old_status.status_code), int(event.status.status_code)); - handleNodeStatusChange(event); - } - entry = new_entry_value; -} - -void NodeStatusMonitor::handleNodeStatus(const ReceivedDataStructure& msg) -{ - Entry new_entry_value; - new_entry_value.time_since_last_update_ms100 = 0; - new_entry_value.status_code = msg.status_code; - - changeNodeStatus(msg.getSrcNodeID(), new_entry_value); - - handleNodeStatusMessage(msg); -} - -void NodeStatusMonitor::handleTimerEvent(const TimerEvent&) -{ - const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; - - for (uint8_t i = 1; i <= NodeID::Max; i++) - { - const NodeID nid(i); - UAVCAN_ASSERT(nid.isUnicast()); - Entry& entry = getEntry(nid); - if (entry.time_since_last_update_ms100 >= 0 && - entry.status_code != protocol::NodeStatus::STATUS_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) - { - Entry new_entry_value; - new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100; - new_entry_value.status_code = protocol::NodeStatus::STATUS_OFFLINE; - changeNodeStatus(nid, new_entry_value); - } - } - } -} - -int NodeStatusMonitor::start() -{ - const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus)); - if (res >= 0) - { - TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); - } - return res; -} - -void NodeStatusMonitor::forgetNode(NodeID node_id) -{ - if (node_id.isValid()) - { - Entry& entry = getEntry(node_id); - entry = Entry(); - } - else - { - UAVCAN_ASSERT(0); - } -} - -NodeStatusMonitor::NodeStatus NodeStatusMonitor::getNodeStatus(NodeID node_id) const -{ - if (!node_id.isValid()) - { - UAVCAN_ASSERT(0); - return NodeStatus(); - } - NodeStatus status; - const Entry& entry = getEntry(node_id); - if (entry.time_since_last_update_ms100 >= 0) - { - status.known = true; - status.status_code = entry.status_code; - } - return status; -} - -NodeID NodeStatusMonitor::findNodeWithWorstStatus() const -{ - NodeID nid_with_worst_status; - NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK; - - for (uint8_t i = 1; i <= NodeID::Max; i++) - { - const NodeID nid(i); - UAVCAN_ASSERT(nid.isUnicast()); - const Entry& entry = getEntry(nid); - if (entry.time_since_last_update_ms100 >= 0) - { - if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid()) - { - nid_with_worst_status = nid; - worst_status_code = entry.status_code; - } - } - } - return nid_with_worst_status; -} - -}