void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
if (!needToQuery(msg.getSrcNodeID()))
{
return;
}
NodeData* data = node_map_.access(msg.getSrcNodeID());
if (data == NULL)
{
trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get());
data = node_map_.insert(msg.getSrcNodeID(), NodeData());
if (data == NULL)
{
getNode().registerInternalFailure("NodeDiscoverer OOM");
return;
}
}
UAVCAN_ASSERT(data != NULL);
if (msg.uptime_sec < data->last_seen_uptime)
{
trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get());
data->num_get_node_info_attempts = 0;
}
data->last_seen_uptime = msg.uptime_sec;
if (!isRunning())
{
startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs));
trace(TraceDiscoveryTimerStart, getPeriod().toUSec());
}
}
void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
{
if (result.isSuccessful())
{
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d",
int(result.getCallID().server_node_id.get()));
finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id);
}
else
{
trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get());
NodeData* const data = node_map_.access(result.getCallID().server_node_id);
if (data == NULL)
{
return; // Probably it is a known node now
}
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer",
"GetNodeInfo request to %d has timed out, %d attempts",
int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts));
data->num_get_node_info_attempts++;
if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo)
{
finalizeNodeDiscovery(NULL, result.getCallID().server_node_id);
// Warning: data pointer is invalidated now
}
}
}
