queue map manager working

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:"
+#if FSFW_CPP_OSTREAM_ENABLED == 1
-		        << " Creation failed." << std::endl;
+		sif::error << "MessageQueue::MessageQueue: Creation failed" << std::endl;
-		sif::error << "Specified Message Depth: " << messageDepth
+		sif::error << "Specified Message Depth: " << messageDepth << std::endl;
-		        << std::endl;
+		sif::error << "Specified Maximum Message Size: " << maxMessageSize << std::endl;
-		sif::error << "Specified Maximum Message Size: "
+#else
-		        << maxMessageSize << std::endl;
+		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<InternalErrorReporterIF>(
+			        get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
-					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<MessageQueueId_t>(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) {
-
+        return MessageQueueIF::DESTINATION_INVALID;
-	if(sendTo == MessageQueueIF::NO_QUEUE or sendTo == 0x00) {
-		return MessageQueueIF::DESTINATION_INVALID;
 	}
-	else  {
+
-		destination = reinterpret_cast<QueueHandle_t>(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,
+        result = xQueueSendToBack(destination, static_cast<const void*>(message->getBuffer()), 0);
-                static_cast<const void*>(message->getBuffer()), 0);
     }
     else {
-        /* If the call context is from an interrupt,
+        /* If the call context is from an interrupt, request a context switch if a higher priority
-         * request a context switch if a higher priority task
+        task was blocked by the interrupt. */
-         * was blocked by the interrupt. */
         BaseType_t xHigherPriorityTaskWoken = pdFALSE;
         result = xQueueSendFromISR(reinterpret_cast<QueueHandle_t>(sendTo),
-                static_cast<const void*>(message->getBuffer()),
+                static_cast<const void*>(message->getBuffer()), &xHigherPriorityTaskWoken);
-                &xHigherPriorityTaskWoken);
         if(xHigherPriorityTaskWoken == pdTRUE) {
             TaskManagement::requestContextSwitch(callContext);
         }

osal/FreeRTOS/MessageQueue.h

@@ -11,11 +11,6 @@
 #include <freertos/queue.h>
 #include <fsfw/ipc/MessageQueueMessage.h>
 
-// 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;

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