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

Node status monitor moved to header

Browse Source
This commit is contained in:
Pavel Kirienko 2015-05-09 12:24:31 +03:00
parent 894d951c33
commit 00ec7186b0
2 changed files with 124 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -5,10 +5,13 @@
#ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
#define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
#include <uavcan/debug.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/protocol/NodeStatus.hpp>
#include <cassert>
#include <cstdlib>
namespace uavcan
{
@ -40,7 +43,7 @@ public:
};
private:
static const unsigned TimerPeriodMs100 = 5;
enum { TimerPeriodMs100 = 5 };
typedef MethodBinder<NodeStatusMonitor*,
void (NodeStatusMonitor::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
@ -65,13 +68,71 @@ private:
mutable Entry entries_[NodeID::Max]; // [1, NodeID::Max]
Entry& getEntry(NodeID node_id) const;
Entry& getEntry(NodeID node_id) const
{
if (node_id.get() < 1 || node_id.get() > NodeID::Max)
{
handleFatalError("NodeStatusMonitor NodeID");
}
return entries_[node_id.get() - 1];
}
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);
if (entry.status_code != new_entry_value.status_code)
{
NodeStatusChangeEvent event;
event.node_id = node_id;
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg);
event.old_status.known = entry.time_since_last_update_ms100 >= 0;
event.old_status.status_code = entry.status_code;
virtual void handleTimerEvent(const TimerEvent&);
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;
}
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;
changeNodeStatus(msg.getSrcNodeID(), new_entry_value);
handleNodeStatusMessage(msg);
}
virtual void handleTimerEvent(const TimerEvent&)
{
const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
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.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;
changeNodeStatus(nid, new_entry_value);
}
}
}
}
protected:
/**
@ -107,25 +168,79 @@ public:
* Destroy the object to stop it.
* Returns negative error code.
*/
int start();
int start()
{
const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
if (res >= 0)
{
TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
}
return res;
}
/**
* Make the node unknown.
*/
void forgetNode(NodeID node_id);
void forgetNode(NodeID node_id)
{
if (node_id.isValid())
{
Entry& entry = getEntry(node_id);
entry = Entry();
}
else
{
UAVCAN_ASSERT(0);
}
}
/**
* Returns status of a given node.
* Unknown nodes are considered offline.
*/
NodeStatus getNodeStatus(NodeID node_id) const;
NodeStatus getNodeStatus(NodeID node_id) const
{
if (!node_id.isValid())
{
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 status;
}
/**
* This helper method allows to quickly estimate the overall network health.
* Status 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 findNodeWithWorstStatus() const
{
NodeID nid_with_worst_status;
NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK;
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)
{
if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid())
{
nid_with_worst_status = nid;
worst_status_code = entry.status_code;
}
}
}
return nid_with_worst_status;
}
};
}

143
libuavcan/src/protocol/uc_node_status_monitor.cpp
View File

@ -1,143 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan/debug.hpp>
#include <cassert>
#include <cstdlib>
namespace uavcan
{
const unsigned NodeStatusMonitor::TimerPeriodMs100;
NodeStatusMonitor::Entry& NodeStatusMonitor::getEntry(NodeID node_id) const
{
if (node_id.get() < 1 || node_id.get() > NodeID::Max)
{
handleFatalError("NodeStatusMonitor NodeID");
}
return entries_[node_id.get() - 1];
}
void NodeStatusMonitor::changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
{
Entry& entry = getEntry(node_id);
if (entry.status_code != new_entry_value.status_code)
{
NodeStatusChangeEvent event;
event.node_id = node_id;
event.old_status.known = entry.time_since_last_update_ms100 >= 0;
event.old_status.status_code = entry.status_code;
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;
}
void NodeStatusMonitor::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;
changeNodeStatus(msg.getSrcNodeID(), new_entry_value);
handleNodeStatusMessage(msg);
}
void NodeStatusMonitor::handleTimerEvent(const TimerEvent&)
{
const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
for (uint8_t i = 1; i <= NodeID::Max; i++)
{
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.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;
changeNodeStatus(nid, new_entry_value);
}
}
}
}
int NodeStatusMonitor::start()
{
const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
if (res >= 0)
{
TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
}
return res;
}
void NodeStatusMonitor::forgetNode(NodeID node_id)
{
if (node_id.isValid())
{
Entry& entry = getEntry(node_id);
entry = Entry();
}
else
{
UAVCAN_ASSERT(0);
}
}
NodeStatusMonitor::NodeStatus NodeStatusMonitor::getNodeStatus(NodeID node_id) const
{
if (!node_id.isValid())
{
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 status;
}
NodeID NodeStatusMonitor::findNodeWithWorstStatus() const
{
NodeID nid_with_worst_status;
NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK;
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)
{
if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid())
{
nid_with_worst_status = nid;
worst_status_code = entry.status_code;
}
}
}
return nid_with_worst_status;
}
}