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

GlobalTimeSyncSlave

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-23 21:35:01 +04:00
parent 42fdadba2c
commit d56d5d5dca
3 changed files with 269 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
+48
View File
@@ -0,0 +1,48 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <uavcan/node/subscriber.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/protocol/GlobalTimeSync.hpp>
namespace uavcan
{
class GlobalTimeSyncSlave : Noncopyable
{
typedef MethodBinder<GlobalTimeSyncSlave*,
void (GlobalTimeSyncSlave::*)(const ReceivedDataStructure<protocol::GlobalTimeSync>&)> GlobalTimeSyncCallback;
// Static buffers are explicitly disabled because time should never be unicasted.
Subscriber<protocol::GlobalTimeSync, GlobalTimeSyncCallback, 2, 0> sub_;
UtcTime prev_ts_utc_;
MonotonicTime prev_ts_mono_;
enum State { Update, Adjust } state;
NodeID master_nid_;
uint8_t prev_iface_index_;
ISystemClock& getSystemClock() { return sub_.getNode().getSystemClock(); }
void adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
void updateFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
void processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
void handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
public:
GlobalTimeSyncSlave(INode& node)
: sub_(node)
, state(Update)
, prev_iface_index_(0xFF)
{ }
int start();
};
}
libuavcan/src/protocol/global_time_sync_slave.cpp
+87
View File
@@ -0,0 +1,87 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cassert>
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include <uavcan/debug.hpp>
namespace uavcan
{
void GlobalTimeSyncSlave::adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
assert(msg.prev_utc_usec > 0);
const UtcDuration adjustment = UtcTime::fromUSec(msg.prev_utc_usec) - prev_ts_utc_;
UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i",
static_cast<long long>(adjustment.toUSec()),
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
getSystemClock().adjustUtc(adjustment);
state = Update;
}
void GlobalTimeSyncSlave::updateFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i",
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
prev_ts_utc_ = msg.getUtcTimestamp();
prev_ts_mono_ = msg.getMonotonicTimestamp();
master_nid_ = msg.getSrcNodeID();
prev_iface_index_ = msg.getIfaceIndex();
state = Adjust;
}
void GlobalTimeSyncSlave::processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
const MonotonicDuration time_since_prev_sync = msg.getMonotonicTimestamp() - prev_ts_mono_;
assert(!time_since_prev_sync.isNegative());
const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero();
const bool switch_master = msg.getSrcNodeID() < master_nid_;
const bool timeout = time_since_prev_sync.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS;
if (switch_master || timeout || needs_init)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i timeout=%i",
int(needs_init), int(switch_master), int(timeout));
updateFromMsg(msg);
}
else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_)
{
if (state == Adjust && msg.prev_utc_usec > 0)
{
adjustFromMsg(msg);
}
else
{
updateFromMsg(msg);
}
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i",
int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
}
}
void GlobalTimeSyncSlave::handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
if (msg.getTransferType() == TransferTypeMessageBroadcast)
{
processMsg(msg);
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType()));
}
}
int GlobalTimeSyncSlave::start()
{
return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync));
}
}
libuavcan/test/protocol/global_time_sync_slave.cpp
+134
View File
@@ -0,0 +1,134 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/node/publisher.hpp>
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include "helpers.hpp"
TEST(GlobalTimeSyncSlave, Basic)
{
InterlinkedTestNodesWithClockMock nodes(64, 65);
SystemClockMock& slave_clock = nodes.clock_a;
SystemClockMock& master_clock = nodes.clock_b;
slave_clock.advance(1000000);
master_clock.advance(1000000);
master_clock.monotonic_auto_advance = slave_clock.monotonic_auto_advance = 1000;
master_clock.preserve_utc = slave_clock.preserve_utc = true;
slave_clock.utc = 0; // Not set yet
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalTimeSync> _reg1;
uavcan::GlobalTimeSyncSlave gtss(nodes.a);
uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(nodes.b);
ASSERT_LE(0, gtss.start());
/*
* Empty broadcast
* The slave must only register the timestamp and adjust nothing
*/
uavcan::protocol::GlobalTimeSync gts;
gts.prev_utc_usec = 0;
gts_pub.broadcast(gts);
gts.prev_utc_usec = master_clock.utc;
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
ASSERT_EQ(0, slave_clock.utc);
ASSERT_EQ(1000000, master_clock.utc);
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;
/*
* Follow-up broadcast with proper time
* Slave must adjust now
*/
gts_pub.broadcast(gts);
gts.prev_utc_usec = master_clock.utc;
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(1000000, slave_clock.utc);
ASSERT_EQ(1000000, master_clock.utc);
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;
master_clock.utc += 1000000;
slave_clock.utc += 1000000;
/*
* Next follow-up, slave is synchronized now
* Will update
*/
gts_pub.broadcast(gts);
gts.prev_utc_usec = master_clock.utc;
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(2000000, slave_clock.utc);
ASSERT_EQ(2000000, master_clock.utc);
master_clock.utc += 1000000;
slave_clock.utc += 1000000;
/*
* Next follow-up, slave is synchronized now
* Will adjust
*/
gts_pub.broadcast(gts);
gts.prev_utc_usec = master_clock.utc;
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(3000000, slave_clock.utc);
ASSERT_EQ(3000000, master_clock.utc);
master_clock.utc += 1000000;
slave_clock.utc += 1000000;
ASSERT_EQ(4000000, slave_clock.utc);
ASSERT_EQ(4000000, master_clock.utc);
/*
* Another master
* This one has higher priority, so it will be preferred
*/
SystemClockMock master2_clock(100);
master2_clock.monotonic_auto_advance = 1000;
master2_clock.preserve_utc = true;
PairableCanDriver master2_can(master2_clock);
master2_can.other = &nodes.can_a;
TestNode master2_node(master2_can, master2_clock, 8);
uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub2(master2_node);
/*
* Update step, no adjustment yet
*/
gts.prev_utc_usec = 0;
gts_pub2.broadcast(gts);
gts.prev_utc_usec = master2_clock.utc;
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(4000000, slave_clock.utc);
ASSERT_EQ(100, master2_clock.utc);
master2_clock.utc += 1000000;
/*
* Adjustment
*/
gts_pub2.broadcast(gts);
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(100, slave_clock.utc);
/*
* Another master will be ignored now
*/
gts.prev_utc_usec = 99999999;
// Update
gts_pub.broadcast(gts);
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(100, slave_clock.utc);
// Adjust
gts_pub.broadcast(gts);
nodes.spinBoth(uavcan::MonotonicDuration());
ASSERT_EQ(100, slave_clock.utc);
}