static int getQueueLength(uavcan::CanTxQueue& queue)
{
const uavcan::CanTxQueue::Entry* p = queue.peek();
int length = 0;
while (p)
{
length++;
p = p->getNextListNode();
}
return length;
}
static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame)
{
const uavcan::CanTxQueue::Entry* p = queue.peek();
while (p)
{
if (frame == p->frame)
{
return true;
}
p = p->getNextListNode();
}
return false;
}
/**
* Call this from the main thread only.
* No additional locking is required.
*/
void flushTxQueueTo(uavcan::INode& main_node, std::uint8_t iface_index)
{
std::lock_guard<std::mutex> lock(mutex_);
const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
while (auto e = prioritized_tx_queue_.peek())
{
UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
unsigned(iface_mask), e->toString().c_str());
const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
uavcan::CanTxQueue::Qos(e->qos), e->flags);
if (res <= 0)
{
break;
}
prioritized_tx_queue_.remove(e);
}
}