TransferReceiver timing fix

libuavcan/include/uavcan/transport/transfer_receiver.hpp

@@ -21,6 +21,8 @@ public:
     static const uint16_t MinTransferIntervalMSec     = 1;
     static const uint16_t MaxTransferIntervalMSec     = 0xFFFF;
     static const uint16_t DefaultTransferIntervalMSec = 1000;
+    static const uint16_t MessageMaxTFallbackMSec     = 1000;
+    static const uint16_t ServiceTFallbackMSec        = 500;
 
     static MonotonicDuration getDefaultTransferInterval()
     {
@@ -34,7 +36,7 @@ private:
 
     enum { IfaceIndexNotSet = MaxCanIfaces };
 
-    enum { ErrorCntMask = 31 };
+    enum { ErrorCntMask = 15 };
     enum { IfaceIndexMask = MaxCanIfaces };
 
     MonotonicTime prev_transfer_ts_;
@@ -48,14 +50,18 @@ private:
     TransferID tid_;    // 1 byte field
 
     // 1 byte aligned bitfields:
+    uint8_t tt_service_         : 1;
     uint8_t next_toggle_        : 1;
     uint8_t iface_index_        : 2;
-    mutable uint8_t error_cnt_  : 5;
+    mutable uint8_t error_cnt_  : 4;
 
     bool isInitialized() const { return iface_index_ != IfaceIndexNotSet; }
 
     bool isMidTransfer() const { return buffer_write_pos_ > 0; }
 
+    MonotonicDuration getTFallback() const;
+    MonotonicDuration getTTimeout() const;
+
     void registerError() const;
 
     TidRelation getTidRelation(const RxFrame& frame) const;
@@ -72,14 +78,21 @@ public:
         transfer_interval_msec_(DefaultTransferIntervalMSec),
         this_transfer_crc_(0),
         buffer_write_pos_(0),
+        tt_service_(false),
         next_toggle_(false),
         iface_index_(IfaceIndexNotSet),
         error_cnt_(0)
-    {
+    { }
-#if UAVCAN_DEBUG
+
-        StaticAssert<sizeof(TransferReceiver) == 32>::check();
+    TransferReceiver(const TransferType transfer_type) :
-#endif
+        transfer_interval_msec_(DefaultTransferIntervalMSec),
-    }
+        this_transfer_crc_(0),
+        buffer_write_pos_(0),
+        tt_service_((transfer_type == TransferTypeServiceRequest) || (transfer_type == TransferTypeServiceResponse)),
+        next_toggle_(false),
+        iface_index_(IfaceIndexNotSet),
+        error_cnt_(0)
+    { }
 
     bool isTimedOut(MonotonicTime current_ts) const;
 

libuavcan/src/transport/uc_transfer_listener.cpp

@@ -207,7 +207,7 @@ void TransferListenerBase::handleFrame(const RxFrame& frame)
                 return;
             }
 
-            TransferReceiver new_recv;
+            TransferReceiver new_recv(frame.getTransferType());
             recv = receivers_.insert(key, new_recv);
             if (recv == NULL)
             {

libuavcan/src/transport/uc_transfer_receiver.cpp

@@ -14,6 +14,33 @@ namespace uavcan
 const uint16_t TransferReceiver::MinTransferIntervalMSec;
 const uint16_t TransferReceiver::MaxTransferIntervalMSec;
 const uint16_t TransferReceiver::DefaultTransferIntervalMSec;
+const uint16_t TransferReceiver::MessageMaxTFallbackMSec;
+const uint16_t TransferReceiver::ServiceTFallbackMSec;
+
+MonotonicDuration TransferReceiver::getTFallback() const
+{
+    if (tt_service_)
+    {
+        return MonotonicDuration::fromMSec(ServiceTFallbackMSec);
+    }
+    else
+    {
+        const MonotonicDuration t_fallback = MonotonicDuration::fromMSec(transfer_interval_msec_ * 2);
+        return min(t_fallback, MonotonicDuration::fromMSec(MessageMaxTFallbackMSec));
+    }
+}
+
+MonotonicDuration TransferReceiver::getTTimeout() const
+{
+    if (tt_service_)
+    {
+        return getTFallback();
+    }
+    else
+    {
+        return getTFallback() * 3;
+    }
+}
 
 void TransferReceiver::registerError() const
 {
@@ -183,16 +210,13 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
 
 bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const
 {
-    static const int64_t IntervalMult = (1 << TransferID::BitLen) / 2 + 1;
+    return (current_ts - this_transfer_ts_) > getTTimeout();
-    if (current_ts <= this_transfer_ts_)
-    {
-        return false;
-    }
-    return (current_ts - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_msec_) * 1000 * IntervalMult);
 }
 
 TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
 {
+    UAVCAN_ASSERT((getDataTypeKindForTransferType(frame.getTransferType()) == DataTypeKindService) == tt_service_);
+
     if ((frame.getMonotonicTimestamp().isZero()) ||
         (frame.getMonotonicTimestamp() < prev_transfer_ts_) ||
         (frame.getMonotonicTimestamp() < this_transfer_ts_))
@@ -206,20 +230,17 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
     const bool same_iface = frame.getIfaceIndex() == iface_index_;
     const bool first_fame = frame.isStartOfTransfer();
     const TidRelation tid_rel = getTidRelation(frame);
-    const bool iface_timed_out =
+    const bool iface_timed_out = (frame.getMonotonicTimestamp() - this_transfer_ts_) > getTFallback();
-        (frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_msec_) * 1000 * 2);
 
     // FSM, the hard way
     const bool need_restart =
-        (not_initialized) ||
+        not_initialized ||
-        (receiver_timed_out) ||
+        receiver_timed_out ||
-        (same_iface && first_fame && (tid_rel == TidFuture)) ||
+        ((same_iface || iface_timed_out) && first_fame && (tid_rel == TidFuture));
-        (iface_timed_out && first_fame && (tid_rel == TidFuture));
 
     if (need_restart)
     {
-        const bool error = !not_initialized && !receiver_timed_out;
+        if (!not_initialized && !receiver_timed_out)
-        if (error)
         {
             registerError();
         }

libuavcan/test/transport/transfer_receiver.cpp

@@ -78,7 +78,9 @@ struct Context
     uavcan::TransferReceiver receiver;  // Must be default constructible and copyable
     uavcan::TransferBufferManager<BufSize, 1> bufmgr;
 
-    Context() : bufmgr(pool)
+    Context(uavcan::TransferType tt) :
+        receiver(tt),
+        bufmgr(pool)
     {
         assert(pool.allocate(1) == NULL);
     }
@@ -120,7 +122,7 @@ static bool matchBufferContent(const uavcan::ITransferBuffer* tbb, const std::st
 TEST(TransferReceiver, Basic)
 {
     using uavcan::TransferReceiver;
-    Context<32> context;
+    Context<32> context(uavcan::TransferTypeMessageBroadcast);
     RxFrameGenerator gen(789);
     uavcan::TransferReceiver& rcv = context.receiver;
     uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
@@ -259,7 +261,7 @@ TEST(TransferReceiver, Basic)
 
 TEST(TransferReceiver, OutOfBufferSpace_32bytes)
 {
-    Context<32> context;
+    Context<32> context(uavcan::TransferTypeMessageBroadcast);
     RxFrameGenerator gen(789);
     uavcan::TransferReceiver& rcv = context.receiver;
     uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
@@ -297,7 +299,7 @@ TEST(TransferReceiver, OutOfBufferSpace_32bytes)
 
 TEST(TransferReceiver, OutOfOrderFrames)
 {
-    Context<32> context;
+    Context<32> context(uavcan::TransferTypeMessageBroadcast);
     RxFrameGenerator gen(789);
     uavcan::TransferReceiver& rcv = context.receiver;
     uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
@@ -321,7 +323,7 @@ TEST(TransferReceiver, OutOfOrderFrames)
 
 TEST(TransferReceiver, IntervalMeasurement)
 {
-    Context<32> context;
+    Context<32> context(uavcan::TransferTypeMessageBroadcast);
     RxFrameGenerator gen(789);
     uavcan::TransferReceiver& rcv = context.receiver;
     uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
@@ -351,7 +353,7 @@ TEST(TransferReceiver, IntervalMeasurement)
 
 TEST(TransferReceiver, Restart)
 {
-    Context<32> context;
+    Context<32> context(uavcan::TransferTypeMessageBroadcast);
     RxFrameGenerator gen(789);
     uavcan::TransferReceiver& rcv = context.receiver;
     uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
@@ -379,13 +381,13 @@ TEST(TransferReceiver, Restart)
      * Begins OK, gets an iface timeout, switches to another iface
      */
     CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET100, 1, 103000500), bk)); // Begin
-    CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET001, 1, 106000500), bk)); // 3 sec later, iface timeout
+    CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET001, 1, 104000500), bk)); // 1 sec later, iface timeout
-    CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 1, 106000600), bk)); // Same TID, another iface - ignore
+    CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 1, 104000600), bk)); // Same TID, another iface - ignore
-    CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 2, 106000700), bk)); // Not first frame - ignore
+    CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "-------", SET001, 2, 104000700), bk)); // Not first frame - ignore
-    CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 2, 106000800), bk)); // First, another iface - restart
+    CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, 2, 104000800), bk)); // First, another iface - restart
-    CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET010, 1, 106000600), bk)); // Old iface - ignore
+    CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "-------", SET010, 1, 104000600), bk)); // Old iface - ignore
-    CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET001, 2, 106000900), bk)); // Continuing
+    CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET001, 2, 104000900), bk)); // Continuing
-    CHECK_COMPLETE(    rcv.addFrame(gen(0, "1234567", SET010, 2, 106000910), bk)); // Done
+    CHECK_COMPLETE(    rcv.addFrame(gen(0, "1234567", SET010, 2, 104000910), bk)); // Done
 
     ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "3456712345671234567"));
     ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
@@ -397,7 +399,7 @@ TEST(TransferReceiver, Restart)
 
 TEST(TransferReceiver, UtcTransferTimestamping)
 {
-    Context<32> context;
+    Context<32> context(uavcan::TransferTypeMessageBroadcast);
     RxFrameGenerator gen(789);
     uavcan::TransferReceiver& rcv = context.receiver;
     uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
@@ -448,96 +450,113 @@ TEST(TransferReceiver, UtcTransferTimestamping)
 
 TEST(TransferReceiver, HeaderParsing)
 {
-    Context<32> context;
+    static const std::string SFT_PAYLOAD = "1234567";
-    RxFrameGenerator gen(123);
-    uavcan::TransferReceiver& rcv = context.receiver;
-    uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
-
-    static const uavcan::TransferType ADDRESSED_TRANSFER_TYPES[2] =
-    {
-        uavcan::TransferTypeServiceRequest,
-        uavcan::TransferTypeServiceResponse
-    };
 
     uavcan::TransferID tid;
 
     /*
-     * MFT, message broadcasting
+     * Broadcast
      */
     {
-        gen.bufmgr_key =
+        Context<32> context(uavcan::TransferTypeMessageBroadcast);
-            uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast);
+        RxFrameGenerator gen(123);
-        uavcan::TransferBufferAccessor bk1(context.bufmgr, gen.bufmgr_key);
+        uavcan::TransferReceiver& rcv = context.receiver;
+        uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
 
-        CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), 1), bk1));
+        /*
-        CHECK_COMPLETE(    rcv.addFrame(gen(0, "abcd",     SET011,  tid.get(), 2), bk1));
+         * MFT, message broadcasting
+         */
+        {
+            gen.bufmgr_key =
+                uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast);
+            uavcan::TransferBufferAccessor bk1(context.bufmgr, gen.bufmgr_key);
 
-        ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd"));
+            CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), 1), bk1));
-        ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+            CHECK_COMPLETE(    rcv.addFrame(gen(0, "abcd",     SET011,  tid.get(), 2), bk1));
 
-        tid.increment();
+            ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd"));
-        bk1.remove();
+            ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+
+            tid.increment();
+            bk1.remove();
+        }
+
+        /*
+         * SFT, message broadcasting
+         */
+        {
+            gen.bufmgr_key =
+                uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast);
+            uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key);
+
+            const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), 1000);
+
+            CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk));
+            ASSERT_EQ(0x0000, rcv.getLastTransferCrc());                                     // Default value - zero
+
+            // All bytes are payload, zero overhead
+            ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr()));
+
+            tid.increment();
+        }
     }
 
     /*
-     * MFT, service request/response
+     * Unicast
      */
-    for (unsigned i = 0; i < (sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++)
     {
-        gen.bufmgr_key =
+        Context<32> context(uavcan::TransferTypeServiceRequest);
-            uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]);
+        RxFrameGenerator gen(123);
-        uavcan::TransferBufferAccessor bk2(context.bufmgr, gen.bufmgr_key);
+        uavcan::TransferReceiver& rcv = context.receiver;
+        uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
 
-        const uint64_t ts_monotonic = i + 10;
+        static const uavcan::TransferType ADDRESSED_TRANSFER_TYPES[2] =
+        {
+            uavcan::TransferTypeServiceRequest,
+            uavcan::TransferTypeServiceResponse
+        };
 
-        CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), ts_monotonic), bk2));
+        /*
-        CHECK_COMPLETE(    rcv.addFrame(gen(0, "abcd",    SET011,  tid.get(), ts_monotonic), bk2));
+         * MFT, service request/response
+         */
+        for (unsigned i = 0; i < (sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++)
+        {
+            gen.bufmgr_key =
+                uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]);
+            uavcan::TransferBufferAccessor bk2(context.bufmgr, gen.bufmgr_key);
 
-        ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd"));
+            const uint64_t ts_monotonic = i + 10;
-        ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
 
-        tid.increment();
+            CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "1234567", SET100, tid.get(), ts_monotonic), bk2));
-        bk2.remove();
+            CHECK_COMPLETE(    rcv.addFrame(gen(0, "abcd",    SET011,  tid.get(), ts_monotonic), bk2));
+
+            ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "34567abcd"));
+            ASSERT_EQ(0x3231, rcv.getLastTransferCrc());
+
+            tid.increment();
+            bk2.remove();
+        }
+
+        /*
+         * SFT, message unicast, service request/response
+         */
+        for (unsigned i = 0; i < int(sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++)
+        {
+            gen.bufmgr_key =
+                uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]);
+            uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key);
+
+            const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), i + 10000U);
+
+            CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk));
+            ASSERT_EQ(0x0000, rcv.getLastTransferCrc());                                     // Default value - zero
+
+            // First byte must be ignored
+            ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr()));
+
+            tid.increment();
+        }
     }
 
-    /*
-     * SFT, message broadcasting
-     */
-    static const std::string SFT_PAYLOAD = "1234567";
 
-    {
-        gen.bufmgr_key =
-            uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), uavcan::TransferTypeMessageBroadcast);
-        uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key);
-
-        const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), 1000);
-
-        CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk));
-        ASSERT_EQ(0x0000, rcv.getLastTransferCrc());                                     // Default value - zero
-
-        // All bytes are payload, zero overhead
-        ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr()));
-
-        tid.increment();
-    }
-
-    /*
-     * SFT, message unicast, service request/response
-     */
-    for (unsigned i = 0; i < int(sizeof(ADDRESSED_TRANSFER_TYPES) / sizeof(ADDRESSED_TRANSFER_TYPES[0])); i++)
-    {
-        gen.bufmgr_key =
-            uavcan::TransferBufferManagerKey(gen.bufmgr_key.getNodeID(), ADDRESSED_TRANSFER_TYPES[i]);
-        uavcan::TransferBufferAccessor bk(context.bufmgr, gen.bufmgr_key);
-
-        const uavcan::RxFrame frame = gen(0, SFT_PAYLOAD, SET110, tid.get(), i + 10000U);
-
-        CHECK_SINGLE_FRAME(rcv.addFrame(frame, bk));
-        ASSERT_EQ(0x0000, rcv.getLastTransferCrc());                                     // Default value - zero
-
-        // First byte must be ignored
-        ASSERT_TRUE(std::equal(SFT_PAYLOAD.begin(), SFT_PAYLOAD.end(), frame.getPayloadPtr()));
-
-        tid.increment();
-    }
 }