Vendor-specific status code support

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.
      */

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<typename protocol::NodeStatus::FieldTypes::vendor_specific_status_code>::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.

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()))

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::uint16_t>(std::rand())); // Setting to an arbitrary value
     };
     auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(60000), do_nothing_once_a_minute);
 

libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp

@@ -4,6 +4,7 @@
 
 #include <iostream>
 #include <iomanip>
+#include <bitset>
 #include <unordered_map>
 #include <uavcan_linux/uavcan_linux.hpp>
 #include <uavcan/protocol/node_status_monitor.hpp>
@@ -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<int, std::uint64_t> uptimes_;
+    std::unordered_map<int, uavcan::protocol::NodeStatus> status_registry_;
 
     virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg)
     {
-        uptimes_[msg.getSrcNodeID().get()] = msg.uptime_sec;
+        status_registry_[msg.getSrcNodeID().get()] = msg;
     }
 
     static std::pair<OstreamColorizer, std::string>
@@ -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);