static uavcan_linux::SubNodePtr initSubNode(unsigned num_ifaces, uavcan::INode& main_node)
{
std::cout << "Initializing sub node" << std::endl;
typedef VirtualCanDriver<QueuePoolSize> Driver;
std::shared_ptr<Driver> driver(new Driver(num_ifaces));
auto node = uavcan_linux::makeSubNode(driver);
node->setNodeID(main_node.getNodeID());
main_node.getDispatcher().installRxFrameListener(driver.get());
return node;
}
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()),
_main_node(main_node),
_server_instance(_subnode, _storage_backend, _tracer),
_fileserver_backend(_subnode),
_node_info_retriever(_subnode),
_fw_upgrade_trigger(_subnode, _fw_version_checker),
_fw_server(_subnode, _fileserver_backend),
_param_getset_client(_subnode),
_param_opcode_client(_subnode),
_param_restartnode_client(_subnode),
_beep_pub(_subnode),
_enumeration_indication_sub(_subnode),
_enumeration_client(_subnode),
_enumeration_getset_client(_subnode),
_enumeration_save_client(_subnode)
{
}
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()),
_main_node(main_node),
_tracer(),
_storage_backend(),
_fw_version_checker(),
_server_instance(_subnode, _storage_backend, _tracer),
_fileserver_backend(_subnode),
_node_info_retriever(_subnode),
_fw_upgrade_trigger(_subnode, _fw_version_checker),
_fw_server(_subnode, _fileserver_backend),
_count_in_progress(false),
_count_index(0),
_param_in_progress(0),
_param_index(0),
_param_list_in_progress(false),
_param_list_all_nodes(false),
_param_list_node_id(1),
_param_dirty_bitmap{0, 0, 0, 0},
_param_save_opcode(0),
_cmd_in_progress(false),
_param_response_pub(nullptr),
_param_getset_client(_subnode),
_param_opcode_client(_subnode),
_param_restartnode_client(_subnode),
_mutex_inited(false),
_check_fw(false),
_esc_enumeration_active(false),
_esc_enumeration_index(0),
_beep_pub(_subnode),
_enumeration_indication_sub(_subnode),
_enumeration_client(_subnode),
_enumeration_getset_client(_subnode),
_enumeration_save_client(_subnode)
{
}
/**
* 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);
}
}