Esempio n. 1
0
    virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
                                   const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
                                   uavcan::MonotonicTime deadline)
    {
        assert(this);
        //std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;

        for (unsigned i = 0; i < ifaces.size(); i++)
        {
            ifaces.at(i).pending_tx = (pending_tx[i] == NULL) ? uavcan::CanFrame() : *pending_tx[i];
        }

        if (select_failure)
        {
            return -1;
        }

        const uavcan::uint8_t valid_iface_mask = uavcan::uint8_t((1 << getNumIfaces()) - 1);
        EXPECT_FALSE(inout_masks.write & ~valid_iface_mask);
        EXPECT_FALSE(inout_masks.read & ~valid_iface_mask);

        uavcan::uint8_t out_write_mask = 0;
        uavcan::uint8_t out_read_mask = 0;
        for (unsigned i = 0; i < getNumIfaces(); i++)
        {
            const uavcan::uint8_t mask = uavcan::uint8_t(1 << i);
            if ((inout_masks.write & mask) && ifaces.at(i).writeable)
            {
                out_write_mask |= mask;
            }
            if ((inout_masks.read & mask) && (ifaces.at(i).rx.size() || ifaces.at(i).loopback.size()))
            {
                out_read_mask |= mask;
            }
        }
        inout_masks.write = out_write_mask;
        inout_masks.read = out_read_mask;
        if ((out_write_mask | out_read_mask) == 0)
        {
            const uavcan::MonotonicTime ts = iclock.getMonotonic();
            const uavcan::MonotonicDuration diff = deadline - ts;
            SystemClockMock* const mock = dynamic_cast<SystemClockMock*>(&iclock);
            if (mock)
            {
                if (diff.isPositive())
                {
                    mock->advance(uint64_t(diff.toUSec()));   // Emulating timeout
                }
            }
            else
            {
                if (diff.isPositive())
                {
                    usleep(unsigned(diff.toUSec()));
                }
            }
            return 0;
        }
        return 1;  // This value is not being checked anyway, it just has to be greater than zero
    }
Esempio n. 2
0
bool BusEvent::wait(uavcan::MonotonicDuration duration)
{
    struct hrt_call wait_call;

    irqstate_t irs = irqsave();
    if (_signal) {
        _signal = 0;
        irqrestore(irs);
        return true;
    }

    sem_init(&_wait_semaphore, 0, 0);
    irqrestore(irs);

    hrt_call_after(&wait_call, duration.toUSec(), (hrt_callout) signalFromCallOut, this);
    sem_wait(&_wait_semaphore);

    hrt_cancel(&wait_call);

    irs = irqsave();
    if (_signal) {
        _signal = 0;
        irqrestore(irs);

        return true;
    }
    irqrestore(irs);

    return false;
}
Esempio n. 3
0
void NodeStatusProvider::setStatusPublishingPeriod(uavcan::MonotonicDuration period)
{
    const MonotonicDuration maximum = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_PUBLICATION_PERIOD_MS);
    const MonotonicDuration minimum = MonotonicDuration::fromMSec(protocol::NodeStatus::MIN_PUBLICATION_PERIOD_MS);

    period = min(period, maximum);
    period = max(period, minimum);
    TimerBase::startPeriodic(period);

    const MonotonicDuration tx_timeout = period - MonotonicDuration::fromUSec(period.toUSec() / 20);
    node_status_pub_.setTxTimeout(tx_timeout);

    UAVCAN_TRACE("NodeStatusProvider", "Status pub period: %s, TX timeout: %s",
                 period.toString().c_str(), node_status_pub_.getTxTimeout().toString().c_str());
}
 /**
  * Note that this method may return spuriously.
  */
 void waitFor(uavcan::MonotonicDuration duration)
 {
     std::unique_lock<std::mutex> lk(m_);
     (void)cv_.wait_for(lk, std::chrono::microseconds(duration.toUSec()));
 }