mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
132 lines
4.2 KiB
C++
132 lines
4.2 KiB
C++
/*
|
|
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
|
*/
|
|
|
|
#include <gtest/gtest.h>
|
|
#include <uavcan/protocol/global_time_sync_master.hpp>
|
|
#include <uavcan/protocol/global_time_sync_slave.hpp>
|
|
#include "helpers.hpp"
|
|
|
|
struct GlobalTimeSyncMasterTestNode
|
|
{
|
|
SystemClockDriver clock;
|
|
PairableCanDriver can;
|
|
TestNode node;
|
|
|
|
GlobalTimeSyncMasterTestNode(uavcan::NodeID nid)
|
|
: can(clock)
|
|
, node(can, clock, nid)
|
|
{ }
|
|
};
|
|
|
|
struct GlobalTimeSyncTestNetwork
|
|
{
|
|
GlobalTimeSyncMasterTestNode slave;
|
|
GlobalTimeSyncMasterTestNode master_low;
|
|
GlobalTimeSyncMasterTestNode master_high;
|
|
|
|
GlobalTimeSyncTestNetwork()
|
|
: slave(64)
|
|
, master_low(120)
|
|
, master_high(8)
|
|
{
|
|
slave.can.others.insert(&master_low.can);
|
|
master_low.can.others.insert(&slave.can);
|
|
master_high.can.others.insert(&slave.can);
|
|
}
|
|
|
|
void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9))
|
|
{
|
|
assert(!duration.isNegative());
|
|
unsigned nspins3 = unsigned(duration.toMSec() / 3);
|
|
nspins3 = nspins3 ? nspins3 : 2;
|
|
while (nspins3 --> 0)
|
|
{
|
|
ASSERT_LE(0, slave.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
|
|
ASSERT_LE(0, master_low.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
|
|
ASSERT_LE(0, master_high.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
|
|
}
|
|
}
|
|
};
|
|
|
|
TEST(GlobalTimeSyncMaster, Basic)
|
|
{
|
|
GlobalTimeSyncTestNetwork nwk;
|
|
|
|
uavcan::GlobalDataTypeRegistry::instance().reset();
|
|
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalTimeSync> _reg1;
|
|
|
|
uavcan::GlobalTimeSyncSlave slave(nwk.slave.node);
|
|
uavcan::GlobalTimeSyncMaster master_low(nwk.master_low.node);
|
|
uavcan::GlobalTimeSyncMaster master_high(nwk.master_high.node);
|
|
|
|
ASSERT_FALSE(master_low.isInitialized());
|
|
|
|
ASSERT_LE(0, slave.start());
|
|
ASSERT_LE(0, master_low.init());
|
|
ASSERT_LE(0, master_high.init());
|
|
|
|
ASSERT_TRUE(master_low.isInitialized());
|
|
ASSERT_FALSE(slave.isActive());
|
|
|
|
/*
|
|
* Simple synchronization
|
|
*/
|
|
ASSERT_LE(0, master_low.publish()); // Update
|
|
nwk.spinAll();
|
|
|
|
usleep(400000);
|
|
ASSERT_LE(0, master_low.publish()); // Adjustment
|
|
nwk.spinAll();
|
|
|
|
// Synchronization complete.
|
|
ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
|
|
ASSERT_TRUE(slave.isActive());
|
|
ASSERT_EQ(nwk.master_low.node.getNodeID(), slave.getMasterNodeID());
|
|
|
|
/*
|
|
* Moving clocks forward and re-syncing with another master
|
|
*/
|
|
static const uavcan::UtcDuration OneDay = uavcan::UtcDuration::fromMSec(24 * 3600 * 1000);
|
|
nwk.master_high.clock.utc_adjustment = OneDay;
|
|
|
|
usleep(400000);
|
|
ASSERT_LE(0, master_low.publish()); // Update from the old master
|
|
nwk.spinAll();
|
|
|
|
ASSERT_LE(0, master_high.publish()); // Update from the new master
|
|
nwk.spinAll();
|
|
|
|
usleep(400000);
|
|
ASSERT_LE(0, master_low.publish()); // Adjustment from the old master (ignored now)
|
|
ASSERT_LE(0, master_high.publish()); // Adjustment from the new master (accepted)
|
|
nwk.spinAll();
|
|
|
|
// Synchronization complete.
|
|
ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc()));
|
|
ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
|
|
ASSERT_TRUE(slave.isActive());
|
|
ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID());
|
|
|
|
/*
|
|
* Frequent calls to publish()
|
|
*/
|
|
ASSERT_LE(0, master_low.publish()); // Dropped
|
|
ASSERT_LE(0, master_low.publish()); // Dropped
|
|
ASSERT_LE(0, master_low.publish()); // Dropped
|
|
|
|
ASSERT_TRUE(nwk.slave.can.read_queue.empty());
|
|
|
|
usleep(400000);
|
|
ASSERT_LE(0, master_low.publish()); // Accepted
|
|
ASSERT_FALSE(nwk.slave.can.read_queue.empty());
|
|
|
|
nwk.spinAll();
|
|
|
|
// Synchronization did not change
|
|
ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc()));
|
|
ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
|
|
ASSERT_TRUE(slave.isActive());
|
|
ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID());
|
|
}
|