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

RX frames have two timestamps: monotonic - for protocol timings management; utc - for application-level timestamping

Browse Source
This commit is contained in:
Pavel Kirienko 2014-02-11 14:32:45 +04:00
parent b8f6bf3ffa
commit 696451baca
10 changed files with 85 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libuavcan/include/uavcan/can_driver.hpp
View File

@ -93,10 +93,17 @@ public:
/**
* Non-blocking reception.
* out_utc_timestamp_usec must be provided by the driver, ideally by the hardware CAN controller; 0 if unknown.
* Timestamps should be provided by the CAN driver, ideally by the hardware CAN controller.
* Monotonic timestamp is required and can be not precise since it is needed only for
* protocol timing validation (transfer timeouts and inter-transfer intervals).
* UTC timestamp is optional, if available it will be used for precise time synchronization;
* must be set to zero if not available.
* Refer to @ref ISystemClock to learn more about timestamps.
* @param [out] out_ts_monotonic_usec Monotonic timestamp, usec, mandatory.
* @param [out] out_ts_utc_usec UTC timestamp, usec, optional, zero if unknown.
* @return 1 = one frame received, 0 = RX buffer empty, negative for error.
*/
virtual int receive(CanFrame& out_frame, uint64_t& out_utc_timestamp_usec) = 0;
virtual int receive(CanFrame& out_frame, uint64_t& out_ts_monotonic_usec, uint64_t& out_ts_utc_usec) = 0;
/**
* Configure the hardware CAN filters. @ref CanFilterConfig.

6
libuavcan/include/uavcan/internal/transport/can_io.hpp
View File

@ -19,11 +19,13 @@ namespace uavcan
struct CanRxFrame : public CanFrame
{
uint64_t timestamp;
uint64_t ts_monotonic;
uint64_t ts_utc;
uint8_t iface_index;
CanRxFrame()
: timestamp(0)
: ts_monotonic(0)
, ts_utc(0)
, iface_index(0)
{ }
};

9
libuavcan/include/uavcan/internal/transport/transfer.hpp
View File

@ -137,11 +137,13 @@ struct Frame
struct RxFrame : public Frame
{
uint_fast64_t timestamp;
uint_fast64_t ts_monotonic;
uint_fast64_t ts_utc;
uint_fast8_t iface_index;
RxFrame()
: timestamp(0)
: ts_monotonic(0)
, ts_utc(0)
, iface_index(0)
{ }
@ -149,7 +151,8 @@ struct RxFrame : public Frame
{
if (!Frame::parse(can_frame))
return false;
timestamp = can_frame.timestamp;
ts_monotonic = can_frame.ts_monotonic;
ts_utc = can_frame.ts_utc;
iface_index = can_frame.iface_index;
return true;
}

25
libuavcan/include/uavcan/internal/transport/transfer_receiver.hpp
View File

@ -23,9 +23,10 @@ private:
enum TidRelation { TID_SAME, TID_REPEAT, TID_FUTURE };
enum { IFACE_INDEX_NOTSET = 0xFF };
uint64_t prev_transfer_timestamp_;
uint64_t this_transfer_timestamp_;
uint64_t transfer_interval_;
uint64_t prev_transfer_ts_monotonic_;
uint64_t this_transfer_ts_monotonic_;
uint64_t first_frame_ts_utc_;
uint64_t transfer_interval_; // TODO: make 32 bit
ITransferBufferManager* bufmgr_;
TransferBufferManagerKey bufmgr_key_;
TransferID tid_;
@ -48,8 +49,9 @@ private:
public:
TransferReceiver()
: prev_transfer_timestamp_(0)
, this_transfer_timestamp_(0)
: prev_transfer_ts_monotonic_(0)
, this_transfer_ts_monotonic_(0)
, first_frame_ts_utc_(0)
, transfer_interval_(DEFAULT_TRANSFER_INTERVAL)
, bufmgr_(NULL)
, iface_index_(IFACE_INDEX_NOTSET)
@ -57,8 +59,9 @@ public:
{ }
TransferReceiver(ITransferBufferManager* bufmgr, const TransferBufferManagerKey& bufmgr_key)
: prev_transfer_timestamp_(0)
, this_transfer_timestamp_(0)
: prev_transfer_ts_monotonic_(0)
, this_transfer_ts_monotonic_(0)
, first_frame_ts_utc_(0)
, transfer_interval_(DEFAULT_TRANSFER_INTERVAL)
, bufmgr_(bufmgr)
, bufmgr_key_(bufmgr_key)
@ -74,8 +77,9 @@ public:
TransferReceiver& operator=(const TransferReceiver& rhs)
{
cleanup();
prev_transfer_timestamp_ = rhs.prev_transfer_timestamp_;
this_transfer_timestamp_ = rhs.this_transfer_timestamp_;
prev_transfer_ts_monotonic_ = rhs.prev_transfer_ts_monotonic_;
this_transfer_ts_monotonic_ = rhs.this_transfer_ts_monotonic_;
first_frame_ts_utc_ = rhs.first_frame_ts_utc_;
transfer_interval_ = rhs.transfer_interval_;
bufmgr_ = rhs.bufmgr_;
tid_ = rhs.tid_;
@ -89,7 +93,8 @@ public:
ResultCode addFrame(const RxFrame& frame);
uint64_t getLastTransferTimestamp() const { return prev_transfer_timestamp_; }
uint64_t getLastTransferTimestampMonotonic() const { return prev_transfer_ts_monotonic_; }
uint64_t getLastTransferTimestampUtc() const { return first_frame_ts_utc_; }
uint64_t getInterval() const { return transfer_interval_; }
};

2
libuavcan/src/transport/can_io.cpp
View File

@ -347,7 +347,7 @@ int CanIOManager::receive(CanRxFrame& frame, uint64_t monotonic_deadline)
assert(0); // Nonexistent interface
continue;
}
const int res = iface->receive(frame, frame.timestamp);
const int res = iface->receive(frame, frame.ts_monotonic, frame.ts_utc);
if (res == 0)
{
assert(0); // select() reported that iface has pending RX frames, but receive() returned none

2
libuavcan/src/transport/transfer.cpp
View File

@ -106,7 +106,7 @@ std::string Frame::toString() const
std::string RxFrame::toString() const
{
std::ostringstream os; // C++03 doesn't support long long, so we need ostream to print the timestamp
os << Frame::toString() << " ts=" << timestamp << " iface=" << int(iface_index);
os << Frame::toString() << " ts_m=" << ts_monotonic << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
return os.str();
}

29
libuavcan/src/transport/transfer_receiver.cpp
View File

@ -34,16 +34,16 @@ TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& fr
void TransferReceiver::updateTransferTimings()
{
assert(this_transfer_timestamp_ > 0);
assert(this_transfer_ts_monotonic_ > 0);
const uint64_t prev_prev_ts = prev_transfer_timestamp_;
prev_transfer_timestamp_ = this_transfer_timestamp_;
const uint64_t prev_prev_ts = prev_transfer_ts_monotonic_;
prev_transfer_ts_monotonic_ = this_transfer_ts_monotonic_;
if ((prev_prev_ts != 0) &&
(prev_transfer_timestamp_ != 0) &&
(prev_transfer_timestamp_ >= prev_prev_ts))
(prev_transfer_ts_monotonic_ != 0) &&
(prev_transfer_ts_monotonic_ >= prev_prev_ts))
{
uint64_t interval = prev_transfer_timestamp_ - prev_prev_ts;
uint64_t interval = prev_transfer_ts_monotonic_ - prev_prev_ts;
interval = std::max(std::min(interval, MAX_TRANSFER_INTERVAL), MIN_TRANSFER_INTERVAL);
transfer_interval_ = (transfer_interval_ * 7 + interval) / 8;
}
@ -95,7 +95,10 @@ bool TransferReceiver::validate(const RxFrame& frame) const
TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame)
{
if (frame.frame_index == 0)
this_transfer_timestamp_ = frame.timestamp;
{
this_transfer_ts_monotonic_ = frame.ts_monotonic;
first_frame_ts_utc_ = frame.ts_utc;
}
if ((frame.frame_index == 0) && frame.last_frame) // Single-frame transfer
{
@ -137,7 +140,7 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame)
bool TransferReceiver::isTimedOut(uint64_t timestamp) const
{
static const int INTERVAL_MULT = (1 << TransferID::BITLEN) / 2 - 1;
const uint64_t ts = this_transfer_timestamp_;
const uint64_t ts = this_transfer_ts_monotonic_;
if (timestamp <= ts)
return false;
return (timestamp - ts) > (transfer_interval_ * INTERVAL_MULT);
@ -149,16 +152,16 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame)
assert(bufmgr_key_.getNodeID() == frame.source_node_id);
assert(bufmgr_key_.getTransferType() == frame.transfer_type);
if ((frame.timestamp == 0) ||
(frame.timestamp < prev_transfer_timestamp_) ||
(frame.timestamp < this_transfer_timestamp_))
if ((frame.ts_monotonic == 0) ||
(frame.ts_monotonic < prev_transfer_ts_monotonic_) ||
(frame.ts_monotonic < this_transfer_ts_monotonic_))
{
return RESULT_NOT_COMPLETE;
}
const bool not_initialized = !isInitialized();
const bool iface_timed_out = (frame.timestamp - this_transfer_timestamp_) > (transfer_interval_ * 2);
const bool receiver_timed_out = isTimedOut(frame.timestamp);
const bool iface_timed_out = (frame.ts_monotonic - this_transfer_ts_monotonic_) > (transfer_interval_ * 2);
const bool receiver_timed_out = isTimedOut(frame.ts_monotonic);
const bool same_iface = frame.iface_index == iface_index_;
const bool first_fame = frame.frame_index == 0;
const TidRelation tid_rel = getTidRelation(frame);

14
libuavcan/test/transport/can/io.cpp
View File

@ -69,7 +69,7 @@ public:
return 1;
}
int receive(uavcan::CanFrame& out_frame, uint64_t& out_utc_timestamp_usec)
int receive(uavcan::CanFrame& out_frame, uint64_t& out_ts_monotonic_usec, uint64_t& out_ts_utc_usec)
{
assert(this);
EXPECT_TRUE(rx.size()); // Shall never be called when not readable
@ -80,7 +80,8 @@ public:
const FrameWithTime frame = rx.front();
rx.pop();
out_frame = frame.frame;
out_utc_timestamp_usec = frame.time;
out_ts_monotonic_usec = frame.time;
out_ts_utc_usec = 0;
return 1;
}
@ -178,10 +179,11 @@ TEST(CanIOManager, CanDriverMock)
EXPECT_EQ(0, mask_wr);
EXPECT_EQ(2, mask_rd);
CanFrame fr2;
uint64_t timestamp;
EXPECT_EQ(1, driver.getIface(1)->receive(fr2, timestamp));
uint64_t ts_monotonic, ts_utc;
EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc));
EXPECT_EQ(fr1, fr2);
EXPECT_EQ(100, timestamp);
EXPECT_EQ(100, ts_monotonic);
EXPECT_EQ(0, ts_utc);
// #0 WR, #1 RD, Select failure
driver.ifaces.at(0).writeable = true;
@ -203,7 +205,7 @@ static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFr
<< " " << frame.toString(uavcan::CanFrame::STR_ALIGNED) << std::endl;
}
return (static_cast<const uavcan::CanFrame&>(rxframe) == frame) &&
(rxframe.timestamp == timestamp) && (rxframe.iface_index == iface_index);
(rxframe.ts_monotonic == timestamp) && (rxframe.iface_index == iface_index);
}
TEST(CanIOManager, Reception)

13
libuavcan/test/transport/transfer.cpp
View File

@ -147,11 +147,11 @@ TEST(Transfer, RxFrameParse)
// Default
can_rx_frame.id = CanFrame::FLAG_EFF;
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
ASSERT_EQ(0, rx_frame.timestamp);
ASSERT_EQ(0, rx_frame.ts_monotonic);
ASSERT_EQ(0, rx_frame.iface_index);
// Custom
can_rx_frame.timestamp = 123;
can_rx_frame.ts_monotonic = 123;
can_rx_frame.iface_index = 2;
Frame frame;
@ -160,7 +160,7 @@ TEST(Transfer, RxFrameParse)
*static_cast<CanFrame*>(&can_rx_frame) = frame.compile();
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
ASSERT_EQ(123, rx_frame.timestamp);
ASSERT_EQ(123, rx_frame.ts_monotonic);
ASSERT_EQ(2, rx_frame.iface_index);
ASSERT_EQ(456, rx_frame.data_type_id);
ASSERT_EQ(uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, rx_frame.transfer_type);
@ -174,7 +174,7 @@ TEST(Transfer, FrameToString)
// RX frame default
RxFrame rx_frame;
EXPECT_EQ("dtid=0 tt=0 snid=0 idx=0 last=0 tid=0 payload=[] ts=0 iface=0", rx_frame.toString());
EXPECT_EQ("dtid=0 tt=0 snid=0 idx=0 last=0 tid=0 payload=[] ts_m=0 ts_utc=0 iface=0", rx_frame.toString());
// RX frame max len
rx_frame.data_type_id = Frame::DATA_TYPE_ID_MAX;
@ -186,11 +186,12 @@ TEST(Transfer, FrameToString)
rx_frame.payload_len = Frame::PAYLOAD_LEN_MAX;
for (int i = 0; i < rx_frame.payload_len; i++)
rx_frame.payload[i] = i;
rx_frame.timestamp = 0xFFFFFFFFFFFFFFFF;
rx_frame.ts_monotonic = 0xFFFFFFFFFFFFFFFF;
rx_frame.ts_utc = 0xFFFFFFFFFFFFFFFF;
rx_frame.iface_index = 3;
EXPECT_EQ(
"dtid=1023 tt=3 snid=127 idx=31 last=1 tid=15 payload=[00 01 02 03 04 05 06 07] ts=18446744073709551615 iface=3",
"dtid=1023 tt=3 snid=127 idx=31 last=1 tid=15 payload=[00 01 02 03 04 05 06 07] ts_m=18446744073709551615 ts_utc=18446744073709551615 iface=3",
rx_frame.toString());
// Plain frame default

36
libuavcan/test/transport/transfer_receiver.cpp
View File

@ -34,7 +34,7 @@ struct RxFrameGenerator
uavcan::RxFrame frm;
frm.iface_index = iface_index;
frm.timestamp = timestamp;
frm.ts_monotonic = timestamp;
frm.data_type_id = data_type_id;
frm.frame_index = frame_index;
@ -110,14 +110,14 @@ TEST(TransferReceiver, Basic)
* Empty
*/
ASSERT_EQ(TransferReceiver::DEFAULT_TRANSFER_INTERVAL, rcv.getInterval());
ASSERT_EQ(0, rcv.getLastTransferTimestamp());
ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic());
/*
* Single frame transfer with zero ts, must be ignored
*/
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "Foo", 0, true, 0, 0)));
ASSERT_EQ(TransferReceiver::DEFAULT_TRANSFER_INTERVAL, rcv.getInterval());
ASSERT_EQ(0, rcv.getLastTransferTimestamp());
ASSERT_EQ(0, rcv.getLastTransferTimestampMonotonic());
/*
* Valid compound transfer
@ -128,7 +128,7 @@ TEST(TransferReceiver, Basic)
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "12345678foo"));
ASSERT_EQ(TransferReceiver::DEFAULT_TRANSFER_INTERVAL, rcv.getInterval()); // Not initialized yet
ASSERT_EQ(100, rcv.getLastTransferTimestamp());
ASSERT_EQ(100, rcv.getLastTransferTimestampMonotonic());
/*
* Compound transfer mixed with invalid frames; buffer was not released explicitly
@ -146,7 +146,7 @@ TEST(TransferReceiver, Basic)
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "12345678abcdefgh"));
ASSERT_GT(TransferReceiver::DEFAULT_TRANSFER_INTERVAL, rcv.getInterval());
ASSERT_LT(TransferReceiver::MIN_TRANSFER_INTERVAL, rcv.getInterval());
ASSERT_EQ(1000, rcv.getLastTransferTimestamp());
ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic());
ASSERT_FALSE(rcv.isTimedOut(1000));
ASSERT_FALSE(rcv.isTimedOut(5000));
ASSERT_TRUE(rcv.isTimedOut(60000000));
@ -160,10 +160,10 @@ TEST(TransferReceiver, Basic)
ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer must be removed
ASSERT_GT(TransferReceiver::DEFAULT_TRANSFER_INTERVAL, rcv.getInterval());
ASSERT_EQ(2200, rcv.getLastTransferTimestamp());
ASSERT_EQ(2200, rcv.getLastTransferTimestampMonotonic());
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 3, 2500)));
ASSERT_EQ(2500, rcv.getLastTransferTimestamp());
ASSERT_EQ(2500, rcv.getLastTransferTimestampMonotonic());
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 0, 3000))); // Old TID
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "", 0, true, 15,3100))); // Old TID
@ -172,32 +172,32 @@ TEST(TransferReceiver, Basic)
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 15,3400))); // Old TID, wrong iface
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "", 0, true, 3, 3500))); // Old TID, wrong iface
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 8, 3600)));
ASSERT_EQ(3600, rcv.getLastTransferTimestamp());
ASSERT_EQ(3600, rcv.getLastTransferTimestampMonotonic());
/*
* Timeouts
*/
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 9, 100000))); // Wrong iface - ignored
CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 10, 600000))); // Accepted due to iface timeout
ASSERT_EQ(600000, rcv.getLastTransferTimestamp());
ASSERT_EQ(600000, rcv.getLastTransferTimestampMonotonic());
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 0, true, 11, 600100)));// Ignored - old iface 0
CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 11, 600100)));
ASSERT_EQ(600100, rcv.getLastTransferTimestamp());
ASSERT_EQ(600100, rcv.getLastTransferTimestampMonotonic());
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 11, 600100)));// Old TID
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 11, 100000000)));// Accepted - global timeout
ASSERT_EQ(100000000, rcv.getLastTransferTimestamp());
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic());
CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 12, 100000100)));
ASSERT_EQ(100000100, rcv.getLastTransferTimestamp());
ASSERT_EQ(100000100, rcv.getLastTransferTimestampMonotonic());
ASSERT_TRUE(rcv.isTimedOut(900000000));
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 0, false, 11, 900000000)));// Global timeout
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "12345678", 0, false, 11, 900000100)));// Wrong iface
CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 1, true, 11, 900000200)));// Wrong iface
CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", 1, true, 11, 900000200)));
ASSERT_EQ(900000000, rcv.getLastTransferTimestamp());
ASSERT_EQ(900000000, rcv.getLastTransferTimestampMonotonic());
ASSERT_FALSE(rcv.isTimedOut(1000));
ASSERT_FALSE(rcv.isTimedOut(900000200));
ASSERT_TRUE(rcv.isTimedOut(1000 * 1000000));
@ -231,7 +231,7 @@ TEST(TransferReceiver, OutOfBufferSpace_32bytes)
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 10, 100000300))); // 32
CHECK_COMPLETE( rcv.addFrame(gen(1, "", 4, true, 10, 100000400))); // 32
ASSERT_EQ(100000000, rcv.getLastTransferTimestamp());
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic());
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "12345678123456781234567812345678"));
/*
@ -243,7 +243,7 @@ TEST(TransferReceiver, OutOfBufferSpace_32bytes)
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 3, false, 11, 100001200))); // 32
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", 4, true, 11, 100001300))); // 40 // EOT, ignored - lost sync
ASSERT_EQ(100000000, rcv.getLastTransferTimestamp());
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic());
ASSERT_FALSE(bufmgr.access(gen.bufmgr_key)); // Buffer should be removed
}
@ -262,7 +262,7 @@ TEST(TransferReceiver, UnterminatedTransfer)
content += "12345678";
}
CHECK_COMPLETE(rcv.addFrame(gen(1, "12345678", uavcan::Frame::FRAME_INDEX_MAX, true, 0, 1100)));
ASSERT_EQ(1000, rcv.getLastTransferTimestamp());
ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic());
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), content));
}
@ -281,7 +281,7 @@ TEST(TransferReceiver, OutOfOrderFrames)
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "--------", 4, true, 10, 100000200))); // Out of order
CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, 10, 100000400)));
ASSERT_EQ(100000000, rcv.getLastTransferTimestamp());
ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic());
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "12345678qwertyuiabcd"));
}
@ -304,7 +304,7 @@ TEST(TransferReceiver, IntervalMeasurement)
CHECK_COMPLETE( rcv.addFrame(gen(1, "abcd", 2, true, tid.get(), timestamp)));
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "12345678qwertyuiabcd"));
ASSERT_EQ(timestamp, rcv.getLastTransferTimestamp());
ASSERT_EQ(timestamp, rcv.getLastTransferTimestampMonotonic());
timestamp += INTERVAL;
tid.increment();