virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument)
{
const uavcan::MonotonicDuration ts = SystemClockDriver().getMonotonic() - startup_ts_;
std::cout << "EVENT [" << id_ << "]\t" << ts.toString() << "\t"
<< code << "\t" << getEventName(code) << "\t" << argument << std::endl;
event_log_.push_back(EventLogEntry(code, argument));
}
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
}
void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9))
{
assert(!duration.isNegative());
unsigned nspins3 = unsigned(duration.toMSec() / 3);
nspins3 = nspins3 ? nspins3 : 2;
while (nspins3 --> 0)
{
ASSERT_LE(0, slave.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
ASSERT_LE(0, master_low.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
ASSERT_LE(0, master_high.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
}
}
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());
}
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;
}
/*
* BusEvent
*/
bool BusEvent::wait(uavcan::MonotonicDuration duration)
{
static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF;
const uavcan::int64_t msec = duration.toMSec();
msg_t ret = msg_t();
if (msec <= 0)
{
# if (CH_KERNEL_MAJOR == 2)
ret = sem_.waitTimeout(TIME_IMMEDIATE);
# else // ChibiOS 3
ret = sem_.wait(TIME_IMMEDIATE);
# endif
}
else
{
# if (CH_KERNEL_MAJOR == 2)
ret = sem_.waitTimeout((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
# else // ChibiOS 3
ret = sem_.wait((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
# endif
}
# if (CH_KERNEL_MAJOR == 2)
return ret == RDY_OK;
# else // ChibiOS 3
return ret == MSG_OK;
# endif
}
/*
* BusEvent
*/
bool BusEvent::wait(uavcan::MonotonicDuration duration)
{
static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF;
const uavcan::int64_t msec = duration.toMSec();
msg_t ret = msg_t();
if (msec <= 0)
{
ret = sem_.waitTimeout(TIME_IMMEDIATE);
}
else
{
ret = sem_.waitTimeout((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
}
return ret == RDY_OK;
}
/**
* 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()));
}