tx_queue.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <gtest/gtest.h>
00006 #include <uavcan/transport/can_io.hpp>
00007 #include "can.hpp"
00008 
00009 
00010 static int getQueueLength(uavcan::CanTxQueue& queue)
00011 {
00012     const uavcan::CanTxQueue::Entry* p = queue.peek();
00013     int length = 0;
00014     while (p)
00015     {
00016         length++;
00017         p = p->getNextListNode();
00018     }
00019     return length;
00020 }
00021 
00022 static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame)
00023 {
00024     const uavcan::CanTxQueue::Entry* p = queue.peek();
00025     while (p)
00026     {
00027         if (frame == p->frame)
00028         {
00029             return true;
00030         }
00031         p = p->getNextListNode();
00032     }
00033     return false;
00034 }
00035 
00036 TEST(CanTxQueue, Qos)
00037 {
00038     const uavcan::CanIOFlags flags = 0;
00039     uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags);
00040     uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags);
00041 
00042     EXPECT_FALSE(e1.qosHigherThan(e2));
00043     EXPECT_FALSE(e2.qosHigherThan(e1));
00044     EXPECT_FALSE(e1.qosLowerThan(e2));
00045     EXPECT_FALSE(e2.qosLowerThan(e1));
00046 
00047     e2.qos = uavcan::CanTxQueue::Persistent;
00048 
00049     EXPECT_FALSE(e1.qosHigherThan(e2));
00050     EXPECT_TRUE(e2.qosHigherThan(e1));
00051     EXPECT_TRUE(e1.qosLowerThan(e2));
00052     EXPECT_FALSE(e2.qosLowerThan(e1));
00053 
00054     e1.qos = uavcan::CanTxQueue::Persistent;
00055     e1.frame.id -= 1;
00056 
00057     EXPECT_TRUE(e1.qosHigherThan(e2));
00058     EXPECT_FALSE(e2.qosHigherThan(e1));
00059     EXPECT_FALSE(e1.qosLowerThan(e2));
00060     EXPECT_TRUE(e2.qosLowerThan(e1));
00061 }
00062 
00063 TEST(CanTxQueue, TxQueue)
00064 {
00065     using uavcan::CanTxQueue;
00066     using uavcan::CanFrame;
00067 
00068     ASSERT_GE(40, sizeof(CanTxQueue::Entry)); // should be true for any platforms, though not required
00069 
00070     uavcan::PoolAllocator<40 * 4, 40> pool;
00071 
00072     SystemClockMock clockmock;
00073 
00074     CanTxQueue queue(pool, clockmock, 99999);
00075     EXPECT_TRUE(queue.isEmpty());
00076 
00077     const uavcan::CanIOFlags flags = 0;
00078 
00079     // Descending priority
00080     const CanFrame f0 = makeCanFrame(0, "f0", EXT);
00081     const CanFrame f1 = makeCanFrame(10, "f1", EXT);
00082     const CanFrame f2 = makeCanFrame(20, "f2", EXT);
00083     const CanFrame f3 = makeCanFrame(100, "f3", EXT);
00084     const CanFrame f4 = makeCanFrame(10000, "f4", EXT);
00085     const CanFrame f5 = makeCanFrame(99999, "f5", EXT);
00086     const CanFrame f5a = makeCanFrame(99999, "f5a", EXT);
00087     const CanFrame f6 = makeCanFrame(999999, "f6", EXT);
00088 
00089     /*
00090      * Priority insertion
00091      */
00092     queue.push(f4, tsMono(100), CanTxQueue::Persistent, flags);
00093     EXPECT_FALSE(queue.isEmpty());
00094     EXPECT_EQ(1, pool.getNumUsedBlocks());
00095     EXPECT_EQ(f4, queue.peek()->frame);
00096     EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5));
00097     EXPECT_TRUE(queue.topPriorityHigherOrEqual(f4)); // Equal
00098     EXPECT_FALSE(queue.topPriorityHigherOrEqual(f3));
00099 
00100     queue.push(f3, tsMono(200), CanTxQueue::Persistent, flags);
00101     EXPECT_EQ(f3, queue.peek()->frame);
00102 
00103     queue.push(f0, tsMono(300), CanTxQueue::Volatile, flags);
00104     EXPECT_EQ(f0, queue.peek()->frame);
00105 
00106     queue.push(f1, tsMono(400), CanTxQueue::Volatile, flags);
00107     EXPECT_EQ(f0, queue.peek()->frame);              // Still f0, since it is highest
00108     EXPECT_TRUE(queue.topPriorityHigherOrEqual(f0)); // Equal
00109     EXPECT_TRUE(queue.topPriorityHigherOrEqual(f1));
00110 
00111     // Out of free memory now
00112 
00113     EXPECT_EQ(0, queue.getRejectedFrameCount());
00114     EXPECT_EQ(4, getQueueLength(queue));
00115     EXPECT_TRUE(isInQueue(queue, f0));
00116     EXPECT_TRUE(isInQueue(queue, f1));
00117     EXPECT_TRUE(isInQueue(queue, f3));
00118     EXPECT_TRUE(isInQueue(queue, f4));
00119 
00120     const CanTxQueue::Entry* p = queue.peek();
00121     while (p)
00122     {
00123         std::cout << p->toString() << std::endl;
00124         p = p->getNextListNode();
00125     }
00126 
00127     /*
00128      * QoS
00129      */
00130     EXPECT_FALSE(isInQueue(queue, f2));
00131     queue.push(f2, tsMono(100), CanTxQueue::Volatile, flags);     // Non preempting, will be rejected
00132     EXPECT_FALSE(isInQueue(queue, f2));
00133 
00134     queue.push(f2, tsMono(500), CanTxQueue::Persistent, flags);   // Will override f1 (f3 and f4 are presistent)
00135     EXPECT_TRUE(isInQueue(queue, f2));
00136     EXPECT_FALSE(isInQueue(queue, f1));
00137     EXPECT_EQ(4, getQueueLength(queue));
00138     EXPECT_EQ(2, queue.getRejectedFrameCount());
00139     EXPECT_EQ(f0, queue.peek()->frame);            // Check the priority
00140 
00141     queue.push(f5, tsMono(600), CanTxQueue::Persistent, flags);   // Will override f0 (rest are presistent)
00142     EXPECT_TRUE(isInQueue(queue, f5));
00143     EXPECT_FALSE(isInQueue(queue, f0));
00144     EXPECT_EQ(f2, queue.peek()->frame);            // Check the priority
00145 
00146     // No volatile frames left now
00147 
00148     queue.push(f5a, tsMono(700), CanTxQueue::Persistent, flags);   // Will override f5 (same frame, same QoS)
00149     EXPECT_TRUE(isInQueue(queue, f5a));
00150     EXPECT_FALSE(isInQueue(queue, f5));
00151 
00152     queue.push(f6, tsMono(700), CanTxQueue::Persistent, flags);    // Will be rejected (lowest QoS)
00153     EXPECT_FALSE(isInQueue(queue, f6));
00154 
00155     EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
00156     EXPECT_TRUE(queue.topPriorityHigherOrEqual(f2));   // Equal
00157     EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5a));
00158     EXPECT_EQ(4, getQueueLength(queue));
00159     EXPECT_EQ(4, pool.getNumUsedBlocks());
00160     EXPECT_EQ(5, queue.getRejectedFrameCount());
00161     EXPECT_TRUE(isInQueue(queue, f2));
00162     EXPECT_TRUE(isInQueue(queue, f3));
00163     EXPECT_TRUE(isInQueue(queue, f4));
00164     EXPECT_TRUE(isInQueue(queue, f5a));
00165     EXPECT_EQ(f2, queue.peek()->frame);            // Check the priority
00166 
00167     /*
00168      * Expiration
00169      */
00170     clockmock.monotonic = 101;
00171     queue.push(f0, tsMono(800), CanTxQueue::Volatile, flags);     // Will replace f4 which is expired now
00172     EXPECT_TRUE(isInQueue(queue, f0));
00173     EXPECT_FALSE(isInQueue(queue, f4));
00174     EXPECT_EQ(6, queue.getRejectedFrameCount());
00175 
00176     clockmock.monotonic = 1001;
00177     queue.push(f5, tsMono(2000), CanTxQueue::Volatile, flags);    // Entire queue is expired
00178     EXPECT_TRUE(isInQueue(queue, f5));
00179     EXPECT_EQ(1, getQueueLength(queue));           // Just one entry left - f5
00180     EXPECT_EQ(1, pool.getNumUsedBlocks());         // Make sure there is no leaks
00181     EXPECT_EQ(10, queue.getRejectedFrameCount());
00182 
00183     queue.push(f0, tsMono(1000), CanTxQueue::Persistent, flags);  // This entry is already expired
00184     EXPECT_EQ(1, getQueueLength(queue));
00185     EXPECT_EQ(1, pool.getNumUsedBlocks());
00186     EXPECT_EQ(11, queue.getRejectedFrameCount());
00187 
00188     /*
00189      * Removing
00190      */
00191     queue.push(f4, tsMono(5000), CanTxQueue::Volatile, flags);
00192     EXPECT_EQ(2, getQueueLength(queue));
00193     EXPECT_TRUE(isInQueue(queue, f4));
00194     EXPECT_EQ(f4, queue.peek()->frame);
00195 
00196     CanTxQueue::Entry* entry = queue.peek();
00197     EXPECT_TRUE(entry);
00198     queue.remove(entry);
00199     EXPECT_FALSE(entry);
00200 
00201     EXPECT_FALSE(isInQueue(queue, f4));
00202     EXPECT_TRUE(isInQueue(queue, f5));
00203 
00204     entry = queue.peek();
00205     EXPECT_TRUE(entry);
00206     queue.remove(entry);
00207     EXPECT_FALSE(entry);
00208 
00209     EXPECT_FALSE(isInQueue(queue, f5));
00210 
00211     EXPECT_EQ(0, getQueueLength(queue));           // Final state checks
00212     EXPECT_EQ(0, pool.getNumUsedBlocks());
00213     EXPECT_EQ(11, queue.getRejectedFrameCount());
00214     EXPECT_FALSE(queue.peek());
00215     EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
00216 }