49 lines
1.4 KiB
C++
49 lines
1.4 KiB
C++
#include "FreeRTOS.h"
|
|
#include "queue.h"
|
|
#include <stdexcept>
|
|
#include <SDL2/SDL.h>
|
|
|
|
QueueHandle_t xQueueCreate(const UBaseType_t uxQueueLength, const UBaseType_t uxItemSize)
|
|
{
|
|
Queue_t *xQueue = new Queue_t;
|
|
if (uxItemSize != 1) {
|
|
throw std::runtime_error("uxItemSize must be 1");
|
|
}
|
|
xQueue->queue.reserve(uxQueueLength);
|
|
return xQueue;
|
|
}
|
|
|
|
BaseType_t xQueueSend(QueueHandle_t xQueue, const void * const pvItemToQueue, TickType_t xTicksToWait)
|
|
{
|
|
Queue_t* pxQueue = ( Queue_t * ) xQueue;
|
|
std::lock_guard<std::mutex> guard(pxQueue->mutex);
|
|
pxQueue->queue.push_back(*reinterpret_cast<const uint8_t *const>(pvItemToQueue));
|
|
return true;
|
|
}
|
|
|
|
BaseType_t xQueueSendFromISR(QueueHandle_t xQueue, const void * const pvItemToQueue, BaseType_t *xHigherPriorityTaskWoken)
|
|
{
|
|
TickType_t xTicksToWait = 0;
|
|
*xHigherPriorityTaskWoken = pdFALSE;
|
|
return xQueueSend(xQueue, pvItemToQueue, 0.0);
|
|
}
|
|
|
|
BaseType_t xQueueReceive(QueueHandle_t xQueue, void * const pvBuffer, TickType_t xTicksToWait)
|
|
{
|
|
Queue_t* pxQueue = ( Queue_t * ) xQueue;
|
|
while (pxQueue->queue.empty()) {
|
|
if (xTicksToWait <= 25) {
|
|
return false;
|
|
}
|
|
SDL_Delay(25);
|
|
xTicksToWait -= 25;
|
|
}
|
|
if (pxQueue->queue.empty()) {
|
|
return false;
|
|
}
|
|
std::lock_guard<std::mutex> guard(pxQueue->mutex);
|
|
uint8_t *buf = reinterpret_cast<uint8_t * const>(pvBuffer);
|
|
*buf = pxQueue->queue.at(0);
|
|
pxQueue->queue.erase(pxQueue->queue.begin());
|
|
return true;
|
|
}
|