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);
}
void setRequestInterval(const MonotonicDuration interval)
{
if (interval.isPositive())
{
request_interval_ = interval;
if (TimerBase::isRunning()) // Restarting with new interval
{
TimerBase::startPeriodic(request_interval_);
}
}
else
{
UAVCAN_ASSERT(0);
}
}
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);
}
