fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Vendor-specific status code support

Browse Source
This commit is contained in:
Pavel Kirienko
2015-03-20 00:24:53 +03:00
parent 3b3c0d3b9e
commit a63c9510fc
5 changed files with 50 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp
+24 -9
View File
@@ -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);