From 3c4289335e21a87a42525c094bf68f54dbb36222 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 14 Jul 2021 10:39:02 +0200
Subject: [PATCH] integrated queue map manager bugfixes

---
 inc/fsfw/osal/freertos/QueueMapManager.h | 3 ++-
 src/osal/freertos/QueueMapManager.cpp    | 6 +++++-
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/inc/fsfw/osal/freertos/QueueMapManager.h b/inc/fsfw/osal/freertos/QueueMapManager.h
index 3aa82594..7e999b0d 100644
--- a/inc/fsfw/osal/freertos/QueueMapManager.h
+++ b/inc/fsfw/osal/freertos/QueueMapManager.h
@@ -39,7 +39,8 @@ private:
     QueueMapManager();
     ~QueueMapManager();
 
-    uint32_t queueCounter = 0;
+    // Start at 1 because 0 might be the NO_QUEUE value
+    uint32_t queueCounter = 1;
     MutexIF* mapLock;
     QueueMap queueMap;
     static QueueMapManager* mqManagerInstance;
diff --git a/src/osal/freertos/QueueMapManager.cpp b/src/osal/freertos/QueueMapManager.cpp
index dcb88a1e..e32cbe1d 100644
--- a/src/osal/freertos/QueueMapManager.cpp
+++ b/src/osal/freertos/QueueMapManager.cpp
@@ -17,7 +17,11 @@ QueueMapManager* QueueMapManager::instance() {
 
 ReturnValue_t QueueMapManager::addMessageQueue(QueueHandle_t queue, MessageQueueId_t* id) {
     MutexGuard lock(mapLock);
-    uint32_t currentId = ++queueCounter;
+    uint32_t currentId = queueCounter++;
+    if(currentId == MessageQueueIF::NO_QUEUE) {
+        // Skip the NO_QUEUE value
+        currentId = queueCounter++;
+    }
     auto returnPair = queueMap.emplace(currentId, queue);
     if(not returnPair.second) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1