From e6868464bfeb7fd7150b5373b895474e33c877bc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 27 May 2021 13:38:40 +0200 Subject: [PATCH] queue map manager working --- osal/FreeRTOS/MessageQueue.cpp | 58 ++++++++++++++----------------- osal/FreeRTOS/MessageQueue.h | 9 +++-- osal/FreeRTOS/QueueMapManager.cpp | 9 +++++ 3 files changed, 39 insertions(+), 37 deletions(-) diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp index 3a0f654ed..a74d32eac 100644 --- a/osal/FreeRTOS/MessageQueue.cpp +++ b/osal/FreeRTOS/MessageQueue.cpp @@ -1,26 +1,23 @@ #include "MessageQueue.h" +#include "QueueMapManager.h" #include "../../objectmanager/ObjectManagerIF.h" #include "../../serviceinterface/ServiceInterfaceStream.h" -// TODO I guess we should have a way of checking if we are in an ISR and then -// use the "fromISR" versions of all calls -// As a first step towards this, introduces system context variable which needs -// to be switched manually -// Haven't found function to find system context. MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): maxMessageSize(maxMessageSize) { handle = xQueueCreate(messageDepth, maxMessageSize); -#if FSFW_CPP_OSTREAM_ENABLED == 1 if (handle == nullptr) { - sif::error << "MessageQueue::MessageQueue:" - << " Creation failed." << std::endl; - sif::error << "Specified Message Depth: " << messageDepth - << std::endl; - sif::error << "Specified Maximum Message Size: " - << maxMessageSize << std::endl; - - } +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "MessageQueue::MessageQueue: Creation failed" << std::endl; + sif::error << "Specified Message Depth: " << messageDepth << std::endl; + sif::error << "Specified Maximum Message Size: " << maxMessageSize << std::endl; +#else + sif::printError("MessageQueue::MessageQueue: Creation failed\n"); + sif::printError("Specified Message Depth: %d\n", messageDepth); + sif::printError("Specified MAximum Message Size: %d\n", maxMessageSize); #endif + } + QueueMapManager::instance()->addMessageQueue(handle, &queueId); } MessageQueue::~MessageQueue() { @@ -62,13 +59,15 @@ ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, callContext); } +QueueHandle_t MessageQueue::getNativeQueueHandle() { + return handle; +} ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) { if (result != pdPASS) { if (not ignoreFault) { InternalErrorReporterIF* internalErrorReporter = objectManager-> - get( - objects::INTERNAL_ERROR_REPORTER); + get(objects::INTERNAL_ERROR_REPORTER); if (internalErrorReporter != nullptr) { internalErrorReporter->queueMessageNotSent(); } @@ -110,7 +109,7 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) { } MessageQueueId_t MessageQueue::getId() const { - return reinterpret_cast(handle); + return queueId; } void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { @@ -132,30 +131,25 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, MessageQueueMessageIF* message, MessageQueueId_t sentFrom, bool ignoreFault, CallContext callContext) { BaseType_t result = pdFALSE; - QueueHandle_t destination = nullptr; - - if(sendTo == MessageQueueIF::NO_QUEUE or sendTo == 0x00) { - return MessageQueueIF::DESTINATION_INVALID; + if(sendTo == MessageQueueIF::NO_QUEUE) { + return MessageQueueIF::DESTINATION_INVALID; } - else { - destination = reinterpret_cast(sendTo); + + QueueHandle_t destination = QueueMapManager::instance()->getMessageQueue(sendTo); + if(destination == nullptr) { + return MessageQueueIF::DESTINATION_INVALID; } message->setSender(sentFrom); - - if(callContext == CallContext::TASK) { - result = xQueueSendToBack(destination, - static_cast(message->getBuffer()), 0); + result = xQueueSendToBack(destination, static_cast(message->getBuffer()), 0); } else { - /* If the call context is from an interrupt, - * request a context switch if a higher priority task - * was blocked by the interrupt. */ + /* If the call context is from an interrupt, request a context switch if a higher priority + task was blocked by the interrupt. */ BaseType_t xHigherPriorityTaskWoken = pdFALSE; result = xQueueSendFromISR(reinterpret_cast(sendTo), - static_cast(message->getBuffer()), - &xHigherPriorityTaskWoken); + static_cast(message->getBuffer()), &xHigherPriorityTaskWoken); if(xHigherPriorityTaskWoken == pdTRUE) { TaskManagement::requestContextSwitch(callContext); } diff --git a/osal/FreeRTOS/MessageQueue.h b/osal/FreeRTOS/MessageQueue.h index 8fa862831..9cee5b22e 100644 --- a/osal/FreeRTOS/MessageQueue.h +++ b/osal/FreeRTOS/MessageQueue.h @@ -11,11 +11,6 @@ #include #include -// TODO: this class assumes that MessageQueueId_t is the same size as void* -// (the FreeRTOS handle type), compiler will catch this but it might be nice -// to have something checking or even an always working solution -// https://scaryreasoner.wordpress.com/2009/02/28/checking-sizeof-at-compile-time/ - /** * @brief This class manages sending and receiving of * message queue messages. @@ -112,6 +107,8 @@ public: bool isDefaultDestinationSet() const override; + QueueHandle_t getNativeQueueHandle(); + protected: /** * @brief Implementation to be called from any send Call within @@ -141,6 +138,8 @@ protected: private: bool defaultDestinationSet = false; QueueHandle_t handle; + MessageQueueId_t queueId = MessageQueueIF::NO_QUEUE; + MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE; MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE; const size_t maxMessageSize; diff --git a/osal/FreeRTOS/QueueMapManager.cpp b/osal/FreeRTOS/QueueMapManager.cpp index 520e54cd6..51cfe11db 100644 --- a/osal/FreeRTOS/QueueMapManager.cpp +++ b/osal/FreeRTOS/QueueMapManager.cpp @@ -2,10 +2,19 @@ #include "../../ipc/MutexFactory.h" #include "../../ipc/MutexGuard.h" +QueueMapManager* QueueMapManager::mqManagerInstance = nullptr; + QueueMapManager::QueueMapManager() { mapLock = MutexFactory::instance()->createMutex(); } +QueueMapManager* QueueMapManager::instance() { + if (mqManagerInstance == nullptr){ + mqManagerInstance = new QueueMapManager(); + } + return QueueMapManager::mqManagerInstance; +} + ReturnValue_t QueueMapManager::addMessageQueue(QueueHandle_t queue, MessageQueueId_t* id) { MutexGuard lock(mapLock); uint32_t currentId = queueCounter++;