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++)