diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index b33af01b8e..d4be18302c 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -63,6 +63,9 @@ target_link_libraries(test_posix ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) add_executable(test_dynamic_node_id_client apps/test_dynamic_node_id_client.cpp) target_link_libraries(test_dynamic_node_id_client ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) +add_executable(test_file_server apps/test_file_server.cpp) +target_link_libraries(test_file_server ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) + # # Tools # diff --git a/libuavcan_drivers/linux/apps/test_file_server.cpp b/libuavcan_drivers/linux/apps/test_file_server.cpp new file mode 100644 index 0000000000..9ea9851c73 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_file_server.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include "debug.hpp" +// UAVCAN +#include +// UAVCAN Linux drivers +#include +// UAVCAN POSIX drivers +#include +#include // Compilability test + +namespace +{ + +uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) +{ + auto node = uavcan_linux::makeNode(ifaces); + + node->setNodeID(nid); + node->setName(name.c_str()); + node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + + { + const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get()); + + uavcan::protocol::HardwareVersion hwver; + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); + std::cout << hwver << std::endl; + + node->setHardwareVersion(hwver); + } + + const int start_res = node->start(); + ENFORCE(0 == start_res); + + uavcan::NetworkCompatibilityCheckResult check_result; + ENFORCE(0 == node->checkNetworkCompatibility(check_result)); + if (!check_result.isOk()) + { + throw std::runtime_error("Network conflict with node " + std::to_string(check_result.conflicting_node.get())); + } + + node->setStatusOk(); + + return node; +} + +void runForever(const uavcan_linux::NodePtr& node) +{ + uavcan_posix::BasicFileSeverBackend backend(*node); + + uavcan::FileServer server(*node, backend); + + const int server_init_res = server.start(); + if (server_init_res < 0) + { + throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res)); + } + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + if (res < 0) + { + std::cerr << "Spin error: " << res << std::endl; + } + } +} + +struct Options +{ + uavcan::NodeID node_id; + std::vector ifaces; +}; + +Options parseOptions(int argc, const char** argv) +{ + const char* const executable_name = *argv++; + argc--; + + const auto enforce = [executable_name](bool condition, const char* error_text) { + if (!condition) + { + std::cerr << error_text << "\n" + << "Usage:\n\t" + << executable_name + << " [can-iface-name-N...]" + << std::endl; + std::exit(1); + } + }; + + enforce(argc >= 2, "Not enough arguments"); + + /* + * Node ID is always at the first position + */ + argc--; + const int node_id = std::stoi(*argv++); + enforce(node_id >= 1 && node_id <= 127, "Invalid node ID"); + + Options out; + out.node_id = uavcan::NodeID(std::uint8_t(node_id)); + + while (argc --> 0) + { + const std::string token(*argv++); + + if (token[0] != '-') + { + out.ifaces.push_back(token); + } + else + { + enforce(false, "Unexpected argument"); + } + } + + return out; +} + +} + +int main(int argc, const char** argv) +{ + try + { + auto options = parseOptions(argc, argv); + + std::cout << "Self node ID: " << int(options.node_id.get()) << "\n" + "Num ifaces: " << options.ifaces.size() << "\n" +#ifdef NDEBUG + "Build mode: Release" +#else + "Build mode: Debug" +#endif + << std::endl; + + auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_test_file_server"); + runForever(node); + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Error: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index bec6d91fd7..11e291a955 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -226,7 +226,7 @@ protected: * stay the course of the read, it may leave a dangling entry. * This call back handles the garbage collection. */ - virtual void handleTimerEvent(const uavcan::TimerEvent& event) + virtual void handleTimerEvent(const uavcan::TimerEvent&) { removeExpired(&head_); } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index ff09260b8b..58143e1056 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -221,7 +221,7 @@ protected: * @return True - the class will begin sending update requests. * False - the node will be ignored, no request will be sent. */ - virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID, const uavcan::protocol::GetNodeInfo::Response& node_info, FirmwareFilePath& out_firmware_file_path) {