diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 857b058a5a..4252ec1b3e 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -42,3 +42,6 @@ target_link_libraries(test_node ${UAVCAN_LIB} rt) add_executable(test_node_status_monitor test/test_node_status_monitor.cpp) target_link_libraries(test_node_status_monitor ${UAVCAN_LIB} rt) + +add_executable(test_time_sync test/test_time_sync.cpp) +target_link_libraries(test_time_sync ${UAVCAN_LIB} rt) diff --git a/libuavcan_drivers/linux/test/test_time_sync.cpp b/libuavcan_drivers/linux/test/test_time_sync.cpp new file mode 100644 index 0000000000..3f549ae604 --- /dev/null +++ b/libuavcan_drivers/linux/test/test_time_sync.cpp @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include "debug.hpp" + + +static 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()); + + uavcan::NodeInitializationResult init_result; + const int start_res = node->start(init_result); + ENFORCE(0 == start_res); + if (!init_result.isOk()) + { + throw std::runtime_error("Network conflict with node " + std::to_string(init_result.conflicting_node.get())); + } + + node->setStatusOk(); + return node; +} + +static void runForever(const uavcan_linux::NodePtr& node) +{ + uavcan::GlobalTimeSyncMaster tsmaster(*node); + ENFORCE(0 == tsmaster.init()); + + uavcan::GlobalTimeSyncSlave tsslave(*node); + ENFORCE(0 == tsslave.start()); + + auto publish_sync_if_master = [&](const uavcan::TimerEvent&) + { + bool i_am_master = false; + if (tsslave.isActive()) + { + const uavcan::NodeID master_node = tsslave.getMasterNodeID(); + assert(master_node.isValid()); + if (node->getNodeID() < master_node) + { + std::cout << "Overriding the lower priority master " << int(master_node.get()) << std::endl; + i_am_master = true; + } + else + { + std::cout << "There is other master of higher priority " << int(master_node.get()) << std::endl; + } + } + else + { + std::cout << "No other masters present" << std::endl; + i_am_master = true; + } + + // Don't forget to disable slave adjustments if we're master + tsslave.suppress(i_am_master); + + if (i_am_master) + { + ENFORCE(0 <= tsmaster.publish()); + } + }; + + auto sync_publish_timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(1000), publish_sync_if_master); + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) + { + (void)node->logError("spin", "Error %*", res); + } + } +} + +int main(int argc, const char** argv) +{ + if (argc < 3) + { + std::cout << "Usage:\n\t" << argv[0] << " [can-iface-name-N...]" << std::endl; + return 1; + } + const int self_node_id = std::stoi(argv[1]); + std::vector iface_names; + for (int i = 2; i < argc; i++) + { + iface_names.emplace_back(argv[i]); + } + uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node_status_monitor"); + runForever(node); + return 0; +}