From a63c9510fc6d3c4a40a64bba379227dc784ec988 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 20 Mar 2015 00:24:53 +0300 Subject: [PATCH] Vendor-specific status code support --- libuavcan/include/uavcan/node/node.hpp | 8 +++++ .../uavcan/protocol/node_status_provider.hpp | 12 +++++++ .../src/protocol/uc_node_status_provider.cpp | 5 +++ libuavcan_drivers/linux/apps/test_node.cpp | 1 + .../linux/apps/uavcan_status_monitor.cpp | 33 ++++++++++++++----- 5 files changed, 50 insertions(+), 9 deletions(-) diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index c04bf1103b..a3e88ff829 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -180,6 +180,14 @@ public: (void)proto_nsp_.forcePublish(); } + /** + * Updates the vendor-specific status code. + */ + void setVendorSpecificStatusCode(NodeStatusProvider::VendorSpecificStatusCode code) + { + proto_nsp_.setVendorSpecificStatusCode(code); + } + /** * Sets the node version information. */ diff --git a/libuavcan/include/uavcan/protocol/node_status_provider.hpp b/libuavcan/include/uavcan/protocol/node_status_provider.hpp index 524dd43c35..ced87a08aa 100644 --- a/libuavcan/include/uavcan/protocol/node_status_provider.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_provider.hpp @@ -49,6 +49,9 @@ class UAVCAN_EXPORT NodeStatusProvider : private TimerBase void handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&, protocol::GetNodeInfo::Response& rsp); public: + typedef typename StorageType::Type + VendorSpecificStatusCode; + explicit NodeStatusProvider(INode& node) : TimerBase(node) , creation_timestamp_(node.getMonotonicTime()) @@ -92,6 +95,15 @@ public: void setStatusCritical() { setStatusCode(protocol::NodeStatus::STATUS_CRITICAL); } void setStatusOffline() { setStatusCode(protocol::NodeStatus::STATUS_OFFLINE); } + /** + * Local node vendor-specific status code control. + */ + void setVendorSpecificStatusCode(VendorSpecificStatusCode code); + VendorSpecificStatusCode getVendorSpecificStatusCode() const + { + return node_info_.status.vendor_specific_status_code; + } + /** * Local node name control. * Can be set only once before the provider is started. diff --git a/libuavcan/src/protocol/uc_node_status_provider.cpp b/libuavcan/src/protocol/uc_node_status_provider.cpp index 7a6425684e..ebff29c533 100644 --- a/libuavcan/src/protocol/uc_node_status_provider.cpp +++ b/libuavcan/src/protocol/uc_node_status_provider.cpp @@ -130,6 +130,11 @@ void NodeStatusProvider::setStatusCode(uint8_t code) node_info_.status.status_code = code; } +void NodeStatusProvider::setVendorSpecificStatusCode(VendorSpecificStatusCode code) +{ + node_info_.status.vendor_specific_status_code = code; +} + void NodeStatusProvider::setName(const char* name) { if ((name != NULL) && (*name != '\0') && (node_info_.name.empty())) diff --git a/libuavcan_drivers/linux/apps/test_node.cpp b/libuavcan_drivers/linux/apps/test_node.cpp index 2241f246ca..4bd4eb872f 100644 --- a/libuavcan_drivers/linux/apps/test_node.cpp +++ b/libuavcan_drivers/linux/apps/test_node.cpp @@ -84,6 +84,7 @@ static void runForever(const uavcan_linux::NodePtr& node) auto do_nothing_once_a_minute = [&node](const uavcan::TimerEvent&) { node->logInfo("timer", "Another minute passed..."); + node->setVendorSpecificStatusCode(static_cast(std::rand())); // Setting to an arbitrary value }; auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(60000), do_nothing_once_a_minute); diff --git a/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp b/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp index c8374ded0d..540c68dce6 100644 --- a/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -18,6 +19,7 @@ struct OstreamColorizer Yellow = 33, Blue = 34, Magenta = 35, + Cyan = 36, Default = 39 }; explicit OstreamColorizer(Color color = Default) : color_(color) { } @@ -33,11 +35,11 @@ private: class Monitor : public uavcan::NodeStatusMonitor { uavcan_linux::TimerPtr timer_; - std::unordered_map uptimes_; + std::unordered_map status_registry_; virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure& msg) { - uptimes_[msg.getSrcNodeID().get()] = msg.uptime_sec; + status_registry_[msg.getSrcNodeID().get()] = msg; } static std::pair @@ -49,7 +51,7 @@ class Monitor : public uavcan::NodeStatusMonitor } if (status_code == uavcan::protocol::NodeStatus::STATUS_INITIALIZING) { - return { OstreamColorizer(OstreamColorizer::Blue), "INITIALIZING" }; + return { OstreamColorizer(OstreamColorizer::Cyan), "INITIALIZING" }; } if (status_code == uavcan::protocol::NodeStatus::STATUS_WARNING) { @@ -68,20 +70,33 @@ class Monitor : public uavcan::NodeStatusMonitor void printStatusLine(uavcan::NodeID nid, const uavcan::NodeStatusMonitor::NodeStatus& status) { + const auto original_flags = std::cout.flags(); + const auto color_and_string = statusCodeToColoredString(status.status_code); const int nid_int = nid.get(); + const auto uptime = status_registry_[nid_int].uptime_sec; + const int vendor_code = status_registry_[nid_int].vendor_specific_status_code; + std::cout << color_and_string.first; - std::cout << " " << std::setw(3) << std::left << nid_int << " | " - << std::setw(13) << std::left << color_and_string.second << " | " - << uptimes_[nid_int]; - std::cout << OstreamColorizer() << "\n"; + + std::cout << " " << std::setw(3) << std::left << nid_int << " | " // Node ID + << std::setw(13) << std::left << color_and_string.second << " | " // Status name + << std::setw(12) << uptime << " | " // Uptime + << "0x" << std::hex << std::setw(4) << std::setfill('0') << vendor_code // Vendor, hex + << " 0b" << std::dec << std::bitset<8>((vendor_code >> 8) & 0xFF) // Vendor, bin, high + << "'" << std::bitset<8>(vendor_code & 0xFF) // Vendor, bin, low + << " " << vendor_code; // Vendor, dec + + std::cout << OstreamColorizer() << std::setfill(' ') << "\n"; + std::cout.width(0); + std::cout.flags(original_flags); } void redraw(const uavcan::TimerEvent&) { std::cout << "\x1b\x5b\x48" << "\x1b\x5b\x32\x4a" - << " NID | Status | Uptime\n" - << "-----+---------------+--------\n"; + << " NID | Status | Uptime (sec) | Vendor-specific status (hex/bin/dec)\n" + << "-----+---------------+--------------+--------------------------------------\n"; for (unsigned i = 1; i <= uavcan::NodeID::Max; i++) { const auto s = getNodeStatus(i);