void handleTimerEvent(const TimerEvent&)
{
if (get_node_info_client_.hasPendingCalls())
{
return;
}
const NodeID node_id = pickNextNodeToQueryAndCleanupMap();
if (!node_id.isUnicast())
{
trace(TraceDiscoveryTimerStop, 0);
stop();
return;
}
if (!handler_.canDiscoverNewNodes())
{
return; // Timer must continue to run in order to not stuck when it unlocks
}
trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
int(node_id.get()));
const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request());
if (res < 0)
{
getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call");
}
}
/**
* Invokes storage IO.
*/
int setVotedFor(NodeID node_id)
{
if (!node_id.isValid())
{
UAVCAN_ASSERT(0);
return -ErrInvalidParam;
}
tracer_.onEvent(TraceRaftVotedForUpdate, node_id.get());
StorageMarshaller io(storage_);
uint32_t tmp = node_id.get();
int res = io.setAndGetBack(getVotedForKey(), tmp);
if (res < 0)
{
return res;
}
if (node_id.get() != tmp)
{
return -ErrFailure;
}
voted_for_ = node_id;
return 0;
}
bool operator()(const NodeID node_id, const FirmwareFilePath&)
{
if (node_id.get() > owner.last_queried_node_id_ &&
!owner.begin_fw_update_client_.hasPendingCallToServer(node_id))
{
output = min(output, node_id.get());
}
return false;
}
bool needToQuery(NodeID node_id)
{
UAVCAN_ASSERT(node_id.isUnicast());
/*
* Fast check
*/
if (committed_node_mask_[node_id.get()])
{
return false;
}
/*
* Slow check - may involve full log search
*/
const INodeDiscoveryHandler::NodeAwareness awareness = handler_.checkNodeAwareness(node_id);
if (awareness == INodeDiscoveryHandler::NodeAwarenessUnknown)
{
return true;
}
else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownButNotCommitted)
{
removeNode(node_id);
return false;
}
else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted)
{
trace(TraceDiscoveryCommitCacheUpdated, node_id.get());
committed_node_mask_[node_id.get()] = true;
removeNode(node_id);
return false;
}
else
{
UAVCAN_ASSERT(0);
return false;
}
}
int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id)
{
Allocation msg;
msg.unique_id.resize(msg.unique_id.capacity());
copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin());
msg.node_id = allocated_node_id.get();
trace(TraceAllocationResponse, msg.node_id);
return allocation_pub_.broadcast(msg);
}
void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
{
trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == NULL) ? 0U : 0x100U));
removeNode(node_id);
/*
* It is paramount to check if the server is still interested to receive this data.
* Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with
* duplicate node ID in the log.
*/
if (needToQuery(node_id))
{
handler_.handleNewNodeDiscovery(unique_id_or_null, node_id);
}
}
virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info)
{
FirmwareFilePath firmware_file_path;
const bool update_needed = checker_.shouldRequestFirmwareUpdate(node_id, node_info, firmware_file_path);
if (update_needed)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires update; file path: %s",
int(node_id.get()), firmware_file_path.c_str());
trySetPendingNode(node_id, firmware_file_path);
}
else
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get()));
pending_nodes_.remove(node_id);
}
}
virtual void handleTimerEvent(const TimerEvent&)
{
if (pending_nodes_.isEmpty())
{
TimerBase::stop();
UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer stopped");
return;
}
const NodeID node_id = pickNextNodeID();
if (!node_id.isUnicast())
{
return;
}
FirmwareFilePath* const path = pending_nodes_.access(node_id);
if (path == UAVCAN_NULLPTR)
{
UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map
return;
}
protocol::file::BeginFirmwareUpdate::Request req;
req.source_node_id = getNode().getNodeID().get();
if (!common_path_prefix_.empty())
{
req.image_file_remote_path.path += common_path_prefix_.c_str();
req.image_file_remote_path.path.push_back(protocol::file::Path::SEPARATOR);
}
req.image_file_remote_path.path += path->c_str();
UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d with path: %s",
int(node_id.get()), req.image_file_remote_path.path.c_str());
const int call_res = begin_fw_update_client_.call(node_id, req);
if (call_res < 0)
{
getNode().registerInternalFailure("FirmwareUpdateTrigger call");
}
}
/*
* Methods of INodeInfoListener
*/
virtual void handleNodeInfoUnavailable(NodeID node_id)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d could not provide GetNodeInfo response", int(node_id.get()));
pending_nodes_.remove(node_id); // For extra paranoia
}
void removeNode(const NodeID node_id)
{
node_map_.remove(node_id);
trace(TraceDiscoveryNodeRemoved, node_id.get());
}
/**
* Initialization is performed as follows (every step may fail and return an error):
* 1. Log is restored or initialized.
* 2. Current term is restored. If there was no current term stored and the log is empty, it will be initialized
* with zero.
* 3. VotedFor value is restored. If there was no VotedFor value stored, the log is empty, and the current term is
* zero, the value will be initialized with zero.
*/
int init()
{
/*
* Reading log
*/
int res = log_.init();
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", "Log init failed: %d", res);
return res;
}
const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex());
if (last_entry == UAVCAN_NULLPTR)
{
UAVCAN_ASSERT(0);
return -ErrLogic;
}
const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0);
StorageMarshaller io(storage_);
/*
* Reading currentTerm
*/
if (storage_.get(getCurrentTermKey()).empty() && log_is_empty)
{
// First initialization
current_term_ = 0;
res = io.setAndGetBack(getCurrentTermKey(), current_term_);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to init current term: %d", res);
return res;
}
if (current_term_ != 0)
{
return -ErrFailure;
}
}
else
{
// Restoring
res = io.get(getCurrentTermKey(), current_term_);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to read current term: %d", res);
return res;
}
}
tracer_.onEvent(TraceRaftCurrentTermRestored, current_term_);
if (current_term_ < last_entry->term)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)",
unsigned(current_term_), unsigned(last_entry->term));
return -ErrLogic;
}
/*
* Reading votedFor
*/
if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0))
{
// First initialization
voted_for_ = NodeID(0);
uint32_t stored_voted_for = 0;
res = io.setAndGetBack(getVotedForKey(), stored_voted_for);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to init votedFor: %d", res);
return res;
}
if (stored_voted_for != 0)
{
return -ErrFailure;
}
}
else
{
// Restoring
uint32_t stored_voted_for = 0;
res = io.get(getVotedForKey(), stored_voted_for);
if (res < 0)
{
UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState",
"Failed to read votedFor: %d", res);
return res;
}
if (stored_voted_for > NodeID::Max)
{
return -ErrInvalidConfiguration;
}
voted_for_ = NodeID(uint8_t(stored_voted_for));
}
tracer_.onEvent(TraceRaftVotedForRestored, voted_for_.get());
return 0;
}
void changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
{
Entry& entry = getEntry(node_id);
if (entry.status != new_entry_value.status)
{
NodeStatusChangeEvent event;
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;
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());
handleNodeStatusChange(event);
}
entry = new_entry_value;
}