fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 03:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

GlobalTimeSyncSlave: methods isActive(), getMasterNodeID()

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-24 10:53:41 +04:00
parent d56d5d5dca
commit 438340ae98
3 changed files with 49 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
+6
View File
@@ -21,11 +21,13 @@ class GlobalTimeSyncSlave : Noncopyable
UtcTime prev_ts_utc_;
MonotonicTime prev_ts_mono_;
MonotonicTime last_adjustment_ts_;
enum State { Update, Adjust } state;
NodeID master_nid_;
uint8_t prev_iface_index_;
ISystemClock& getSystemClock() { return sub_.getNode().getSystemClock(); }
const ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); }
void adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
@@ -43,6 +45,10 @@ public:
{ }
int start();
bool isActive() const;
NodeID getMasterNodeID() const;
};
}
libuavcan/src/protocol/global_time_sync_slave.cpp
+12
View File
@@ -19,6 +19,7 @@ void GlobalTimeSyncSlave::adjustFromMsg(const ReceivedDataStructure<protocol::Gl
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
getSystemClock().adjustUtc(adjustment);
last_adjustment_ts_ = msg.getMonotonicTimestamp();
state = Update;
}
@@ -84,4 +85,15 @@ int GlobalTimeSyncSlave::start()
return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync));
}
bool GlobalTimeSyncSlave::isActive() const
{
const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_;
return !last_adjustment_ts_.isZero() && since_prev_adj.toMSec() <= protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS;
}
NodeID GlobalTimeSyncSlave::getMasterNodeID() const
{
return isActive() ? master_nid_ : NodeID();
}
}
libuavcan/test/protocol/global_time_sync_slave.cpp
+31
View File
@@ -29,6 +29,8 @@ TEST(GlobalTimeSyncSlave, Basic)
uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(nodes.b);
ASSERT_LE(0, gtss.start());
ASSERT_FALSE(gtss.isActive());
ASSERT_FALSE(gtss.getMasterNodeID().isValid());
/*
* Empty broadcast
@@ -44,6 +46,9 @@ TEST(GlobalTimeSyncSlave, Basic)
std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl;
std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl;
ASSERT_FALSE(gtss.isActive());
ASSERT_FALSE(gtss.getMasterNodeID().isValid());
/*
* Follow-up broadcast with proper time
* Slave must adjust now
@@ -59,6 +64,9 @@ TEST(GlobalTimeSyncSlave, Basic)
master_clock.utc += 1000000;
slave_clock.utc += 1000000;
ASSERT_TRUE(gtss.isActive());
ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
/*
* Next follow-up, slave is synchronized now
* Will update
@@ -72,6 +80,9 @@ TEST(GlobalTimeSyncSlave, Basic)
master_clock.utc += 1000000;
slave_clock.utc += 1000000;
ASSERT_TRUE(gtss.isActive());
ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
/*
* Next follow-up, slave is synchronized now
* Will adjust
@@ -87,6 +98,9 @@ TEST(GlobalTimeSyncSlave, Basic)
ASSERT_EQ(4000000, slave_clock.utc);
ASSERT_EQ(4000000, master_clock.utc);
ASSERT_TRUE(gtss.isActive());
ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
/*
* Another master
* This one has higher priority, so it will be preferred
@@ -112,6 +126,9 @@ TEST(GlobalTimeSyncSlave, Basic)
master2_clock.utc += 1000000;
ASSERT_TRUE(gtss.isActive());
ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
/*
* Adjustment
*/
@@ -119,6 +136,9 @@ TEST(GlobalTimeSyncSlave, Basic)
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(100, slave_clock.utc);
ASSERT_TRUE(gtss.isActive());
ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
/*
* Another master will be ignored now
*/
@@ -131,4 +151,15 @@ TEST(GlobalTimeSyncSlave, Basic)
gts_pub.broadcast(gts);
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(100, slave_clock.utc);
ASSERT_TRUE(gtss.isActive());
ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
/*
* Timeout
*/
slave_clock.advance(100000000);
ASSERT_FALSE(gtss.isActive());
ASSERT_FALSE(gtss.getMasterNodeID().isValid());
}