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

NodeStatusMonitor update

Browse Source
This commit is contained in:
Pavel Kirienko
2015-07-08 23:24:59 +03:00
parent e98ba01e22
commit 2fa78ddda2
2 changed files with 168 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/protocol/node_status_monitor.hpp
+103 -40
View File
@@ -22,17 +22,36 @@ namespace uavcan
class UAVCAN_EXPORT NodeStatusMonitor class UAVCAN_EXPORT NodeStatusMonitor
{ {
public: public:
typedef typename StorageType<typename protocol::NodeStatus::FieldTypes::status_code>::Type NodeStatusCode;
struct NodeStatus struct NodeStatus
{ {
NodeStatusCode status_code; ///< Current status code; OFFLINE if not known. uint8_t health : 2;
bool known; ///< True if the node was online at least once. uint8_t mode : 3;
uint8_t sub_mode : 3;
NodeStatus() NodeStatus() :
: status_code(protocol::NodeStatus::STATUS_OFFLINE) health(protocol::NodeStatus::HEALTH_OK),
, known(false) mode(protocol::NodeStatus::MODE_OFFLINE),
{ } sub_mode(0)
{
StaticAssert<protocol::NodeStatus::FieldTypes::health::BitLen == 2>::check();
StaticAssert<protocol::NodeStatus::FieldTypes::mode::BitLen == 3>::check();
StaticAssert<protocol::NodeStatus::FieldTypes::sub_mode::BitLen == 3>::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 struct NodeStatusChangeEvent
@@ -40,6 +59,11 @@ public:
NodeID node_id; NodeID node_id;
NodeStatus status; NodeStatus status;
NodeStatus old_status; NodeStatus old_status;
bool was_known;
NodeStatusChangeEvent() :
was_known(false)
{ }
}; };
private: private:
@@ -57,11 +81,10 @@ private:
struct Entry struct Entry
{ {
NodeStatusCode status_code; NodeStatus status;
int8_t time_since_last_update_ms100; int8_t time_since_last_update_ms100;
Entry() Entry() :
: status_code(protocol::NodeStatus::STATUS_OFFLINE) time_since_last_update_ms100(-1)
, time_since_last_update_ms100(-1)
{ } { }
}; };
@@ -79,20 +102,18 @@ private:
void changeNodeStatus(const NodeID node_id, const Entry new_entry_value) void changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
{ {
Entry& entry = getEntry(node_id); Entry& entry = getEntry(node_id);
if (entry.status_code != new_entry_value.status_code) if (entry.status != new_entry_value.status)
{ {
NodeStatusChangeEvent event; 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; UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: [%s] --> [%s]", int(node_id.get()),
event.old_status.status_code = entry.status_code; (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); handleNodeStatusChange(event);
} }
entry = new_entry_value; entry = new_entry_value;
@@ -100,11 +121,13 @@ private:
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg) void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{ {
Entry new_entry_value; Entry new_entry;
new_entry_value.time_since_last_update_ms100 = 0; new_entry.time_since_last_update_ms100 = 0;
new_entry_value.status_code = msg.status_code; 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); handleNodeStatusMessage(msg);
} }
@@ -117,17 +140,19 @@ private:
{ {
const NodeID nid(i); const NodeID nid(i);
UAVCAN_ASSERT(nid.isUnicast()); UAVCAN_ASSERT(nid.isUnicast());
Entry& entry = getEntry(nid); Entry& entry = getEntry(nid);
if (entry.time_since_last_update_ms100 >= 0 && 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 = entry.time_since_last_update_ms100 =
int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100)); int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100) if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100)
{ {
Entry new_entry_value; Entry new_entry_value;
new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100; 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); changeNodeStatus(nid, new_entry_value);
} }
} }
@@ -209,6 +234,7 @@ public:
/** /**
* Returns status of a given node. * Returns status of a given node.
* Unknown nodes are considered offline. * Unknown nodes are considered offline.
* Complexity O(1).
*/ */
NodeStatus getNodeStatus(NodeID node_id) const NodeStatus getNodeStatus(NodeID node_id) const
{ {
@@ -217,25 +243,41 @@ public:
UAVCAN_ASSERT(0); UAVCAN_ASSERT(0);
return NodeStatus(); return NodeStatus();
} }
NodeStatus status;
const Entry& entry = getEntry(node_id); const Entry& entry = getEntry(node_id);
if (entry.time_since_last_update_ms100 >= 0) if (entry.time_since_last_update_ms100 >= 0)
{ {
status.known = true; return entry.status;
status.status_code = entry.status_code;
} }
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. * 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. * 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; NodeID nid_with_worst_health;
NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK; uint8_t worst_health = protocol::NodeStatus::HEALTH_OK;
for (uint8_t i = 1; i <= NodeID::Max; i++) for (uint8_t i = 1; i <= NodeID::Max; i++)
{ {
@@ -244,14 +286,35 @@ public:
const Entry& entry = getEntry(nid); const Entry& entry = getEntry(nid);
if (entry.time_since_last_update_ms100 >= 0) 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; nid_with_worst_health = nid;
worst_status_code = entry.status_code; 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 <typename Operator>
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);
}
}
} }
}; };
libuavcan/test/protocol/node_status_monitor.cpp
+65 -64
View File
@@ -7,11 +7,13 @@
#include <uavcan/protocol/node_status_provider.hpp> #include <uavcan/protocol/node_status_provider.hpp>
#include "helpers.hpp" #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::uint32_t uptime_sec, uavcan::TransferID tid)
{ {
uavcan::protocol::NodeStatus msg; uavcan::protocol::NodeStatus msg;
msg.status_code = status_code; msg.health = health;
msg.mode = mode;
msg.uptime_sec = uptime_sec; msg.uptime_sec = uptime_sec;
emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid); emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
} }
@@ -46,38 +48,37 @@ TEST(NodeStatusMonitor, Basic)
/* /*
* Empty NSM, no nodes were registered yet * 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(nsm.isNodeKnown(uavcan::NodeID(123)));
ASSERT_FALSE(st.known); ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(123)).mode);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
/* /*
* Some new status messages * 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); 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); 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); shortSpin(node);
ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstStatus()); ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstHealth());
st = nsm.getNodeStatus(uavcan::NodeID(10)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
st = nsm.getNodeStatus(uavcan::NodeID(9)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
st = nsm.getNodeStatus(uavcan::NodeID(11)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
/* /*
* Timeout * Timeout
@@ -86,21 +87,21 @@ TEST(NodeStatusMonitor, Basic)
clock_mock.advance(500000); clock_mock.advance(500000);
shortSpin(node); shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(10)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
clock_mock.advance(500000); clock_mock.advance(500000);
shortSpin(node); shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(9)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
clock_mock.advance(500000); clock_mock.advance(500000);
shortSpin(node); shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(11)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
/* /*
* Will timeout now * Will timeout now
@@ -108,68 +109,68 @@ TEST(NodeStatusMonitor, Basic)
clock_mock.advance(4000000); clock_mock.advance(4000000);
shortSpin(node); shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(10)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
st = nsm.getNodeStatus(uavcan::NodeID(9)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
st = nsm.getNodeStatus(uavcan::NodeID(11)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
/* /*
* Recovering one node, adding two extra * 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); shortSpin(node);
publishNodeStatus(can, 127, NodeStatus::STATUS_WARNING, 9999, 1); publishNodeStatus(can, 127, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 9999, 1);
shortSpin(node); shortSpin(node);
publishNodeStatus(can, 1, NodeStatus::STATUS_OK, 1234, 1); publishNodeStatus(can, 1, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 1234, 1);
shortSpin(node); shortSpin(node);
/* /*
* Making sure OFFLINE is still worst status * Making sure OFFLINE is still worst status
*/ */
ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus()); ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth());
/* /*
* Final validation * Final validation
*/ */
st = nsm.getNodeStatus(uavcan::NodeID(10)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
st = nsm.getNodeStatus(uavcan::NodeID(9)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
st = nsm.getNodeStatus(uavcan::NodeID(11)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(11)).health);
st = nsm.getNodeStatus(uavcan::NodeID(127)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(127)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(127)).mode);
ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(127)).health);
st = nsm.getNodeStatus(uavcan::NodeID(1)); ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(1)));
ASSERT_TRUE(st.known); ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(1)).mode);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(1)).health);
/* /*
* Forgetting * Forgetting
*/ */
nsm.forgetNode(127); nsm.forgetNode(127);
st = nsm.getNodeStatus(uavcan::NodeID(127)); ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(127)));
ASSERT_FALSE(st.known); ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(127)).mode);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(127)).health);
nsm.forgetNode(9); nsm.forgetNode(9);
st = nsm.getNodeStatus(uavcan::NodeID(9)); ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_FALSE(st.known); ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code); ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(9)).health);
} }