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()));
        }
    }
Exemplo n.º 2
0
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);
}
Exemplo n.º 3
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;
}
Exemplo n.º 4
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);
}