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