fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
libuavcan/include/uavcan/protocol/node_status_monitor.hpp
View File

@ -22,17 +22,36 @@ namespace uavcan
class UAVCAN_EXPORT NodeStatusMonitor
{
public:
typedef typename StorageType<typename protocol::NodeStatus::FieldTypes::status_code>::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.
uint8_t health : 2;
uint8_t mode : 3;
uint8_t sub_mode : 3;
NodeStatus()
: status_code(protocol::NodeStatus::STATUS_OFFLINE)
, known(false)
{ }
NodeStatus() :
health(protocol::NodeStatus::HEALTH_OK),
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
@ -40,6 +59,11 @@ public:
NodeID node_id;
NodeStatus status;
NodeStatus old_status;
bool was_known;
NodeStatusChangeEvent() :
was_known(false)
{ }
};
private:
@ -57,11 +81,10 @@ private:
struct Entry
{
NodeStatusCode status_code;
NodeStatus status;
int8_t time_since_last_update_ms100;
Entry()
: status_code(protocol::NodeStatus::STATUS_OFFLINE)
, time_since_last_update_ms100(-1)
Entry() :
time_since_last_update_ms100(-1)
{ }
};
@ -79,20 +102,18 @@ private:
void changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
{
Entry& entry = getEntry(node_id);
if (entry.status_code != new_entry_value.status_code)
if (entry.status != new_entry_value.status)
{
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;
event.old_status.status_code = entry.status_code;
UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: [%s] --> [%s]", int(node_id.get()),
(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);
}
entry = new_entry_value;
@ -100,11 +121,13 @@ private:
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
Entry new_entry_value;
new_entry_value.time_since_last_update_ms100 = 0;
new_entry_value.status_code = msg.status_code;
Entry new_entry;
new_entry.time_since_last_update_ms100 = 0;
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);
}
@ -117,17 +140,19 @@ private:
{
const NodeID nid(i);
UAVCAN_ASSERT(nid.isUnicast());
Entry& entry = getEntry(nid);
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 =
int8_t(entry.time_since_last_update_ms100 + int8_t(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;
new_entry_value.status = NodeStatus();
changeNodeStatus(nid, new_entry_value);
}
}
@ -209,6 +234,7 @@ public:
/**
* Returns status of a given node.
* Unknown nodes are considered offline.
* Complexity O(1).
*/
NodeStatus getNodeStatus(NodeID node_id) const
{
@ -217,25 +243,41 @@ public:
UAVCAN_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 entry.status;
}
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.
* 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.
*/
NodeID findNodeWithWorstStatus() const
NodeID findNodeWithWorstHealth() const
{
NodeID nid_with_worst_status;
NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK;
NodeID nid_with_worst_health;
uint8_t worst_health = protocol::NodeStatus::HEALTH_OK;
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
@ -244,14 +286,35 @@ public:
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())
if (entry.status.health > worst_health || !nid_with_worst_health.isValid())
{
nid_with_worst_status = nid;
worst_status_code = entry.status_code;
nid_with_worst_health = nid;
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);
}
}
}
};

129
libuavcan/test/protocol/node_status_monitor.cpp
View File

@ -7,11 +7,13 @@
#include <uavcan/protocol/node_status_provider.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::protocol::NodeStatus msg;
msg.status_code = status_code;
msg.health = health;
msg.mode = mode;
msg.uptime_sec = uptime_sec;
emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
}
@ -46,38 +48,37 @@ TEST(NodeStatusMonitor, Basic)
/*
* 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(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(123)));
ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(123)).mode);
/*
* 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);
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);
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);
ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstStatus());
ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstHealth());
st = nsm.getNodeStatus(uavcan::NodeID(10));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
st = nsm.getNodeStatus(uavcan::NodeID(11));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
/*
* Timeout
@ -86,21 +87,21 @@ TEST(NodeStatusMonitor, Basic)
clock_mock.advance(500000);
shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(10));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
clock_mock.advance(500000);
shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
clock_mock.advance(500000);
shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(11));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
/*
* Will timeout now
@ -108,68 +109,68 @@ TEST(NodeStatusMonitor, Basic)
clock_mock.advance(4000000);
shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(10));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
st = nsm.getNodeStatus(uavcan::NodeID(11));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
/*
* 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);
publishNodeStatus(can, 127, NodeStatus::STATUS_WARNING, 9999, 1);
publishNodeStatus(can, 127, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 9999, 1);
shortSpin(node);
publishNodeStatus(can, 1, NodeStatus::STATUS_OK, 1234, 1);
publishNodeStatus(can, 1, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 1234, 1);
shortSpin(node);
/*
* Making sure OFFLINE is still worst status
*/
ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus());
ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth());
/*
* Final validation
*/
st = nsm.getNodeStatus(uavcan::NodeID(10));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
st = nsm.getNodeStatus(uavcan::NodeID(11));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(11)).health);
st = nsm.getNodeStatus(uavcan::NodeID(127));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(127)));
ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(127)).mode);
ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(127)).health);
st = nsm.getNodeStatus(uavcan::NodeID(1));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(1)));
ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(1)).mode);
ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(1)).health);
/*
* Forgetting
*/
nsm.forgetNode(127);
st = nsm.getNodeStatus(uavcan::NodeID(127));
ASSERT_FALSE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(127)));
ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(127)).mode);
ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(127)).health);
nsm.forgetNode(9);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_FALSE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(9)));
ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(9)).health);
}