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

Improved NodeDiscoverer logic

Browse Source
This commit is contained in:
Pavel Kirienko
2015-05-13 23:23:03 +03:00
parent e2ef4a4518
commit 3db54cd6af
2 changed files with 30 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp
+27 -27
View File
@@ -86,6 +86,27 @@ class NodeDiscoverer : TimerBase
typedef Map<NodeID, NodeData, 10> NodeMap;
class HighestUptimeSearcher : ::uavcan::Noncopyable
{
uint32_t highest_uptime_sec_;
NodeID node_id_;
public:
HighestUptimeSearcher() : highest_uptime_sec_(0) { }
bool operator()(const NodeID& key, const NodeData& value)
{
if (value.last_seen_uptime >= highest_uptime_sec_)
{
highest_uptime_sec_ = value.last_seen_uptime;
node_id_ = key;
}
return false;
}
NodeID getNodeWithHighestUptime() const { return node_id_; }
};
/**
* When this number of attempts has been made, the discoverer will give up and assume that the node
* does not implement this service.
@@ -121,34 +142,13 @@ class NodeDiscoverer : TimerBase
NodeID pickNextNodeToQuery() const
{
const unsigned node_map_size = node_map_.getSize();
HighestUptimeSearcher searcher;
// Searching the lowest number of attempts made
uint8_t lowest_number_of_attempts = static_cast<uint8_t>(MaxAttemptsToGetNodeInfo + 1U);
for (unsigned i = 0; i < node_map_size; i++)
{
const NodeMap::KVPair* const kv = node_map_.getByIndex(i);
UAVCAN_ASSERT(kv != NULL);
lowest_number_of_attempts = min(lowest_number_of_attempts, kv->value.num_get_node_info_attempts);
}
const NodeID* const out = node_map_.findFirstKey<HighestUptimeSearcher&>(searcher);
(void)out;
UAVCAN_ASSERT(out == NULL);
// Now, among nodes with this number of attempts selecting the one with highest uptime.
NodeID output;
uint32_t largest_uptime = 0;
for (unsigned i = 0; i < node_map_size; i++)
{
const NodeMap::KVPair* const kv = node_map_.getByIndex(i);
UAVCAN_ASSERT(kv != NULL);
if ((kv->value.num_get_node_info_attempts == lowest_number_of_attempts) &&
(kv->value.last_seen_uptime >= largest_uptime))
{
largest_uptime = kv->value.last_seen_uptime;
output = kv->key;
}
}
// An invalid node ID will be returned only if there's no nodes at all.
return output;
return searcher.getNodeWithHighestUptime();
}
bool needToQuery(NodeID node_id)
@@ -319,7 +319,7 @@ class NodeDiscoverer : TimerBase
if (!isRunning())
{
startPeriodic(get_node_info_client_.getRequestTimeout() * 2);
startPeriodic(get_node_info_client_.getRequestTimeout());
trace(TraceDiscoveryTimerStart, getPeriod().toUSec());
}
}
libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp
+3 -3
View File
@@ -134,7 +134,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
*/
handler.can_discover = true;
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1400));
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
@@ -149,7 +149,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
node_status.uptime_sec += 5U;
ASSERT_LE(0, node_status_pub.broadcast(node_status));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1400));
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
@@ -167,7 +167,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
get_node_info_server.response.hardware_version.unique_id[14] = 52;
ASSERT_LE(0, get_node_info_server.start());
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900));
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1400));
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));