C++ UAVCAN_ASSERT函数代码示例

int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags)
{
    const uint8_t num_ifaces = getNumIfaces();

    while (true)
    {
        CanSelectMasks masks;
        masks.write = makePendingTxMask();
        masks.read = uint8_t((1 << num_ifaces) - 1);
        {
            const int select_res = callSelect(masks, blocking_deadline);
            if (select_res < 0)
            {
                return -ErrDriver;
            }
        }

        // Write - if buffers are not empty, one frame will be sent for each iface per one receive() call
        for (uint8_t i = 0; i < num_ifaces; i++)
        {
            if (masks.write & (1 << i))
            {
                (void)sendFromTxQueue(i);  // It may fail, we don't care. Requested operation was receive, not send.
            }
        }

        // Read
        for (uint8_t i = 0; i < num_ifaces; i++)
        {
            if (masks.read & (1 << i))
            {
                ICanIface* const iface = driver_.getIface(i);
                if (iface == NULL)
                {
                    UAVCAN_ASSERT(0);   // Nonexistent interface
                    continue;
                }
                const int res = iface->receive(out_frame, out_frame.ts_mono, out_frame.ts_utc, out_flags);
                if (res == 0)
                {
                    UAVCAN_ASSERT(0);   // select() reported that iface has pending RX frames, but receive() returned none
                    continue;
                }
                out_frame.iface_index = i;
                if ((res > 0) && !(out_flags & CanIOFlagLoopback))
                {
                    counters_[i].frames_rx += 1;
                }
                return (res < 0) ? -ErrDriver : res;
            }
        }

        // Timeout checked in the last order - this way we can operate with expired deadline:
        if (sysclock_.getMonotonic() >= blocking_deadline)
        {
            break;
        }
    }
    return 0;
}

void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&)
{
    // This method implements Rule B
    UAVCAN_ASSERT(preferred_node_id_.isValid());
    if (isAllocationComplete())
    {
        UAVCAN_ASSERT(0);
        terminate();
        return;
    }

    // Filling the message
    protocol::dynamic_node_id::Allocation msg;
    msg.node_id = preferred_node_id_.get();
    msg.first_part_of_unique_id = true;

    msg.unique_id.resize(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST);
    copy(unique_id_, unique_id_ + msg.unique_id.size(), msg.unique_id.begin());
    UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_));

    // Broadcasting
    UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 1st stage: preferred ID: %d",
                 static_cast<int>(preferred_node_id_.get()));
    const int res = dnida_pub_.broadcast(msg);
    if (res < 0)
    {
        dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed");
    }
}

ITransferBuffer* TransferBufferManager::create(const TransferBufferManagerKey& key)
{
    if (key.isEmpty())
    {
        UAVCAN_ASSERT(0);
        return UAVCAN_NULLPTR;
    }
    remove(key);

    TransferBufferManagerEntry* tbme = TransferBufferManagerEntry::instantiate(allocator_, max_buf_size_);
    if (tbme == UAVCAN_NULLPTR)
    {
        return UAVCAN_NULLPTR;     // Epic fail.
    }

    buffers_.insert(tbme);

    UAVCAN_TRACE("TransferBufferManager", "Buffer created [num=%u], %s", getNumBuffers(), key.toString().c_str());

    if (tbme != UAVCAN_NULLPTR)
    {
        UAVCAN_ASSERT(tbme->isEmpty());
        tbme->reset(key);
    }
    return tbme;
}

int DataTypeInfoProvider::start()
{
    int res = 0;

    res = cats_srv_.start(
        ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest));
    if (res < 0)
    {
        goto fail;
    }

    res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest));
    if (res < 0)
    {
        goto fail;
    }

    UAVCAN_ASSERT(res >= 0);
    return res;

fail:
    UAVCAN_ASSERT(res < 0);
    cats_srv_.stop();
    gdti_srv_.stop();
    return res;
}

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

/*
 * MultiFrameIncomingTransfer
 */
MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc,
                                                       const RxFrame& last_frame, TransferBufferAccessor& tba)
    : IncomingTransfer(ts_mono, ts_utc, last_frame.getPriority(), last_frame.getTransferType(),
                       last_frame.getTransferID(), last_frame.getSrcNodeID(), last_frame.getIfaceIndex())
    , buf_acc_(tba)
{
    UAVCAN_ASSERT(last_frame.isValid());
    UAVCAN_ASSERT(last_frame.isLast());
}

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 Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame)
{
    RxFrame frame;
    if (!frame.parse(can_frame))
    {
        UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str());
        UAVCAN_ASSERT(0);  // No way!
        return;
    }
    UAVCAN_ASSERT(frame.getSrcNodeID() == getNodeID());
    loopback_listeners_.invokeListeners(frame);
}

 OutgoingTransferRegistryKey(DataTypeID data_type_id, TransferType transfer_type, NodeID destination_node_id)
     : data_type_id_(data_type_id)
     , transfer_type_(transfer_type)
     , destination_node_id_(destination_node_id)
 {
     UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == destination_node_id.isBroadcast());
     /*
      * Service response transfers must use the same Transfer ID as matching service request transfer,
      * so this registry is not applicable for service response transfers at all.
      */
     UAVCAN_ASSERT(transfer_type != TransferTypeServiceResponse);
 }

DataTypeSignature GlobalDataTypeRegistry::computeAggregateSignature(DataTypeKind kind,
                                                                    DataTypeIDMask& inout_id_mask) const
{
    UAVCAN_ASSERT(isFrozen());  // Computing the signature if the registry is not frozen is pointless

    const List* list = selectList(kind);
    if (!list)
    {
        UAVCAN_ASSERT(0);
        return DataTypeSignature();
    }

    int prev_dtid = -1;
    DataTypeSignature signature;
    bool signature_initialized = false;
    Entry* p = list->get();
    while (p)
    {
        const DataTypeDescriptor& desc = p->descriptor;
        const int dtid = desc.getID().get();

        if (inout_id_mask[unsigned(dtid)])
        {
            if (signature_initialized)
            {
                signature.extend(desc.getSignature());
            }
            else
            {
                signature = DataTypeSignature(desc.getSignature());
            }
            signature_initialized = true;
        }

        UAVCAN_ASSERT(prev_dtid < dtid);  // Making sure that list is ordered properly
        prev_dtid++;
        while (prev_dtid < dtid)
        {
            inout_id_mask[unsigned(prev_dtid++)] = false; // Erasing bits for missing types
        }
        UAVCAN_ASSERT(prev_dtid == dtid);

        p = p->getNextListNode();
    }
    prev_dtid++;
    while (prev_dtid <= DataTypeID::Max)
    {
        inout_id_mask[unsigned(prev_dtid++)] = false;
    }

    return signature;
}

void TimerBase::startPeriodic(MonotonicDuration period)
{
    UAVCAN_ASSERT(period < MonotonicDuration::getInfinite());
    stop();
    period_ = period;
    DeadlineHandler::startWithDelay(period);
}

    /**
     * Invokes storage IO.
     */
    int setCurrentTerm(Term term)
    {
        if (term < current_term_)
        {
            UAVCAN_ASSERT(0);
            return -ErrInvalidParam;
        }

        tracer_.onEvent(TraceRaftCurrentTermUpdate, term);

        StorageMarshaller io(storage_);

        Term tmp = term;
        int res = io.setAndGetBack(getCurrentTermKey(), tmp);
        if (res < 0)
        {
            return res;
        }

        if (tmp != term)
        {
            return -ErrFailure;
        }

        current_term_ = term;
        return 0;
    }

    void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
    {
        if (!needToQuery(msg.getSrcNodeID()))
        {
            return;
        }

        NodeData* data = node_map_.access(msg.getSrcNodeID());
        if (data == NULL)
        {
            trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get());

            data = node_map_.insert(msg.getSrcNodeID(), NodeData());
            if (data == NULL)
            {
                getNode().registerInternalFailure("NodeDiscoverer OOM");
                return;
            }
        }
        UAVCAN_ASSERT(data != NULL);

        if (msg.uptime_sec < data->last_seen_uptime)
        {
            trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get());
            data->num_get_node_info_attempts = 0;
        }
        data->last_seen_uptime = msg.uptime_sec;

        if (!isRunning())
        {
            startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs));
            trace(TraceDiscoveryTimerStart, getPeriod().toUSec());
        }
    }

    NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const
    {
        out_at_least_one_request_needed = false;

        for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin
        {
            last_picked_node_++;
            if (last_picked_node_ > NodeID::Max)
            {
                last_picked_node_ = 1;
            }
            UAVCAN_ASSERT((last_picked_node_ >= 1) &&
                          (last_picked_node_ <= NodeID::Max));

            const Entry& entry = getEntry(last_picked_node_);

            if (entry.request_needed)
            {
                out_at_least_one_request_needed = true;

                if (entry.updated_since_last_attempt &&
                    !get_node_info_client_.hasPendingCallToServer(last_picked_node_))
                {
                    UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
                    return NodeID(last_picked_node_);
                }
            }
        }

        return NodeID();        // No node could be found
    }

    /**
     * Invokes storage IO.
     */
    int setVotedFor(NodeID node_id)
    {
        if (!node_id.isValid())
        {
            UAVCAN_ASSERT(0);
            return -ErrInvalidParam;
        }

        tracer_.onEvent(TraceRaftVotedForUpdate, node_id.get());

        StorageMarshaller io(storage_);

        uint32_t tmp = node_id.get();
        int res = io.setAndGetBack(getVotedForKey(), tmp);
        if (res < 0)
        {
            return res;
        }

        if (node_id.get() != tmp)
        {
            return -ErrFailure;
        }

        voted_for_ = node_id;
        return 0;
    }