コード例 #1
0
 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)
 { }
コード例 #2
0
    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()));
        }
    }
コード例 #3
0
    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();
    }