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

GlobalTimeSyncSlave Transfer ID and timing validation (not tested)

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-24 15:14:03 +04:00
parent 56748523fc
commit 31ed44ff5c
3 changed files with 27 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dsdl/uavcan/protocol/0.GlobalTimeSync.uavcan
+3 -2
View File
@@ -5,8 +5,9 @@
# This message is not intended for unicast transfers.
#
uint16 MAX_PUBLICATION_PERIOD_MS = 2000
uint16 DEFAULT_PUBLICATION_PERIOD_MS = 1000
uint16 MAX_PUBLICATION_PERIOD_MS = 1100
uint16 MIN_PUBLICATION_PERIOD_MS = 350
uint16 PUBLISHER_TIMEOUT_MS = 10000
# UTC time in microseconds when the PREVIOUS GlobalTimeSync message was transmitted.
libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
+3
View File
@@ -24,6 +24,7 @@ class GlobalTimeSyncSlave : Noncopyable
MonotonicTime last_adjustment_ts_;
enum State { Update, Adjust } state_;
NodeID master_nid_;
TransferID prev_tid_;
uint8_t prev_iface_index_;
ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); }
@@ -48,6 +49,8 @@ public:
bool isActive() const;
NodeID getMasterNodeID() const;
MonotonicTime getLastAdjustmentTime() const { return last_adjustment_ts_; }
};
}
libuavcan/src/protocol/global_time_sync_slave.cpp
+21 -8
View File
@@ -32,27 +32,40 @@ void GlobalTimeSyncSlave::updateFromMsg(const ReceivedDataStructure<protocol::Gl
prev_ts_mono_ = msg.getMonotonicTimestamp();
master_nid_ = msg.getSrcNodeID();
prev_iface_index_ = msg.getIfaceIndex();
state_ = Adjust;
prev_tid_ = msg.getTransferID();
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 MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_;
assert(!since_prev_msg.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;
const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS;
if (switch_master || timeout || needs_init)
if (switch_master || pub_timeout || needs_init)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i timeout=%i",
int(needs_init), int(switch_master), int(timeout));
UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i",
int(needs_init), int(switch_master), int(pub_timeout));
updateFromMsg(msg);
}
else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_)
{
if (state_ == Adjust && msg.prev_utc_usec > 0)
if (state_ == Adjust)
{
const bool msg_invalid = msg.prev_utc_usec == 0;
const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1;
const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS;
if (msg_invalid || wrong_tid || wrong_timing)
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i",
int(msg_invalid), int(wrong_tid), int(wrong_timing));
state_ = Update;
}
}
if (state_ == Adjust)
{
adjustFromMsg(msg);
}