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");
}
}
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)
{ }
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()));
}
}
