global_time_sync_slave.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
3  */
4 
5 #include <gtest/gtest.h>
6 #include <uavcan/node/publisher.hpp>
7 #include <uavcan/protocol/global_time_sync_slave.hpp>
8 #include "helpers.hpp"
9 
10 
11 TEST(GlobalTimeSyncSlave, Basic)
12 {
13  InterlinkedTestNodesWithClockMock nodes(64, 65);
14 
15  SystemClockMock& slave_clock = nodes.clock_a;
16  SystemClockMock& master_clock = nodes.clock_b;
17 
18  slave_clock.advance(1000000);
19  master_clock.advance(1000000);
20 
21  master_clock.monotonic_auto_advance = slave_clock.monotonic_auto_advance = 1000;
22  master_clock.preserve_utc = slave_clock.preserve_utc = true;
23  slave_clock.utc = 0; // Not set yet
24 
25  uavcan::GlobalDataTypeRegistry::instance().reset();
26  uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalTimeSync> _reg1;
27 
28  uavcan::GlobalTimeSyncSlave gtss(nodes.a);
29  uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(nodes.b);
30 
31  ASSERT_LE(0, gtss.start());
32  ASSERT_FALSE(gtss.isActive());
33  ASSERT_FALSE(gtss.getMasterNodeID().isValid());
34 
35  /*
36  * Empty broadcast
37  * The slave must only register the timestamp and adjust nothing
38  */
39  uavcan::protocol::GlobalTimeSync gts;
40  gts.previous_transmission_timestamp_usec = 0;
41  gts_pub.broadcast(gts);
42  gts.previous_transmission_timestamp_usec = master_clock.utc;
43  nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
44  ASSERT_EQ(0, slave_clock.utc);
45  ASSERT_EQ(1000000, master_clock.utc);
46  std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl;
47  std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl;
48 
49  ASSERT_FALSE(gtss.isActive());
50  ASSERT_FALSE(gtss.getMasterNodeID().isValid());
51 
52  /*
53  * Follow-up broadcast with proper time
54  * Slave must adjust now
55  */
56  gts_pub.broadcast(gts);
57  gts.previous_transmission_timestamp_usec = master_clock.utc;
58  nodes.spinBoth(uavcan::MonotonicDuration());
59  ASSERT_EQ(1000000, slave_clock.utc);
60  ASSERT_EQ(1000000, master_clock.utc);
61  std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl;
62  std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl;
63 
64  master_clock.utc += 1000000;
65  slave_clock.utc += 1000000;
66 
67  ASSERT_TRUE(gtss.isActive());
68  ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
69 
70  /*
71  * Next follow-up, slave is synchronized now
72  * Will update
73  */
74  gts_pub.broadcast(gts);
75  gts.previous_transmission_timestamp_usec = master_clock.utc;
76  nodes.spinBoth(uavcan::MonotonicDuration());
77  ASSERT_EQ(2000000, slave_clock.utc);
78  ASSERT_EQ(2000000, master_clock.utc);
79 
80  master_clock.utc += 1000000;
81  slave_clock.utc += 1000000;
82 
83  ASSERT_TRUE(gtss.isActive());
84  ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
85 
86  /*
87  * Next follow-up, slave is synchronized now
88  * Will adjust
89  */
90  gts_pub.broadcast(gts);
91  gts.previous_transmission_timestamp_usec = master_clock.utc;
92  nodes.spinBoth(uavcan::MonotonicDuration());
93  ASSERT_EQ(3000000, slave_clock.utc);
94  ASSERT_EQ(3000000, master_clock.utc);
95 
96  master_clock.utc += 1000000;
97  slave_clock.utc += 1000000;
98  ASSERT_EQ(4000000, slave_clock.utc);
99  ASSERT_EQ(4000000, master_clock.utc);
100 
101  ASSERT_TRUE(gtss.isActive());
102  ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
103 
104  /*
105  * Another master
106  * This one has higher priority, so it will be preferred
107  */
108  SystemClockMock master2_clock(100);
109  master2_clock.monotonic_auto_advance = 1000;
110  master2_clock.preserve_utc = true;
111  PairableCanDriver master2_can(master2_clock);
112  master2_can.others.insert(&nodes.can_a);
113  TestNode master2_node(master2_can, master2_clock, 8);
114 
115  uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub2(master2_node);
116 
117  /*
118  * Update step, no adjustment yet
119  */
120  gts.previous_transmission_timestamp_usec = 0;
121  gts_pub2.broadcast(gts);
122  gts.previous_transmission_timestamp_usec = master2_clock.utc;
123  nodes.spinBoth(uavcan::MonotonicDuration());
124  ASSERT_EQ(4000000, slave_clock.utc);
125  ASSERT_EQ(100, master2_clock.utc);
126 
127  master2_clock.utc += 1000000;
128 
129  ASSERT_TRUE(gtss.isActive());
130  ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
131 
132  /*
133  * Adjustment
134  */
135  gts_pub2.broadcast(gts);
136  nodes.spinBoth(uavcan::MonotonicDuration());
137  ASSERT_EQ(100, slave_clock.utc);
138 
139  ASSERT_TRUE(gtss.isActive());
140  ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
141 
142  /*
143  * Another master will be ignored now
144  */
145  gts.previous_transmission_timestamp_usec = 99999999;
146  // Update
147  gts_pub.broadcast(gts);
148  nodes.spinBoth(uavcan::MonotonicDuration());
149  ASSERT_EQ(100, slave_clock.utc);
150  // Adjust
151  gts_pub.broadcast(gts);
152  nodes.spinBoth(uavcan::MonotonicDuration());
153  ASSERT_EQ(100, slave_clock.utc);
154 
155  ASSERT_TRUE(gtss.isActive());
156  ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
157 
158  /*
159  * Timeout
160  */
161  slave_clock.advance(100000000);
162 
163  ASSERT_FALSE(gtss.isActive());
164  ASSERT_FALSE(gtss.getMasterNodeID().isValid());
165 }
166 
167 
168 #if !defined(BYTE_ORDER) || !defined(LITTLE_ENDIAN) || (BYTE_ORDER != LITTLE_ENDIAN)
169 # error "This test cannot be executed on this platform"
170 #endif
171 
172 static uavcan::Frame makeSyncMsg(uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid)
173 {
174  uavcan::Frame frame(uavcan::protocol::GlobalTimeSync::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
175  snid, uavcan::NodeID::Broadcast, tid);
176  frame.setStartOfTransfer(true);
177  frame.setEndOfTransfer(true);
178  EXPECT_EQ(7, frame.setPayload(reinterpret_cast<uint8_t*>(&usec), 7)); // Assuming little endian!!!
179  return frame;
180 }
181 
182 static void broadcastSyncMsg(CanIfaceMock& iface, uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid)
183 {
184  const uavcan::Frame frame = makeSyncMsg(usec, snid, tid);
185  uavcan::CanFrame can_frame;
186  ASSERT_TRUE(frame.compile(can_frame));
187  iface.pushRx(can_frame);
188 }
189 
190 
191 TEST(GlobalTimeSyncSlave, Validation)
192 {
193  SystemClockMock slave_clock;
194  slave_clock.monotonic = 1000000;
195  slave_clock.preserve_utc = true;
196 
197  CanDriverMock slave_can(3, slave_clock);
198  for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++)
199  {
200  slave_can.ifaces.at(i).enable_utc_timestamping = true;
201  }
202 
203  TestNode node(slave_can, slave_clock, 64);
204 
205  uavcan::GlobalTimeSyncSlave gtss(node);
206  uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(node);
207 
208  ASSERT_LE(0, gtss.start());
209  ASSERT_FALSE(gtss.isActive());
210  ASSERT_FALSE(gtss.getMasterNodeID().isValid());
211  ASSERT_EQ(0, slave_clock.utc);
212 
213  /*
214  * Update/adjust/update
215  */
216  broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0); // Locked on this
217  broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored
218  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
219 
220  broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead
221  broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored
222  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
223 
224  ASSERT_TRUE(gtss.isActive());
225  ASSERT_EQ(8, gtss.getMasterNodeID().get());
226  ASSERT_EQ(1000, slave_clock.utc);
227 
228  broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 2); // Update
229  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
230 
231  ASSERT_EQ(1000, slave_clock.utc);
232  std::cout << slave_clock.monotonic << std::endl;
233 
234  /*
235  * TID jump simulates a frame loss
236  */
237  broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 4); // Adjustment skipped - expected TID 3, update instead
238  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
239 
240  ASSERT_TRUE(gtss.isActive());
241  ASSERT_EQ(8, gtss.getMasterNodeID().get());
242  ASSERT_EQ(1000, slave_clock.utc);
243  std::cout << slave_clock.monotonic << std::endl;
244 
245  /*
246  * Valid adjustment - continuing from TID 4
247  */
248  broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 5); // Slave UTC was 1000, master reports 3000 --> shift ahead
249  broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 5);
250  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
251 
252  ASSERT_TRUE(gtss.isActive());
253  ASSERT_EQ(8, gtss.getMasterNodeID().get());
254  ASSERT_EQ(3000, slave_clock.utc);
255  std::cout << slave_clock.monotonic << std::endl;
256 
257  /*
258  * Update, then very long delay with correct TID
259  */
260  broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 6); // Valid update, slave UTC is 3000
261  broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 6);
262  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
263 
264  slave_clock.monotonic += 5000000;
265 
266  broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 7); // Adjustment skipped
267  broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 7);
268  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
269 
270  broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 8); // Valid adjustment now
271  broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 8);
272  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
273 
274  ASSERT_TRUE(gtss.isActive());
275  ASSERT_EQ(8, gtss.getMasterNodeID().get());
276  ASSERT_EQ(5000, slave_clock.utc);
277  std::cout << slave_clock.monotonic << std::endl;
278 }
279 
280 
281 TEST(GlobalTimeSyncSlave, Suppression)
282 {
283  SystemClockMock slave_clock;
284  slave_clock.monotonic = 1000000;
285  slave_clock.preserve_utc = true;
286 
287  CanDriverMock slave_can(3, slave_clock);
288  for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++)
289  {
290  slave_can.ifaces.at(i).enable_utc_timestamping = true;
291  }
292 
293  TestNode node(slave_can, slave_clock, 64);
294 
295  uavcan::GlobalTimeSyncSlave gtss(node);
296  uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(node);
297 
298  ASSERT_LE(0, gtss.start());
299  ASSERT_EQ(0, slave_clock.utc);
300 
301  gtss.suppress(true);
302 
303  broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0); // Locked on this
304  broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored
305  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
306 
307  broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead
308  broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored
309  ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
310 
311  ASSERT_TRUE(gtss.isActive());
312  ASSERT_EQ(8, gtss.getMasterNodeID().get());
313  ASSERT_EQ(0, slave_clock.utc); // The clock shall not be asjusted
314 }
uavcan::uint8_t
std::uint8_t uint8_t
Definition: std.hpp:24
PairableCanDriver::others
std::set< PairableCanDriver * > others
Definition: test_node.hpp:55
pyuavcan_v0.introspect.node
node
Definition: introspect.py:398
uavcan::Frame::setStartOfTransfer
void setStartOfTransfer(bool x)
Definition: frame.hpp:81
uavcan::NodeID::Broadcast
static const NodeID Broadcast
Definition: transfer.hpp:122
uavcan::Frame::setPayload
uint8_t setPayload(const uint8_t *data, unsigned len)
Definition: uc_frame.cpp:16
SystemClockMock::advance
void advance(uint64_t usec) const
Definition: libuavcan/libuavcan/test/clock.hpp:28
CanDriverMock::getNumIfaces
virtual uavcan::uint8_t getNumIfaces() const
Definition: libuavcan/libuavcan/test/transport/can/can.hpp:274
uavcan::NodeID::get
uint8_t get() const
Definition: transfer.hpp:132
broadcastSyncMsg
static void broadcastSyncMsg(CanIfaceMock &iface, uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid)
Definition: global_time_sync_slave.cpp:182
InterlinkedTestNodes::can_a
PairableCanDriver can_a
Definition: test_node.hpp:153
global_time_sync_slave.hpp
uavcan::INode::spin
int spin(MonotonicTime deadline)
Definition: abstract_node.hpp:63
uavcan::INode::getNodeID
NodeID getNodeID() const
Definition: abstract_node.hpp:39
uavcan::uint64_t
std::uint64_t uint64_t
Definition: std.hpp:27
uavcan::NodeID::isValid
bool isValid() const
Definition: transfer.hpp:134
uavcan::Frame::compile
bool compile(CanFrame &can_frame) const
Definition: uc_frame.cpp:112
uavcan::GlobalDataTypeRegistry::instance
static GlobalDataTypeRegistry & instance()
Definition: uc_global_data_type_registry.cpp:128
frame
uavcan::CanFrame frame
Definition: can.cpp:78
makeSyncMsg
static uavcan::Frame makeSyncMsg(uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid)
Definition: global_time_sync_slave.cpp:172
uavcan::DurationBase< MonotonicDuration >::fromMSec
static MonotonicDuration fromMSec(int64_t ms)
Definition: time.hpp:41
InterlinkedTestNodes::spinBoth
int spinBoth(uavcan::MonotonicDuration duration)
Definition: test_node.hpp:176
CanIfaceMock::pushRx
void pushRx(const uavcan::CanFrame &frame)
Definition: libuavcan/libuavcan/test/transport/can/can.hpp:68
uavcan::Frame::setEndOfTransfer
void setEndOfTransfer(bool x)
Definition: frame.hpp:82
TEST
TEST(GlobalTimeSyncSlave, Basic)
Definition: global_time_sync_slave.cpp:11
uavcan::Publisher::broadcast
int broadcast(const DataType &message)
Definition: publisher.hpp:52

uavcan_communicator
Author(s):
autogenerated on Wed Jan 11 2023 03:59:39