From 2fa78ddda2d4cb206961b77be051745edd5faf3f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 8 Jul 2015 23:24:59 +0300 Subject: [PATCH] NodeStatusMonitor update --- .../uavcan/protocol/node_status_monitor.hpp | 143 +++++++++++++----- .../test/protocol/node_status_monitor.cpp | 129 ++++++++-------- 2 files changed, 168 insertions(+), 104 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index 0e5c80aa68..85bb692280 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -22,17 +22,36 @@ namespace uavcan class UAVCAN_EXPORT NodeStatusMonitor { public: - typedef typename StorageType::Type NodeStatusCode; - struct NodeStatus { - NodeStatusCode status_code; ///< Current status code; OFFLINE if not known. - bool known; ///< True if the node was online at least once. + uint8_t health : 2; + uint8_t mode : 3; + uint8_t sub_mode : 3; - NodeStatus() - : status_code(protocol::NodeStatus::STATUS_OFFLINE) - , known(false) - { } + NodeStatus() : + health(protocol::NodeStatus::HEALTH_OK), + mode(protocol::NodeStatus::MODE_OFFLINE), + sub_mode(0) + { + StaticAssert::check(); + StaticAssert::check(); + StaticAssert::check(); + } + + bool operator!=(const NodeStatus rhs) const { return !operator==(rhs); } + bool operator==(const NodeStatus rhs) const + { + return std::memcmp(this, &rhs, sizeof(NodeStatus)) == 0; + } + +#if UAVCAN_TOSTRING + std::string toString() const + { + char buf[40]; + (void)snprintf(buf, sizeof(buf), "health=%d mode=%d sub_mode=%d", int(health), int(mode), int(sub_mode)); + return std::string(buf); + } +#endif }; struct NodeStatusChangeEvent @@ -40,6 +59,11 @@ public: NodeID node_id; NodeStatus status; NodeStatus old_status; + bool was_known; + + NodeStatusChangeEvent() : + was_known(false) + { } }; private: @@ -57,11 +81,10 @@ private: struct Entry { - NodeStatusCode status_code; + NodeStatus status; int8_t time_since_last_update_ms100; - Entry() - : status_code(protocol::NodeStatus::STATUS_OFFLINE) - , time_since_last_update_ms100(-1) + Entry() : + time_since_last_update_ms100(-1) { } }; @@ -79,20 +102,18 @@ private: 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) + if (entry.status != new_entry_value.status) { NodeStatusChangeEvent event; - event.node_id = node_id; + event.node_id = node_id; + event.old_status = entry.status; + event.status = new_entry_value.status; + event.was_known = entry.time_since_last_update_ms100 >= 0; - event.old_status.known = entry.time_since_last_update_ms100 >= 0; - event.old_status.status_code = entry.status_code; + UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: [%s] --> [%s]", int(node_id.get()), + (event.was_known ? "known" : "new"), + event.old_status.toString().c_str(), event.status.toString().c_str()); - 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; @@ -100,11 +121,13 @@ private: 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; + Entry new_entry; + new_entry.time_since_last_update_ms100 = 0; + new_entry.status.health = msg.health & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1); + new_entry.status.mode = msg.mode & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1); + new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1); - changeNodeStatus(msg.getSrcNodeID(), new_entry_value); + changeNodeStatus(msg.getSrcNodeID(), new_entry); handleNodeStatusMessage(msg); } @@ -117,17 +140,19 @@ private: { 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.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) { Entry new_entry_value; new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100; - new_entry_value.status_code = protocol::NodeStatus::STATUS_OFFLINE; + new_entry_value.status = NodeStatus(); changeNodeStatus(nid, new_entry_value); } } @@ -209,6 +234,7 @@ public: /** * Returns status of a given node. * Unknown nodes are considered offline. + * Complexity O(1). */ NodeStatus getNodeStatus(NodeID node_id) const { @@ -217,25 +243,41 @@ public: 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 entry.status; } - return status; + else + { + return NodeStatus(); + } + } + + /** + * Whether the class has observed this node at least once since initialization. + * Complexity O(1). + */ + bool isNodeKnown(NodeID node_id) const + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return false; + } + return getEntry(node_id).time_since_last_update_ms100 >= 0; } /** * This helper method allows to quickly estimate the overall network health. - * Status of the local node is not considered. + * Health 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 findNodeWithWorstHealth() const { - NodeID nid_with_worst_status; - NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK; + NodeID nid_with_worst_health; + uint8_t worst_health = protocol::NodeStatus::HEALTH_OK; for (uint8_t i = 1; i <= NodeID::Max; i++) { @@ -244,14 +286,35 @@ public: 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()) + if (entry.status.health > worst_health || !nid_with_worst_health.isValid()) { - nid_with_worst_status = nid; - worst_status_code = entry.status_code; + nid_with_worst_health = nid; + worst_health = entry.status.health; } } } - return nid_with_worst_status; + + return nid_with_worst_health; + } + + /** + * Calls the operator for every known node. + * Operator signature: + * void (NodeID, NodeStatus) + */ + template + void forEachNode(Operator op) const + { + 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) + { + op(nid, entry.status); + } + } } }; diff --git a/libuavcan/test/protocol/node_status_monitor.cpp b/libuavcan/test/protocol/node_status_monitor.cpp index 41beb4d8b9..5c3e962e5c 100644 --- a/libuavcan/test/protocol/node_status_monitor.cpp +++ b/libuavcan/test/protocol/node_status_monitor.cpp @@ -7,11 +7,13 @@ #include #include "helpers.hpp" -static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id, uavcan::uint8_t status_code, +static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id, + uavcan::uint8_t health, uavcan::uint8_t mode, uavcan::uint32_t uptime_sec, uavcan::TransferID tid) { uavcan::protocol::NodeStatus msg; - msg.status_code = status_code; + msg.health = health; + msg.mode = mode; msg.uptime_sec = uptime_sec; emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid); } @@ -46,38 +48,37 @@ TEST(NodeStatusMonitor, Basic) /* * Empty NSM, no nodes were registered yet */ - ASSERT_FALSE(nsm.findNodeWithWorstStatus().isValid()); + ASSERT_FALSE(nsm.findNodeWithWorstHealth().isValid()); - uavcan::NodeStatusMonitor::NodeStatus st = nsm.getNodeStatus(uavcan::NodeID(123)); - ASSERT_FALSE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(123))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(123)).mode); /* * Some new status messages */ - publishNodeStatus(can, 10, NodeStatus::STATUS_OK, 12, 0); + publishNodeStatus(can, 10, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 12, 0); shortSpin(node); - ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstStatus()); + ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstHealth()); - publishNodeStatus(can, 9, NodeStatus::STATUS_INITIALIZING, 0, 0); + publishNodeStatus(can, 9, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_INITIALIZATION, 0, 0); shortSpin(node); - ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus()); + ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth()); - publishNodeStatus(can, 11, NodeStatus::STATUS_CRITICAL, 999, 0); + publishNodeStatus(can, 11, NodeStatus::HEALTH_CRITICAL, NodeStatus::MODE_MAINTENANCE, 999, 0); shortSpin(node); - ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstStatus()); + ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstHealth()); - st = nsm.getNodeStatus(uavcan::NodeID(10)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); - st = nsm.getNodeStatus(uavcan::NodeID(9)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); - st = nsm.getNodeStatus(uavcan::NodeID(11)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); /* * Timeout @@ -86,21 +87,21 @@ TEST(NodeStatusMonitor, Basic) clock_mock.advance(500000); shortSpin(node); - st = nsm.getNodeStatus(uavcan::NodeID(10)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); clock_mock.advance(500000); shortSpin(node); - st = nsm.getNodeStatus(uavcan::NodeID(9)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); clock_mock.advance(500000); shortSpin(node); - st = nsm.getNodeStatus(uavcan::NodeID(11)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); /* * Will timeout now @@ -108,68 +109,68 @@ TEST(NodeStatusMonitor, Basic) clock_mock.advance(4000000); shortSpin(node); - st = nsm.getNodeStatus(uavcan::NodeID(10)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); - st = nsm.getNodeStatus(uavcan::NodeID(9)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); - st = nsm.getNodeStatus(uavcan::NodeID(11)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health); /* * Recovering one node, adding two extra */ - publishNodeStatus(can, 11, NodeStatus::STATUS_WARNING, 999, 1); + publishNodeStatus(can, 11, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 999, 1); shortSpin(node); - publishNodeStatus(can, 127, NodeStatus::STATUS_WARNING, 9999, 1); + publishNodeStatus(can, 127, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 9999, 1); shortSpin(node); - publishNodeStatus(can, 1, NodeStatus::STATUS_OK, 1234, 1); + publishNodeStatus(can, 1, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 1234, 1); shortSpin(node); /* * Making sure OFFLINE is still worst status */ - ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus()); + ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth()); /* * Final validation */ - st = nsm.getNodeStatus(uavcan::NodeID(10)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health); - st = nsm.getNodeStatus(uavcan::NodeID(9)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health); - st = nsm.getNodeStatus(uavcan::NodeID(11)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(11)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(11)).health); - st = nsm.getNodeStatus(uavcan::NodeID(127)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(127))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(127)).mode); + ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(127)).health); - st = nsm.getNodeStatus(uavcan::NodeID(1)); - ASSERT_TRUE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); + ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(1))); + ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(1)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(1)).health); /* * Forgetting */ nsm.forgetNode(127); - st = nsm.getNodeStatus(uavcan::NodeID(127)); - ASSERT_FALSE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(127))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(127)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(127)).health); nsm.forgetNode(9); - st = nsm.getNodeStatus(uavcan::NodeID(9)); - ASSERT_FALSE(st.known); - ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(9))); + ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode); + ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(9)).health); }