test_time_sync.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <iostream>
00006 #include <cassert>
00007 #include <uavcan_linux/uavcan_linux.hpp>
00008 #include <uavcan/protocol/global_time_sync_master.hpp>
00009 #include <uavcan/protocol/global_time_sync_slave.hpp>
00010 #include "debug.hpp"
00011 
00012 
00013 static uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
00014                                       const std::string& name)
00015 {
00016     auto node = uavcan_linux::makeNode(ifaces);
00017     node->setNodeID(nid);
00018     node->setName(name.c_str());
00019 
00020     ENFORCE(0 == node->start());
00021 
00022     node->setModeOperational();
00023     return node;
00024 }
00025 
00026 static void runForever(const uavcan_linux::NodePtr& node)
00027 {
00028     uavcan::GlobalTimeSyncMaster tsmaster(*node);
00029     ENFORCE(0 == tsmaster.init());
00030 
00031     uavcan::GlobalTimeSyncSlave tsslave(*node);
00032     ENFORCE(0 == tsslave.start());
00033 
00034     auto publish_sync_if_master = [&](const uavcan::TimerEvent&)
00035     {
00036         bool i_am_master = false;
00037         if (tsslave.isActive())
00038         {
00039             const uavcan::NodeID master_node = tsslave.getMasterNodeID();
00040             assert(master_node.isValid());
00041             if (node->getNodeID() < master_node)
00042             {
00043                 std::cout << "Overriding the lower priority master " << int(master_node.get()) << std::endl;
00044                 i_am_master = true;
00045             }
00046             else
00047             {
00048                 std::cout << "There is other master of higher priority " << int(master_node.get()) << std::endl;
00049             }
00050         }
00051         else
00052         {
00053             std::cout << "No other masters present" << std::endl;
00054             i_am_master = true;
00055         }
00056 
00057         // Don't forget to disable slave adjustments if we're master
00058         tsslave.suppress(i_am_master);
00059 
00060         if (i_am_master)
00061         {
00062             ENFORCE(0 <= tsmaster.publish());
00063         }
00064     };
00065 
00066     auto sync_publish_timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(1000), publish_sync_if_master);
00067 
00068     while (true)
00069     {
00070         const int res = node->spin(uavcan::MonotonicDuration::getInfinite());
00071         if (res < 0)
00072         {
00073             node->logError("spin", "Error %*", res);
00074         }
00075     }
00076 }
00077 
00078 int main(int argc, const char** argv)
00079 {
00080     try
00081     {
00082         if (argc < 3)
00083         {
00084             std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
00085             return 1;
00086         }
00087         const int self_node_id = std::stoi(argv[1]);
00088         std::vector<std::string> iface_names;
00089         for (int i = 2; i < argc; i++)
00090         {
00091             iface_names.emplace_back(argv[i]);
00092         }
00093         uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.linux_test_node_status_monitor");
00094         runForever(node);
00095         return 0;
00096     }
00097     catch (const std::exception& ex)
00098     {
00099         std::cerr << "Exception: " << ex.what() << std::endl;
00100         return 1;
00101     }
00102 }