SyncPacketQueue.h

Go to the documentation of this file.

/* Copyright (c) 2019-2022 Skyward Experimental Rocketry
 * Authors: 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
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
 
#pragma once
 
#include <drivers/timer/TimestampTimer.h>
#include <miosix.h>
#include <utils/Debug.h>
 
#include <cstdint>
#include <list>
#include <ostream>
#include <stdexcept>
#include <string>
#include <vector>
 
#include "CircularBuffer.h"
 
using miosix::ConditionVariable;
using miosix::FastMutex;
using miosix::Lock;
using std::range_error;
 
namespace Boardcore
{
 
template <unsigned int len>
class Packet
{
public:
    Packet() { content.reserve(len); };
 
    ~Packet() { content.clear(); };
 
    size_t append(const uint8_t* msg, size_t msgLen);
 
    inline void markAsReady() { ready = true; }
 
    size_t dump(uint8_t* buf);
 
    void clear();
 
    inline bool isFull() const { return content.size() == len; }
 
    inline bool isEmpty() const { return content.size() == 0; }
 
    inline bool isReady() const { return ready; }
 
    inline uint64_t getTimestamp() const { return timestamp; }
 
    inline size_t size() const { return content.size(); }
 
    inline unsigned int maxSize() const { return len; }
 
    inline unsigned int getMsgCount() const { return msgCounter; }
 
    void print(std::ostream& os) const;
 
    // The buffer itself
    std::vector<uint8_t> content;
 
private:
    unsigned int msgCounter = 0;
    uint64_t timestamp      = 0;
    bool ready              = false;
};
 
template <unsigned int len>
size_t Packet<len>::append(const uint8_t* msg, size_t msgLen)
{
    size_t remaining = len - content.size();
    msgLen           = std::min(remaining, msgLen);
 
    if (msgLen != 0)
    {
        // Set the packet's timestamp when the first message is inserted
        if (content.size() == 0)
            timestamp = TimestampTimer::getTimestamp();
 
        // Append the message to the packet
        content.insert(content.end(), msg, msg + msgLen);
        msgCounter++;
    }
 
    return msgLen;
}
 
template <unsigned int len>
void Packet<len>::clear()
{
    content.clear();
    msgCounter = 0;
    timestamp  = 0;
    ready      = false;
}
 
template <unsigned int len>
size_t Packet<len>::dump(uint8_t* buf)
{
    std::copy(content.begin(), content.end(), buf);
    return content.size();
}
 
template <unsigned int len>
void Packet<len>::print(std::ostream& os) const
{
    os << "timestamp=" << timestamp << ", ready=" << ready
       << ", size=" << content.size() << ", msgCounter=" << msgCounter
       << ", content= ";
 
    for (auto const& i : content)
        os << i;
    os << '\n';
}
 
template <unsigned int pktLen, unsigned int pktNum>
class SyncPacketQueue
{
public:
    bool put(uint8_t* msg, size_t msgLen)
    {
        // Check if the message is empty
        if (msgLen == 0)
            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({});
 
            // Write all the packet
            while (msgLen > 0)
            {
                // If the last packet is ready append a new one
                if (buffer.last().isReady())
                    buffer.put({});
 
                // Append what data is possible to the last packet
                size_t appendedLength = buffer.last().append(msg, msgLen);
 
                // If the packet is full mark it as ready
                if (buffer.last().isFull())
                    buffer.last().markAsReady();
 
                // Go forward in the data
                msgLen -= appendedLength;
                msg += appendedLength;
            }
        }
 
        // Wake all waiting threads
        condVerNotEmpty.broadcast();
 
        return true;
    }
 
    const Packet<pktLen> get()
    {
        Lock<FastMutex> l(mutex);
        return buffer.get();
    }
 
    const Packet<pktLen> pop()
    {
        Lock<FastMutex> l(mutex);
        return buffer.pop();
    }
 
    bool isFull()
    {
        Lock<FastMutex> l(mutex);
 
        if (buffer.count() > 0)
            return buffer.isFull() && buffer.last().isReady();
        else
            return false;
    }
 
    bool isEmpty()
    {
        Lock<FastMutex> l(mutex);
        return buffer.isEmpty();
    }
 
    void waitUntilNotEmpty()
    {
        Lock<FastMutex> l(mutex);
        if (buffer.isEmpty())
            condVerNotEmpty.wait(mutex);
    }
 
    size_t countReady()
    {
        Lock<FastMutex> l(mutex);
 
        if (!buffer.isEmpty())
        {
            return buffer.last().isReady() ? buffer.count()
                                           : buffer.count() - 1;
        }
        else
        {
            return 0;
        }
    }
 
    size_t countNotEmpty()
    {
        Lock<FastMutex> l(mutex);
        return buffer.count();
    }
 
private:
    FastMutex mutex;
    ConditionVariable condVerNotEmpty;
    CircularBuffer<Packet<pktLen>, pktNum> buffer;
};
 
}  // namespace Boardcore