virtual void handleReceivedDataStruct(ReceivedDataStructure<RequestType>& request)
{
UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest);
ServiceResponseDataStructure<ResponseType> response;
if (coerceOrFallback<bool>(callback_, true))
{
UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default
callback_(request, response);
}
else
{
handleFatalError("Srv serv clbk");
}
if (response.isResponseEnabled())
{
publisher_.setPriority(request.getPriority()); // Responding at the same priority.
const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(),
request.getTransferID());
if (res < 0)
{
UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res);
publisher_.getNode().getDispatcher().getTransferPerfCounter().addError();
response_failure_count_++;
}
}
else
{
UAVCAN_TRACE("ServiceServer", "Response was suppressed by the application");
}
}
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 handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
if (msg.getTransferType() == TransferTypeMessageBroadcast)
{
processMsg(msg);
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType()));
}
}
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
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);
handleNodeStatusMessage(msg);
}
ReceivedDataStructureCopy(const ReceivedDataStructure& s) :
ts_monotonic(s.getMonotonicTimestamp()),
ts_utc(s.getUtcTimestamp()),
transfer_type(s.getTransferType()),
transfer_id(s.getTransferID()),
src_node_id(s.getSrcNodeID()),
iface_index(s.getIfaceIndex()),
msg(s)
{ }
virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
{
Entry& entry = getEntry(msg.getSrcNodeID());
if (msg.uptime_sec < entry.uptime_sec)
{
entry.request_needed = true;
entry.num_attempts_made = 0;
startTimerIfNotRunning();
}
entry.uptime_sec = msg.uptime_sec;
entry.updated_since_last_attempt = true;
listeners_.forEach(GenericHandlerCaller<const ReceivedDataStructure<protocol::NodeStatus>&>(
&INodeInfoListener::handleNodeStatusMessage, msg));
}
void processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_;
UAVCAN_ASSERT(!since_prev_msg.isNegative());
const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero();
const bool switch_master = msg.getSrcNodeID() < master_nid_;
// TODO: Make configurable
const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS;
if (switch_master || pub_timeout || needs_init)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i",
int(needs_init), int(switch_master), int(pub_timeout));
updateFromMsg(msg);
}
else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_)
{
if (state_ == Adjust)
{
const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0;
const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1;
const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS;
if (msg_invalid || wrong_tid || wrong_timing)
{
UAVCAN_TRACE("GlobalTimeSyncSlave",
"Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i",
int(msg_invalid), int(wrong_tid), int(wrong_timing));
state_ = Update;
}
}
if (state_ == Adjust)
{
adjustFromMsg(msg);
}
else
{
updateFromMsg(msg);
}
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i",
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
}
}
void handleAllocation(const ReceivedDataStructure<Allocation>& msg)
{
trace(TraceAllocationActivity, msg.getSrcNodeID().get());
if (!msg.isAnonymousTransfer())
{
return; // This is a response from another allocator, ignore
}
/*
* Reset the expected stage on timeout
*/
if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_))
{
UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset");
current_unique_id_.clear();
trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec());
}
/*
* Checking if request stage matches the expected stage
*/
const uint8_t request_stage = detectRequestStage(msg);
if (request_stage == InvalidStage)
{
trace(TraceAllocationBadRequest, msg.unique_id.size());
return; // Malformed request - ignore without resetting
}
const uint8_t expected_stage = getExpectedStage();
if (expected_stage == InvalidStage)
{
UAVCAN_ASSERT(0);
return;
}
if (request_stage != expected_stage)
{
trace(TraceAllocationUnexpectedStage, request_stage);
return; // Ignore - stage mismatch
}
const uint8_t max_expected_bytes =
static_cast<uint8_t>(current_unique_id_.capacity() - current_unique_id_.size());
UAVCAN_ASSERT(max_expected_bytes > 0);
if (msg.unique_id.size() > max_expected_bytes)
{
trace(TraceAllocationBadRequest, msg.unique_id.size());
return; // Malformed request
}
/*
* Updating the local state
*/
for (uint8_t i = 0; i < msg.unique_id.size(); i++)
{
current_unique_id_.push_back(msg.unique_id[i]);
}
trace(TraceAllocationRequestAccepted, current_unique_id_.size());
/*
* Proceeding with allocation if possible
* Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader.
*/
if (current_unique_id_.size() == current_unique_id_.capacity())
{
UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d",
int(msg.node_id));
UniqueID unique_id;
copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin());
current_unique_id_.clear();
{
uint64_t event_agrument = 0;
for (uint8_t i = 0; i < 8; i++)
{
event_agrument |= static_cast<uint64_t>(unique_id[i]) << (56U - i * 8U);
}
trace(TraceAllocationExchangeComplete, static_cast<int64_t>(event_agrument));
}
handler_.handleAllocationRequest(unique_id, msg.node_id);
}
else
{
if (handler_.canPublishFollowupAllocationResponse())
{
publishFollowupAllocationResponse();
}
else
{
trace(TraceAllocationFollowupDenied, 0);
current_unique_id_.clear();
}
}
/*
* It is super important to update timestamp only if the request has been processed successfully.
*/
last_message_timestamp_ = msg.getMonotonicTimestamp();
}
void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg)
{
/*
* TODO This method can blow the stack easily
*/
UAVCAN_ASSERT(preferred_node_id_.isValid());
if (isAllocationComplete())
{
UAVCAN_ASSERT(0);
terminate();
return;
}
startPeriodic(getPeriod()); // Restarting the timer - Rule C
UAVCAN_TRACE("DynamicNodeIDClient", "Request timer reset because of Allocation message from %d",
static_cast<int>(msg.getSrcNodeID().get()));
// Rule D
if (!msg.isAnonymousTransfer() &&
msg.unique_id.size() > 0 &&
msg.unique_id.size() < msg.unique_id.capacity() &&
equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_))
{
// Filling the response message
const uint8_t size_of_unique_id_in_response =
min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST,
static_cast<uint8_t>(msg.unique_id.capacity() - msg.unique_id.size()));
protocol::dynamic_node_id::Allocation second_stage;
second_stage.node_id = preferred_node_id_.get();
second_stage.first_part_of_unique_id = false;
second_stage.unique_id.resize(size_of_unique_id_in_response);
copy(unique_id_ + msg.unique_id.size(),
unique_id_ + msg.unique_id.size() + size_of_unique_id_in_response,
second_stage.unique_id.begin());
UAVCAN_ASSERT(equal(second_stage.unique_id.begin(),
second_stage.unique_id.end(),
unique_id_ + msg.unique_id.size()));
// Broadcasting the response
UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d",
static_cast<int>(preferred_node_id_.get()), static_cast<int>(second_stage.unique_id.size()));
const int res = dnida_pub_.broadcast(second_stage);
if (res < 0)
{
dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed");
}
}
// Rule E
if (!msg.isAnonymousTransfer() &&
msg.unique_id.size() == msg.unique_id.capacity() &&
equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_) &&
msg.node_id > 0)
{
allocated_node_id_ = msg.node_id;
allocator_node_id_ = msg.getSrcNodeID();
UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d",
static_cast<int>(allocated_node_id_.get()), static_cast<int>(allocator_node_id_.get()));
terminate();
UAVCAN_ASSERT(isAllocationComplete());
}
}
