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()));
}
}
int NodeStatusProvider::publish()
{
const MonotonicDuration uptime = getNode().getMonotonicTime() - creation_timestamp_;
UAVCAN_ASSERT(uptime.isPositive());
node_info_.status.uptime_sec = uptime.toMSec() / 1000;
UAVCAN_ASSERT(node_info_.status.status_code <= protocol::NodeStatus::FieldTypes::status_code::max());
return node_status_pub_.broadcast(node_info_.status);
}
int GlobalTimeSyncMaster::publish()
{
if (!initialized_)
{
const int res = init();
if (res < 0)
{
return res;
}
}
/*
* Enforce max frequency
*/
const MonotonicTime current_time = node_.getMonotonicTime();
{
const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_;
UAVCAN_ASSERT(since_prev_pub.isPositive());
if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_PUBLICATION_PERIOD_MS)
{
UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped");
return 0;
}
prev_pub_mono_ = current_time;
}
/*
* Obtain common Transfer ID for all masters
*/
TransferID tid;
{
const int tid_res = getNextTransferID(tid);
if (tid_res < 0)
{
return tid_res;
}
}
for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
{
const int res = iface_masters_[i]->publish(tid, current_time);
if (res < 0)
{
return res;
}
}
return 0;
}
int GlobalTimeSyncMaster::IfaceMaster::publish(TransferID tid, MonotonicTime current_time)
{
UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback);
UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_));
const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_;
iface_prev_pub_mono_ = current_time;
UAVCAN_ASSERT(since_prev_pub.isPositive());
const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS;
protocol::GlobalTimeSync msg;
msg.prev_utc_usec = long_period ? 0 : prev_tx_utc_.toUSec();
prev_tx_utc_ = UtcTime();
UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i",
static_cast<unsigned long long>(msg.prev_utc_usec), int(iface_index_), int(tid.get()));
return pub_.broadcast(msg, tid);
}