QueueRTOS.h

#pragma once
#include "TinyRobotics/utils/AllocatorPSRAM.h"
#include "TinyRobotics/utils/Config.h"
 
#ifdef ESP32
#include <freertos/queue.h>
 
#include "freertos/FreeRTOS.h"
#else
#ifndef __linux__
#include "FreeRTOS.h"
#include "queue.h"
#endif
#endif
 
namespace tinyrobotics {
 
/**
 * @brief FIFO Queue whch is based on the FreeRTOS queue API.
 * The default allocator will allocate the memory from psram if available
 * @ingroup collections
 * @ingroup concurrency
 * @author Phil Schatzmann
 
 * @tparam T
 */
template <class T, class TAllocator = AllocatorPSRAM<T>>
class QueueRTOS {
 public:
  QueueRTOS(int size, TickType_t writeMaxWait = portMAX_DELAY,
            TickType_t readMaxWait = portMAX_DELAY) {
 
    read_max_wait = readMaxWait;
    write_max_wait = writeMaxWait;
    queue_size = size;
    setup();
  }
 
  ~QueueRTOS() {
 
    end();
  }
 
  void setReadMaxWait(TickType_t ticks) { read_max_wait = ticks; }
 
  void setWriteMaxWait(TickType_t ticks) { write_max_wait = ticks; }
 
  /// (Re-)defines the size
  bool resize(int size) {
    bool result = true;
 
    if (size != queue_size) {
      end();
      queue_size = size;
      result = setup();
    }
    return result;
  }
 
  bool enqueue(T& data) {
 
    if (xQueue == nullptr) return false;
    return xQueueSend(xQueue, (void*)&data, (TickType_t)write_max_wait);
  }
 
  bool peek(T& data) {
 
    if (xQueue == nullptr) return false;
    return xQueuePeek(xQueue, &data, (TickType_t)read_max_wait);
  }
 
  bool dequeue(T& data) {
 
    if (xQueue == nullptr) return false;
    return xQueueReceive(xQueue, &data, (TickType_t)read_max_wait);
  }
 
  size_t size() { return queue_size; }
 
  bool clear() {
 
    if (xQueue == nullptr) return false;
    xQueueReset(xQueue);
    return true;
  }
 
  void end() {
    if (xQueue != nullptr) vQueueDelete(xQueue);
    if (p_data != nullptr) allocator.deallocate((T*)p_data, allocated_size);
  }
 
  bool isEmpty() { return size() == 0; }
 
 protected:
  QueueHandle_t xQueue = nullptr;
  TickType_t write_max_wait = portMAX_DELAY;
  TickType_t read_max_wait = portMAX_DELAY;
  int queue_size;
  StaticQueue_t queue_buffer;
  TAllocator allocator;
  size_t allocated_size = 0;
  uint8_t* p_data = nullptr;
 
  bool setup() {
    if (queue_size > 0) {
#if configSUPPORT_STATIC_ALLOCATION
      allocated_size = (queue_size + 1) * sizeof(T);
      p_data = (uint8_t*)allocator.allocate(allocated_size);
      if (p_data == nullptr) return false;
      xQueue = xQueueCreateStatic(queue_size, sizeof(T), p_data, &queue_buffer);
#else
      xQueue = xQueueCreate(queue_size, sizeof(T));
#endif
      if (xQueue == nullptr) return false;
    }
    return true;
  }
};
 
}  // namespace tinyrobotics