From 01902f07aaea7830b9b19addf0c0b53d44726b17 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 26 Mar 2014 15:42:04 +0400 Subject: [PATCH] NodeStatusMonitor --- .../uavcan/protocol/node_status_monitor.hpp | 101 +++++++++ .../src/protocol/node_status_monitor.cpp | 143 +++++++++++++ .../test/protocol/node_status_monitor.cpp | 197 ++++++++++++++++++ 3 files changed, 441 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/node_status_monitor.hpp create mode 100644 libuavcan/src/protocol/node_status_monitor.cpp create mode 100644 libuavcan/test/protocol/node_status_monitor.cpp diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp new file mode 100644 index 0000000000..5b4fdd1ea4 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + +namespace uavcan +{ + +class NodeStatusMonitor : protected Timer +{ +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. + + NodeStatus() + : status_code(protocol::NodeStatus::STATUS_OFFLINE) + , known(false) + { } + }; + + struct NodeStatusChangeEvent + { + NodeID node_id; + NodeStatus status; + NodeStatus old_status; + }; + +private: + enum { TimerPeriodMs100 = 5 }; + + typedef MethodBinder&)> + NodeStatusCallback; + + Subscriber sub_; + + struct Entry + { + NodeStatusCode status_code; + int8_t time_since_last_update_ms100; + Entry() + : status_code(protocol::NodeStatus::STATUS_OFFLINE) + , time_since_last_update_ms100(-1) + { } + }; + + mutable Entry entries_[NodeID::Max]; // [1, NodeID::Max] + + Entry& getEntry(NodeID node_id) const; + + void changeNodeStatus(const NodeID node_id, const Entry new_entry_value); + + void handleNodeStatus(const ReceivedDataStructure& msg); + + void handleTimerEvent(const TimerEvent&); + +protected: + /** + * Called when a node becomes online, changes status or goes offline. + */ + virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) + { + (void)event; + } + + /** + * Called for every received message uavcan.protocol.NodeStatus after handleNodeStatusChange(). + */ + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + (void)msg; + } + +public: + explicit NodeStatusMonitor(INode& node) + : Timer(node) + , sub_(node) + { } + + virtual ~NodeStatusMonitor() { } + + int start(); + + void forgetNode(NodeID node_id); + + NodeStatus getNodeStatus(NodeID node_id) const; + + NodeID findNodeWithWorstStatus() const; +}; + +} diff --git a/libuavcan/src/protocol/node_status_monitor.cpp b/libuavcan/src/protocol/node_status_monitor.cpp new file mode 100644 index 0000000000..c149f157fe --- /dev/null +++ b/libuavcan/src/protocol/node_status_monitor.cpp @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +NodeStatusMonitor::Entry& NodeStatusMonitor::getEntry(NodeID node_id) const +{ + if (node_id.get() < 1 || node_id.get() > NodeID::Max) + { + handleFatalError("NodeStatusMonitor NodeID out of range"); + } + 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&) +{ + enum { OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100 }; + + for (int i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + 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 += 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) + { + Timer::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 + { + assert(0); + } +} + +NodeStatusMonitor::NodeStatus NodeStatusMonitor::getNodeStatus(NodeID node_id) const +{ + if (!node_id.isValid()) + { + 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 (int i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + 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/test/protocol/node_status_monitor.cpp b/libuavcan/test/protocol/node_status_monitor.cpp new file mode 100644 index 0000000000..52aacc8168 --- /dev/null +++ b/libuavcan/test/protocol/node_status_monitor.cpp @@ -0,0 +1,197 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id, uavcan::uint8_t status_code, + uavcan::uint32_t uptime_sec, uavcan::TransferID tid) +{ + uavcan::StaticTransferBuffer<100> buffer; + uavcan::BitStream bitstream(buffer); + uavcan::ScalarCodec codec(bitstream); + + uavcan::protocol::NodeStatus msg; + msg.status_code = status_code; + msg.uptime_sec = uptime_sec; + + // Manual message publication + ASSERT_LT(0, uavcan::protocol::NodeStatus::encode(msg, codec)); + ASSERT_GE(7, buffer.getMaxWritePos()); + + // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(uavcan::protocol::NodeStatus::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + node_id, uavcan::NodeID::Broadcast, 0, tid, true); + + ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos())); + + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + + for (int i = 0; i < can.getNumIfaces(); i++) + { + can.ifaces.at(i).pushRx(can_frame); + } +} + + +static void shortSpin(TestNode& node) +{ + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(1))); +} + + +TEST(NodeStatusMonitor, Basic) +{ + using uavcan::protocol::NodeStatus; + using uavcan::NodeID; + + SystemClockMock clock_mock(100); + clock_mock.monotonic_auto_advance = 1000; + + CanDriverMock can(2, clock_mock); + + TestNode node(can, clock_mock, 64); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::NodeStatusMonitor nsm(node); + ASSERT_LE(0, nsm.start()); + + ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(1))); + + /* + * Empty NSM, no nodes were registered yet + */ + ASSERT_FALSE(nsm.findNodeWithWorstStatus().isValid()); + + uavcan::NodeStatusMonitor::NodeStatus st = nsm.getNodeStatus(uavcan::NodeID(123)); + ASSERT_FALSE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + + /* + * Some new status messages + */ + publishNodeStatus(can, 10, NodeStatus::STATUS_OK, 12, 0); + shortSpin(node); + ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstStatus()); + + publishNodeStatus(can, 9, NodeStatus::STATUS_INITIALIZING, 0, 0); + shortSpin(node); + ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus()); + + publishNodeStatus(can, 11, NodeStatus::STATUS_CRITICAL, 999, 0); + shortSpin(node); + ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstStatus()); + + st = nsm.getNodeStatus(uavcan::NodeID(10)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); + + st = nsm.getNodeStatus(uavcan::NodeID(9)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code); + + st = nsm.getNodeStatus(uavcan::NodeID(11)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code); + + /* + * Timeout + */ + std::cout << "Starting timeout test, current monotime is " << clock_mock.monotonic << std::endl; + + clock_mock.advance(1000000); + shortSpin(node); + st = nsm.getNodeStatus(uavcan::NodeID(10)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); + + clock_mock.advance(1000000); + shortSpin(node); + st = nsm.getNodeStatus(uavcan::NodeID(9)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code); + + clock_mock.advance(1000000); + shortSpin(node); + st = nsm.getNodeStatus(uavcan::NodeID(11)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code); + + /* + * Will timeout now + */ + clock_mock.advance(2000000); + shortSpin(node); + + st = nsm.getNodeStatus(uavcan::NodeID(10)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + + st = nsm.getNodeStatus(uavcan::NodeID(9)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + + st = nsm.getNodeStatus(uavcan::NodeID(11)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + + /* + * Recovering one node, adding two extra + */ + publishNodeStatus(can, 11, NodeStatus::STATUS_WARNING, 999, 0); + shortSpin(node); + + publishNodeStatus(can, 127, NodeStatus::STATUS_WARNING, 9999, 0); + shortSpin(node); + + publishNodeStatus(can, 1, NodeStatus::STATUS_OK, 1234, 0); + shortSpin(node); + + /* + * Making sure OFFLINE is still worst status + */ + ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus()); + + /* + * Final validation + */ + st = nsm.getNodeStatus(uavcan::NodeID(10)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + + st = nsm.getNodeStatus(uavcan::NodeID(9)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + + st = nsm.getNodeStatus(uavcan::NodeID(11)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code); + + st = nsm.getNodeStatus(uavcan::NodeID(127)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code); + + st = nsm.getNodeStatus(uavcan::NodeID(1)); + ASSERT_TRUE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); + + /* + * Forgetting + */ + nsm.forgetNode(127); + st = nsm.getNodeStatus(uavcan::NodeID(127)); + ASSERT_FALSE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); + + nsm.forgetNode(9); + st = nsm.getNodeStatus(uavcan::NodeID(9)); + ASSERT_FALSE(st.known); + ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); +}