diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index 9f355c0365769056e174e6557f02e21f69319449..79a63e0cc9e6b940ebac1a3acd1d74c14338efa8 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -348,11 +348,12 @@ void MavlinkDriver<PktLength, OutQueueSize>::runSender()
// Get the first packet in the queue, without removing it
pkt = outQueue.get();
- uint64_t age = (uint64_t)miosix::getTick() - pkt.timestamp();
// If the packet is ready or too old, send it
+ uint64_t age = TimestampTimer::getInstance().getTimestamp() -
+ pkt.getTimestamp();
if (pkt.isReady() || age >= outBufferMaxAge)
{
- outQueue.pop(); // remove from queue
+ outQueue.pop(); // Remove the packet from queue
LOG_DEBUG(logger, "Sending packet. Size: {} (age: {})",
pkt.size(), age);
diff --git a/src/shared/utils/collections/CircularBuffer.h b/src/shared/utils/collections/CircularBuffer.h
index fa6a2e58286996793ee9c7b949b26da61e4cb1db..6182bdcc5b7967af9b90570dada7d62c635438ce 100644
--- a/src/shared/utils/collections/CircularBuffer.h
+++ b/src/shared/utils/collections/CircularBuffer.h
@@ -32,7 +32,7 @@ namespace Boardcore
{
/**
- * Implementation of an non-synchronized circular buffer
+ * Implementation of an non-synchronized circular buffer.
*/
template <typename T, unsigned int Size>
class CircularBuffer
@@ -41,21 +41,25 @@ class CircularBuffer
public:
CircularBuffer() {}
+
virtual ~CircularBuffer() {}
/**
- * Puts a copy of the element in the buffer
- * @param elem element
+ * @brief Puts a copy of the element in the buffer.
+ *
+ * @param elem Element to be added to the queue.
+ * @return The element added.
*/
virtual T& put(const T& elem)
{
buffer[writePtr] = elem;
T& added = buffer[writePtr];
+ // Advance the read pointer if the two pointers match
if (!empty && writePtr == readPtr)
- {
readPtr = (readPtr + 1) % Size;
- }
+
+ // Advance the write pointer
writePtr = (writePtr + 1) % Size;
empty = false;
@@ -64,20 +68,19 @@ public:
}
/**
- * Gets an element from the buffer, without removing it
- * Index starts from the oldest element in the buffer: get(0) returns the
- * same element as get()
+ * @brief Gets an element from the buffer, without removing it.
*
- * @warning Remember to catch the exception!
- * @throw range_error if index >= count()
+ * Index starts from the oldest element in the buffer.
+ * get() returns the first element.
*
- * @param i Index of the elemnt to get, starting from the oldest
- *
- * @return the element
+ * @warning Remember to catch the exception!
+ * @throw range_error if index >= count().
+ * @param i Index of the element to get, starting from the oldest.
+ * @return The element.
*/
- virtual T& get(unsigned int i)
+ virtual T& get(unsigned int i = 0)
{
- if (i < CircularBuffer<T, Size>::count())
+ if (i < count())
{
int ptr = (readPtr + i) % Size;
return buffer[ptr];
@@ -89,33 +92,18 @@ public:
/**
* @brief Returns the last element added in the buffer.
*
- * @throw range_error if buffer is empty
* @warning Remember to catch the exception!
- * @return the element
+ * @throw range_error if buffer is empty.
+ * @return The element.
*/
virtual T& last() { return get(count() - 1); }
/**
- * Gets the first element from the buffer, without removing it
- * @throw range_error if buffer is empty
- * @warning Remember to catch the exception!
- * @return the element
- */
- virtual T& get()
- {
- if (!empty)
- {
- return buffer[readPtr];
- }
- else
- throw range_error("CircularBuffer is empty!");
- }
-
- /**
- * Pops the first element in the buffer.
- * @throw range_error if buffer is empty
+ * @brief Pops the first element in the buffer.
+ *
* @warning Remember to catch the exception!
- * @return the element that has been popped
+ * @throw range_error if buffer is empty.
+ * @return The element that has been popped.
*/
virtual const T& pop()
{
@@ -133,8 +121,9 @@ public:
}
/**
- * Counts the elements in the buffer
- * @return number of elements in the buffer
+ * @brief Counts the elements in the buffer.
+ *
+ * @return Number of elements in the buffer.
*/
virtual size_t count() const
{
@@ -157,16 +146,19 @@ public:
return CircularBuffer<T, Size>::count() == Size;
}
/**
- * Returns the maximum number of elements that can be stored in the buffer
- * @return buffer size
+ * @brief Returns the maximum number of elements that can be stored in the
+ * buffer.
+ *
+ * @return Buffer size.
*/
size_t getSize() const { return Size; }
protected:
T buffer[Size];
- size_t writePtr = 0, readPtr = 0;
- bool empty = true;
+ size_t writePtr = 0;
+ size_t readPtr = 0;
+ bool empty = true;
};
} // namespace Boardcore
diff --git a/src/shared/utils/collections/IRQCircularBuffer.h b/src/shared/utils/collections/IRQCircularBuffer.h
index a6a02e63664400c7b5efc1fc9af1287613862598..e27d15e2a37652df6e822d29fd8d3ff0cd3af104 100644
--- a/src/shared/utils/collections/IRQCircularBuffer.h
+++ b/src/shared/utils/collections/IRQCircularBuffer.h
@@ -34,7 +34,8 @@ namespace Boardcore
{
/**
- * Implementation of a synchronized circular buffer
+ * Implementation of a synchronized circular buffer that can be used inside
+ * interrupt service routines.
*/
template <typename T, unsigned int Size>
class IRQCircularBuffer : public CircularBuffer<T, Size>
@@ -44,8 +45,7 @@ class IRQCircularBuffer : public CircularBuffer<T, Size>
public:
/**
- * Puts a copy of the element in the buffer
- * @param elem element
+ * @brief Puts a copy of the element in the buffer.
*/
T& put(const T& elem) override
{
@@ -55,37 +55,28 @@ public:
}
/**
- * Gets the first element from the buffer, without removing it
- * @warning Remember to catch the exception!
- * @return the element
- * @throws range_error if buffer is empty
- */
- T& get() override
- {
- FastInterruptDisableLock d;
- return Super::get();
- }
-
- /**
- * Gets an element from the buffer, without removing it
- * Index starts at the element returned by get() or pop(): get(0) is
- * the same as get()
+ * @brief Gets an element from the buffer, without removing it.
+ *
+ * Index starts from the oldest element in the buffer.
+ * get() returns the first element.
*
* @warning Remember to catch the exception!
- * @return the element
- * @throws range_error if buffer is empty
+ * @throw range_error if index >= count().
+ * @param i Index of the element to get, starting from the oldest.
+ * @return The element.
*/
- T& get(unsigned int i) override
+ T& get(unsigned int i = 0) override
{
FastInterruptDisableLock d;
return Super::get(i);
}
/**
- * Pops the first element in the buffer.
+ * @brief Pops the first element in the buffer.
+ *
* @warning Remember to catch the exception!
- * @return the element that has been popped
- * @throws range_error if buffer is empty
+ * @throw range_error if buffer is empty.
+ * @return The element that has been popped.
*/
const T& pop() override
{
@@ -94,8 +85,9 @@ public:
}
/**
- * Counts the elements in the buffer
- * @return number of elements in the buffer
+ * @brief Counts the elements in the buffer.
+ *
+ * @return Number of elements in the buffer.
*/
size_t count() const override
{
@@ -116,9 +108,9 @@ public:
}
/**
- * Puts a copy of the element in the buffer
- * Only to be called inside an ISR or with interrupts disabled
- * @param elem element
+ * @brief Puts a copy of the element in the buffer.
+ *
+ * @warning Only to be called inside an ISR or with interrupts disabled.
*/
T& IRQput(const T& elem)
{
@@ -127,8 +119,10 @@ public:
}
/**
- * Puts a copy of the element in the buffer
- * Only to be called inside an ISR or with interrupts disabled
+ * @brief Puts a copy of the element in the buffer.
+ *
+ * @warning Only to be called inside an ISR or with interrupts disabled.
+ *
* @param elem element
* @param hppw Set to true if the woken thread is higher priority than the
* current one, unchanged otherwise
@@ -137,68 +131,59 @@ public:
{
if (waiting && (waiting->IRQgetPriority() >
Thread::IRQgetCurrentThread()->IRQgetPriority()))
- {
hppw = true;
- }
IRQwakeWaitingThread();
return Super::put(elem);
}
/**
- * Gets the first element from the buffer, without removing it
- * Only to be called inside an ISR or with interrupts disabled
- * @warning Remember to catch the exception!
- * @return the element
- * @throws range_error if buffer is empty
- */
- T& IRQget() { return Super::get(); }
-
- /**
- * Gets an element from the buffer, without removing it
- * Index starts at the element returned by get() or pop(): get(0) is
- * the same as get()
- * @warning Only to be called inside an ISR or with interrupts disabled
+ * @brief Gets an element from the buffer, without removing it.
+ *
+ * @warning Only to be called inside an ISR or with interrupts disabled.
+ *
+ * Index starts from the oldest element in the buffer.
+ * get() returns the first element.
+ *
* @warning Remember to catch the exception!
- * @return the element
- * @throws range_error if buffer is empty
+ * @throw range_error if index >= count().
+ * @param i Index of the element to get, starting from the oldest.
+ * @return The element.
*/
- T& IRQget(unsigned int i) { return Super::get(i); }
+ T& IRQget(unsigned int i = 0) { return Super::get(i); }
/**
- * Pops the first element in the buffer.
- * @warning Only to be called inside an ISR or with interrupts disabled
+ * @brief Pops the first element in the buffer.
+ *
+ * @warning Only to be called inside an ISR or with interrupts disabled.
+ *
* @warning Remember to catch the exception!
- * @return the element that has been popped
- * @throws range_error if buffer is empty
+ * @throw range_error if buffer is empty.
+ * @return The element that has been popped.
*/
const T& IRQpop() { return Super::pop(); }
/**
- * Counts the elements in the buffer
- * @warning Only to be called inside an ISR or with interrupts disabled
- * @return number of elements in the buffer
+ * @brief Counts the elements in the buffer.
+ *
+ * @warning Only to be called inside an ISR or with interrupts disabled.
+ *
+ * @return Number of elements in the buffer.
*/
size_t IRQcount() const { return Super::count(); }
/**
- * @brief Returns true if the buffer is empty
- *
- * @warning Only to be called inside an ISR or with interrupts disabled
- * @return empty or not
+ * @warning Only to be called inside an ISR or with interrupts disabled.
*/
bool IRQisEmpty() const { return Super::isEmpty(); }
/**
- * @brief Returns true if the buffer is full
- *
- * @warning Only to be called inside an ISR or with interrupts disabled
- * @return buffer full or not
+ * @warning Only to be called inside an ISR or with interrupts disabled.
*/
bool IRQisFull() const { return Super::isFull(); }
/**
- * @brief Waits until the buffer contains at least one element
+ * @brief Waits until the buffer contains at least one element.
*/
void waitUntilNotEmpty()
{
@@ -228,4 +213,5 @@ private:
Thread* waiting = nullptr;
};
-} // namespace Boardcore
\ No newline at end of file
+
+} // namespace Boardcore
diff --git a/src/shared/utils/collections/SyncCircularBuffer.h b/src/shared/utils/collections/SyncCircularBuffer.h
index 9e05227df79823f100b0ff915530c451fd0862b8..cab438fbb629c00aa98122ed9b3512deca329e57 100644
--- a/src/shared/utils/collections/SyncCircularBuffer.h
+++ b/src/shared/utils/collections/SyncCircularBuffer.h
@@ -55,37 +55,28 @@ public:
}
/**
- * Gets the first element from the buffer, without removing it
- * @warning Remember to catch the exception!
- * @return the element
- * @throws range_error if buffer is empty
- */
- T& get() override
- {
- Lock<FastMutex> l(mutex);
- return Super::get();
- }
-
- /**
- * Gets an element from the buffer, without removing it
- * Index starts at the element returned by get() or pop(): get(0) is
- * the same as get()
+ * @brief Gets an element from the buffer, without removing it.
+ *
+ * Index starts from the oldest element in the buffer.
+ * get() returns the first element.
*
* @warning Remember to catch the exception!
- * @return the element
- * @throws range_error if buffer is empty
+ * @throw range_error if index >= count().
+ * @param i Index of the element to get, starting from the oldest.
+ * @return The element.
*/
- T& get(unsigned int i) override
+ T& get(unsigned int i = 0) override
{
Lock<FastMutex> l(mutex);
return Super::get(i);
}
/**
- * Pops the first element in the buffer.
+ * @brief Pops the first element in the buffer.
+ *
* @warning Remember to catch the exception!
- * @return the element that has been popped
- * @throws range_error if buffer is empty
+ * @throw range_error if buffer is empty.
+ * @return The element that has been popped.
*/
const T& pop() override
{
@@ -94,8 +85,9 @@ public:
}
/**
- * Counts the elements in the buffer
- * @return number of elements in the buffer
+ * @brief Counts the elements in the buffer.
+ *
+ * @return Number of elements in the buffer.
*/
size_t count() const override
{
@@ -116,7 +108,7 @@ public:
}
/**
- * @brief Waits until the buffer contains at least one element
+ * @brief Waits until the buffer contains at least one element.
*/
void waitUntilNotEmpty()
{
diff --git a/src/shared/utils/collections/SyncPacketQueue.h b/src/shared/utils/collections/SyncPacketQueue.h
index a2bd35e33855484f33c6b96dae9655492ca77dd5..3d646d04da08bd600d663c951871e0960ee9b85d 100644
--- a/src/shared/utils/collections/SyncPacketQueue.h
+++ b/src/shared/utils/collections/SyncPacketQueue.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2019 Skyward Experimental Rocketry
- * Author: Alvise de'Faveri Tron
+/* Copyright (c) 2019-2022 Skyward Experimental Rocketry
+ * Author: Alvise de'Faveri Tron, Davide Mor, Alberto Nidasio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -22,6 +22,7 @@
#pragma once
+#include <drivers/timer/TimestampTimer.h>
#include <miosix.h>
#include <utils/Debug.h>
@@ -46,10 +47,9 @@ namespace Boardcore
* @brief The Packet class is used for packing together messages with variable
* lengths into a fixed size packet. Useful for telemetry.
*
- * The buffer can only be appended, read or flushed. The caller can also mark
- * the packet as ready to be sent.
+ * Data can only be appended to the payload. The packet can be marked ready.
*
- * @tparam len Maximum length for the packet.
+ * @tparam len Packet's payload length.
*/
template <unsigned int len>
class Packet
@@ -59,7 +59,7 @@ public:
/**
* @brief Reserves a fixed length for the packet.
*/
- Packet() : msgCounter(0), ts(0), ready(false) { content.reserve(len); };
+ Packet() { content.reserve(len); };
/**
* @brief Clears the buffer.
@@ -69,21 +69,23 @@ public:
/**
* @brief Append a given message to the packet.
*
- * If it's the first message, also set the timestamp.
+ * If the message can't fit inside the remaining space only the first bytes
+ * are copied.
*
* @param msg The message to be appended.
- * @param msgLen Length of msg.
+ * @param msgLen Length of the message.
* @return How many bytes were actually appended.
*/
size_t append(const uint8_t* msg, size_t msgLen);
/**
- * @brief Mark the packet as ready to be sent.
+ * @brief Mark the packet as ready.
*/
inline void markAsReady() { ready = true; }
/**
- * @brief Copies the content of the buffer at a given address.
+ * @brief Copies the content of the payload at the given address.
+ *
* @param buf Where to copy the content.
* @return How many bytes where copied, i.e. the size of the packet.
*/
@@ -112,10 +114,10 @@ public:
/**
* @return The timestamp of the first successful call to append().
*/
- inline uint64_t timestamp() const { return ts; }
+ inline uint64_t getTimestamp() const { return timestamp; }
/**
- * @return The occupied portion of the buffer (bytes).
+ * @return The occupied portion of the buffer [bytes].
*/
inline size_t size() const { return content.size(); }
@@ -133,7 +135,7 @@ public:
/**
* @brief Print information about this object.
*
- * @param os For example, std::cout
+ * @param os For example, std::cout.
*/
void print(std::ostream& os) const;
@@ -141,9 +143,9 @@ public:
std::vector<uint8_t> content;
private:
- unsigned int msgCounter;
- uint64_t ts;
- bool ready;
+ unsigned int msgCounter = 0;
+ uint64_t timestamp = 0;
+ bool ready = false;
};
template <unsigned int len>
@@ -156,9 +158,7 @@ size_t Packet<len>::append(const uint8_t* msg, size_t msgLen)
{
// Set the packet's timestamp when the first message is inserted
if (content.size() == 0)
- {
- ts = miosix::getTick();
- }
+ timestamp = TimestampTimer::getInstance().getTimestamp();
// Append the message to the packet
content.insert(content.end(), msg, msg + msgLen);
@@ -173,7 +173,7 @@ void Packet<len>::clear()
{
content.clear();
msgCounter = 0;
- ts = 0;
+ timestamp = 0;
ready = false;
}
@@ -187,188 +187,167 @@ size_t Packet<len>::dump(uint8_t* buf)
template <unsigned int len>
void Packet<len>::print(std::ostream& os) const
{
- os << "timestamp=" << ts << ", ready=" << ready
+ os << "timestamp=" << timestamp << ", ready=" << ready
<< ", size=" << content.size() << ", msgCounter=" << msgCounter
<< ", content= ";
for (auto const& i : content)
- {
os << i;
- }
os << '\n';
}
-/******************************************************************************
- * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets. The difference
- * is that you pop() Packets but you append() bytes. The bytes will be appended
- * to the first available packet. This class is suitable for synchronization
- * between two threads.
+/**
+ * @brief A SyncPacketQueue is a SyncCircularBuffer of Packets.
*
- * @tparam pktLen Maximum length of each packet. (bytes)
- * @tparam pktNum Total number of packets.
- ******************************************************************************/
+ * The difference is that you pop() Packets but you append() bytes. The bytes
+ * will be appended to the first available packet and the next ones.
+ * This class is suitable for synchronization between two threads.
+ *
+ * @tparam pktLen Maximum length of each packet [bytes].
+ * @tparam pktNum Total number of packets.
+ */
template <unsigned int pktLen, unsigned int pktNum>
class SyncPacketQueue
{
- using Pkt = Packet<pktLen>;
-
public:
/**
- * @brief Try to append a given message to the last packet. If there isn't
- * enough space, the packet is marked as ready and the message is appended
- * to the next packet. If there are no more available packets, the oldest
- * one is overwritten.
+ * @brief Try to append a given message to the packets queue.
+ *
+ * The message is appended to the last packet and if the space isn't enough,
+ * it is divided into successive packets. If there are no more available
+ * packets, the oldest one is overwritten.
+ *
+ * The message isn't added to the queue if there is no space considering all
+ * the queue packets.
*
- * @param msg the message to be appended
- * @param msgLen length of msg
- * @return true if the message was appended correctly
- * @return false if there isn't enough space for the message
+ * @param msg The message to be appended.
+ * @param msgLen Length of the message [bytes].
+ * @return True if the message was appended.
*/
- int put(uint8_t* msg, size_t msgLen)
+ bool put(uint8_t* msg, size_t msgLen)
{
- int dropped = 0;
-
+ // Check if the message is empty
if (msgLen == 0)
- {
- return -1;
- }
+ return false;
+
+ // Check if the queue can hold the packet
+ if (msgLen > pktLen * pktNum)
+ return false;
{
+ // Lock the mutex on the buffer
Lock<FastMutex> l(mutex);
+
// Add an element if there isn't any
if (buffer.count() == 0)
- {
- buffer.put(Pkt{});
- }
+ buffer.put({});
+ // Write all the packet
while (msgLen > 0)
{
+ // If the last packet is ready append a new one
if (buffer.last().isReady())
- {
- if (buffer.isFull())
- {
- // We have dropped a packet
- ++dropped;
- }
+ buffer.put({});
- // If the last pkt is ready, append a new one
- buffer.put(Pkt{});
- // FIXME(davide.mor): Figure out quantum shenanigans
- // uncommenting the following line causes everything to
- // break, why?
+ // Append what data is possible to the last packet
+ size_t appendedLength = buffer.last().append(msg, msgLen);
- // last = buffer.last();
- }
-
- size_t sentLen = buffer.last().append(msg, msgLen);
-
- msgLen -= sentLen;
- msg += sentLen;
-
- // Mark as ready if the packet is full
+ // If the packet is full mark it as ready
if (buffer.last().isFull())
- {
buffer.last().markAsReady();
- }
- }
- cvNotempty.broadcast();
- return dropped;
+ // Go forward in the data
+ msgLen -= appendedLength;
+ msg += appendedLength;
+ }
}
+
+ // Wake all waiting threads
+ condVerNotEmpty.broadcast();
+
+ return true;
}
/**
- * @return a copy of the oldest packet, without removing it from the queue.
+ * @return The oldest packet, without removing it from the queue.
*/
- const Pkt& get()
+ const Packet<pktLen>& get()
{
Lock<FastMutex> l(mutex);
return buffer.get();
}
/**
- * @return the oldest packet, removing it from the queue.
+ * @return The oldest packet, removing it from the queue.
*/
- const Pkt& pop()
+ const Packet<pktLen>& pop()
{
Lock<FastMutex> l(mutex);
return buffer.pop();
}
/**
- * @return true if all the packets have been marked as ready.
+ * @return True if all the packets have been marked as ready.
*/
bool isFull()
{
Lock<FastMutex> l(mutex);
if (buffer.count() > 0)
- {
return buffer.isFull() && buffer.last().isReady();
- }
else
- {
return false;
- }
}
/**
- * @return true if all the packets are completely empty.
+ * @return True if all the packets are completely empty.
*/
bool isEmpty()
{
Lock<FastMutex> l(mutex);
-
return buffer.isEmpty();
}
/**
* @brief Blocks the calling thread until the queue is not empty.
+ *
* Returns immediately if already not empty.
*/
void waitUntilNotEmpty()
{
Lock<FastMutex> l(mutex);
if (buffer.isEmpty())
- {
- cvNotempty.wait(mutex);
- }
+ condVerNotEmpty.wait(mutex);
}
/**
- * @return the number of packets that are ready to be sent.
+ * @return The number of packets that are ready to be sent.
*/
size_t countReady()
{
Lock<FastMutex> l(mutex);
if (!buffer.isEmpty())
- {
return buffer.last().isReady() ? buffer.count()
: buffer.count() - 1;
- }
else
- {
return 0;
- }
}
/**
- * @return the number of packets in use, that are either fully or partially
+ * @return The number of packets in use, that are either fully or partially
* filled.
*/
size_t countNotEmpty()
{
Lock<FastMutex> l(mutex);
-
return buffer.count();
}
private:
FastMutex mutex;
- ConditionVariable cvNotempty;
-
- CircularBuffer<Pkt, pktNum> buffer;
+ ConditionVariable condVerNotEmpty;
+ CircularBuffer<Packet<pktLen>, pktNum> buffer;
};
} // namespace Boardcore
diff --git a/src/tests/catch/test-packetqueue.cpp b/src/tests/catch/test-packetqueue.cpp
index aa2beb55466235a36a0f3336034b1c3bf8bf5262..e0cf2865ac8da3f9dbc691dbcd583c2aaa79ec03 100644
--- a/src/tests/catch/test-packetqueue.cpp
+++ b/src/tests/catch/test-packetqueue.cpp
@@ -82,10 +82,12 @@ TEST_CASE("Packet tests")
SECTION("Adding stuff to packet")
{
+ // Add 5 bytes
+ REQUIRE(p.append(messageBase, 5));
+ uint64_t ts = p.getTimestamp();
- REQUIRE(p.tryAppend(messageBase, 5));
- uint64_t ts = p.timestamp();
- REQUIRE(miosix::getTick() - ts < 5);
+ REQUIRE(Boardcore::TimestampTimer::getInstance().getTimestamp() - ts <
+ 5);
REQUIRE(p.dump(buf) == 5);
COMPARE(buf, BUF_LEN, "01234");
@@ -93,29 +95,21 @@ TEST_CASE("Packet tests")
REQUIRE(p.size() == 5);
REQUIRE(p.getMsgCount() == 1);
- REQUIRE(p.tryAppend(messageBase + 5, 3));
+ // Add 3 bytes
+ REQUIRE(p.append(messageBase + 5, 3));
REQUIRE(p.dump(buf) == 8);
COMPARE(buf, BUF_LEN, "01234567");
REQUIRE(p.isEmpty() == false);
REQUIRE(p.size() == 8);
- REQUIRE_FALSE(p.tryAppend(messageBase + 8, 3));
- REQUIRE(p.dump(buf) == 8);
- COMPARE(buf, BUF_LEN, "01234567");
- REQUIRE(p.isEmpty() == false);
- REQUIRE(p.size() == 8);
- REQUIRE(p.getMsgCount() == 2);
-
- REQUIRE(p.tryAppend(messageBase + 8, 2));
- REQUIRE(p.dump(buf) == 10);
+ // Trying to add 3 more bytes, only 2 should be written
+ REQUIRE(p.append(messageBase + 8, 3) == 2);
+ REQUIRE(p.dump(buf) == PKT_LEN);
COMPARE(buf, BUF_LEN, "0123456789");
REQUIRE(p.isEmpty() == false);
- REQUIRE(p.isFull());
- REQUIRE(p.size() == 10);
+ REQUIRE(p.size() == PKT_LEN);
REQUIRE(p.getMsgCount() == 3);
- REQUIRE(p.timestamp() == ts);
-
p.clear();
REQUIRE(p.isEmpty());
REQUIRE(p.isFull() == false);
@@ -130,7 +124,7 @@ TEST_CASE("Packet tests")
SECTION("Edge cases")
{
INFO("Adding empty msg");
- REQUIRE_FALSE(p.tryAppend(messageBase, 0));
+ REQUIRE_FALSE(p.append(messageBase, 0));
REQUIRE(p.isEmpty());
REQUIRE(p.isFull() == false);
REQUIRE(p.isReady() == false);
@@ -141,29 +135,16 @@ TEST_CASE("Packet tests")
REQUIRE(p.dump(buf) == 0);
INFO("Adding too big msg");
- REQUIRE_FALSE(p.tryAppend(messageBase, PKT_LEN + 1));
-
- REQUIRE(p.isEmpty());
- REQUIRE(p.isFull() == false);
- REQUIRE(p.isReady() == false);
- REQUIRE(p.size() == 0);
- REQUIRE(p.maxSize() == PKT_LEN);
+ REQUIRE(p.append(messageBase, PKT_LEN + 1) == PKT_LEN);
- REQUIRE(p.getMsgCount() == 0);
- REQUIRE(p.dump(buf) == 0);
-
- INFO("Adding something to full packet");
- REQUIRE(p.tryAppend(messageBase, PKT_LEN));
- REQUIRE_FALSE(p.tryAppend(messageBase, 1));
-
- REQUIRE(p.isEmpty() == false);
+ REQUIRE_FALSE(p.isEmpty());
REQUIRE(p.isFull());
- REQUIRE(p.isReady() == false);
+ REQUIRE_FALSE(p.isReady());
REQUIRE(p.size() == PKT_LEN);
REQUIRE(p.maxSize() == PKT_LEN);
REQUIRE(p.getMsgCount() == 1);
- REQUIRE(p.dump(buf) == 10);
+ REQUIRE(p.dump(buf) == PKT_LEN);
COMPARE(buf, 10, "0123456789");
}
}
@@ -182,100 +163,142 @@ TEST_CASE("PacketQueue tests")
SECTION("Normal operation")
{
INFO("Adding two elements to first packet");
- REQUIRE(pq.put(messageBase, 4) == 0);
- REQUIRE(pq.put(messageBase, 4) == 0);
+ REQUIRE(pq.put(messageBase, 4));
+ REQUIRE(pq.put(messageBase, 4));
+ // No packet should be ready
REQUIRE(pq.countReady() == 0);
REQUIRE(pq.countNotEmpty() == 1);
REQUIRE_FALSE(pq.isEmpty());
REQUIRE_FALSE(pq.isFull());
INFO("Adding third element and filling first packet");
- REQUIRE(pq.put(messageBase, 2) == 0);
+ REQUIRE(pq.put(messageBase, 2));
+ // Now one single packet should be filled and ready
REQUIRE(pq.countReady() == 1);
REQUIRE(pq.countNotEmpty() == 1);
REQUIRE_FALSE(pq.isEmpty());
REQUIRE_FALSE(pq.isFull());
+ // Check the packet content
Packet<PKT_LEN> p = pq.get();
REQUIRE(p.getMsgCount() == 3);
REQUIRE(p.isFull());
REQUIRE(p.isReady());
COMPARE(p, "0123012301");
- INFO("Adding element to second packet");
- REQUIRE(pq.put(messageBase + 10, 4) == 0);
+ INFO("Adding more data to create a second packet");
+ REQUIRE(pq.put(messageBase + 10, 4));
+ // The second packet should not be ready
REQUIRE(pq.countReady() == 1);
REQUIRE(pq.countNotEmpty() == 2);
REQUIRE_FALSE(pq.isEmpty());
REQUIRE_FALSE(pq.isFull());
REQUIRE_FALSE(pq.buffer.get(1).isReady());
-
COMPARE(pq.buffer.get(1), "abcd");
p = pq.get(); // Should still return first packet
REQUIRE(p.getMsgCount() == 3);
- INFO(
- "Adding element not fitting the second packet, added to the third");
- REQUIRE(pq.put(messageBase + 10, 7) == 0);
+ INFO("Adding more data to create a third packet");
+ REQUIRE(pq.put(messageBase + 10, 7));
+
p = pq.get(); // Should still return first packet
REQUIRE(p.getMsgCount() == 3);
- REQUIRE(pq.countReady() == 2);
- REQUIRE(pq.countNotEmpty() == 3);
- REQUIRE_FALSE(pq.isEmpty());
- REQUIRE_FALSE(pq.isFull());
-
+ // Check all the packages
REQUIRE(pq.buffer.get(0).isReady());
+ REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+ COMPARE(pq.buffer.get(0), "0123012301");
REQUIRE(pq.buffer.get(1).isReady());
+ REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+ COMPARE(pq.buffer.get(1), "abcdabcdef");
REQUIRE_FALSE(pq.buffer.get(2).isReady());
+ REQUIRE(pq.buffer.get(2).size() == 1);
+ COMPARE(pq.buffer.get(2), "g");
- COMPARE(pq.buffer.get(0), "0123012301");
- COMPARE(pq.buffer.get(1), "abcd");
- COMPARE(pq.buffer.get(2), "abcdefg");
+ // Check the queue stats
+ REQUIRE(pq.countReady() == 2);
+ REQUIRE(pq.countNotEmpty() == 3);
+ REQUIRE_FALSE(pq.isEmpty());
+ REQUIRE_FALSE(pq.isFull());
INFO("Popping first element");
p = pq.pop(); // Should still return first packet
REQUIRE(p.getMsgCount() == 3);
COMPARE(p, "0123012301");
- // Should now return what was the second element
- COMPARE(pq.get(), "abcd");
-
+ // The packets should now be shifted
REQUIRE(pq.buffer.get(0).isReady());
+ REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+ COMPARE(pq.buffer.get(0), "abcdabcdef");
REQUIRE_FALSE(pq.buffer.get(1).isReady());
+ REQUIRE(pq.buffer.get(1).size() == 1);
+ COMPARE(pq.buffer.get(1), "g");
REQUIRE(pq.countReady() == 1);
REQUIRE(pq.countNotEmpty() == 2);
REQUIRE_FALSE(pq.isEmpty());
REQUIRE_FALSE(pq.isFull());
- INFO("Adding a msg back to the first packet and filling it");
- REQUIRE(pq.put(messageBase, 10) == 0);
- REQUIRE(pq.countReady() == 3);
+ INFO("Adding more data to fill the last packet");
+ REQUIRE(pq.put(messageBase, 10));
+ REQUIRE(pq.countReady() == 2);
REQUIRE(pq.countNotEmpty() == 3);
REQUIRE_FALSE(pq.isEmpty());
- REQUIRE(pq.isFull());
+ REQUIRE_FALSE(pq.isFull());
- COMPARE(pq.buffer.get(0), "abcd");
- COMPARE(pq.buffer.get(1), "abcdefg");
- COMPARE(pq.buffer.get(2), "0123456789");
+ // We should now have three packets
+ REQUIRE(pq.buffer.get(0).isReady());
+ REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+ COMPARE(pq.buffer.get(0), "abcdabcdef");
+ REQUIRE(pq.buffer.get(1).isReady());
+ REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+ COMPARE(pq.buffer.get(1), "g012345678");
+ REQUIRE_FALSE(pq.buffer.get(2).isReady());
+ REQUIRE(pq.buffer.get(2).size() == 1);
+ COMPARE(pq.buffer.get(2), "9");
+
+ // If we now add another 10 bytes the last packet, the last byte which
+ // does not fit should be put in a new packet at the start of the queue
+ REQUIRE(pq.put(messageBase, 10));
+ REQUIRE(pq.countReady() == 2);
+ REQUIRE(pq.countNotEmpty() == 3);
+ REQUIRE_FALSE(pq.isEmpty());
+ REQUIRE_FALSE(pq.isFull());
+ REQUIRE(pq.buffer.get(0).isReady());
+ REQUIRE(pq.buffer.get(0).size() == PKT_LEN);
+ COMPARE(pq.buffer.get(0), "g012345678");
+ REQUIRE(pq.buffer.get(1).isReady());
+ REQUIRE(pq.buffer.get(1).size() == PKT_LEN);
+ COMPARE(pq.buffer.get(1), "9012345678");
+ REQUIRE_FALSE(pq.buffer.get(2).isReady());
+ REQUIRE(pq.buffer.get(2).size() == 1);
+ COMPARE(pq.buffer.get(2), "9");
+
+ // And now by adding the last 9 bytes the queue should be marked ready
+ REQUIRE(pq.put(messageBase + 10, 9));
+ REQUIRE(pq.countReady() == 3);
+ REQUIRE(pq.countNotEmpty() == 3);
+ REQUIRE(pq.buffer.get(2).isReady());
+ REQUIRE(pq.buffer.get(2).size() == PKT_LEN);
+ COMPARE(pq.buffer.get(2), "9abcdefghi");
INFO("Popping everything");
p = pq.pop();
- COMPARE(p, "abcd");
REQUIRE(p.isReady());
+ REQUIRE(p.size() == PKT_LEN);
+ COMPARE(p, "g012345678");
p = pq.pop();
- COMPARE(p, "abcdefg");
+ COMPARE(p, "9012345678");
REQUIRE(p.isReady());
p = pq.pop();
- COMPARE(p, "0123456789");
+ COMPARE(p, "9abcdefghi");
REQUIRE(p.isReady());
REQUIRE_FALSE(pq.isFull());
@@ -289,19 +312,19 @@ TEST_CASE("PacketQueue tests")
SECTION("Edge cases")
{
INFO("Adding too big msg");
- REQUIRE(pq.put(messageBase, PKT_LEN + 1) == -1);
+ REQUIRE_FALSE(pq.put(messageBase, PKT_LEN * QUEUE_LEN + 1));
REQUIRE_FALSE(pq.isFull());
REQUIRE(pq.isEmpty());
REQUIRE(pq.countNotEmpty() == 0);
REQUIRE(pq.countReady() == 0);
INFO("Adding empty message");
- REQUIRE(pq.put(messageBase, 0) == -1);
+ REQUIRE_FALSE(pq.put(messageBase, 0));
INFO("Adding something to full queue");
- REQUIRE(pq.put(messageBase, PKT_LEN) == 0);
- REQUIRE(pq.put(messageBase + 5, PKT_LEN) == 0);
- REQUIRE(pq.put(messageBase + 10, PKT_LEN) == 0);
+ REQUIRE(pq.put(messageBase, PKT_LEN));
+ REQUIRE(pq.put(messageBase + 5, PKT_LEN));
+ REQUIRE(pq.put(messageBase + 10, PKT_LEN));
REQUIRE(pq.buffer.count() == 3);
for (int i = 0; i < 3; i++)