From a3d245f5a030c9450c516552283d26da2c799301 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 23 Feb 2021 11:28:12 +0100
Subject: [PATCH 001/125] mutex helper print support and nullptr check

---
 ipc/MutexHelper.h | 49 +++++++++++++++++++++++++++++------------------
 1 file changed, 30 insertions(+), 19 deletions(-)

diff --git a/ipc/MutexHelper.h b/ipc/MutexHelper.h
index befa69bc..5a14249c 100644
--- a/ipc/MutexHelper.h
+++ b/ipc/MutexHelper.h
@@ -2,33 +2,44 @@
 #define FRAMEWORK_IPC_MUTEXHELPER_H_
 
 #include "MutexFactory.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 
 class MutexHelper {
 public:
-	MutexHelper(MutexIF* mutex, MutexIF::TimeoutType timeoutType =
-			MutexIF::TimeoutType::BLOCKING, uint32_t timeoutMs = 0) :
-			internalMutex(mutex) {
-		ReturnValue_t status = mutex->lockMutex(timeoutType,
-		        timeoutMs);
-		if(status == MutexIF::MUTEX_TIMEOUT) {
+    MutexHelper(MutexIF* mutex, MutexIF::TimeoutType timeoutType =
+            MutexIF::TimeoutType::BLOCKING, uint32_t timeoutMs = 0):
+            internalMutex(mutex) {
+        if(mutex == nullptr) {
+            return;
+        }
+        ReturnValue_t status = mutex->lockMutex(timeoutType,
+                timeoutMs);
+        if(status == MutexIF::MUTEX_TIMEOUT) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MutexHelper: Lock of mutex failed with timeout of "
-					<< timeoutMs << " milliseconds!" << std::endl;
+            sif::error << "MutexHelper: Lock of mutex failed with timeout of "
+                    << timeoutMs << " milliseconds!" << std::endl;
+#else
+            sif::printError("MutexHelper: Lock of mutex failed with timeout of %lu milliseconds\n",
+                    timeoutMs);
 #endif
-		}
-		else if(status != HasReturnvaluesIF::RETURN_OK){
+        }
+        else if(status != HasReturnvaluesIF::RETURN_OK){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MutexHelper: Lock of Mutex failed with code "
-			        << status << std::endl;
+            sif::error << "MutexHelper: Lock of Mutex failed with code "
+                    << status << std::endl;
+#else
+            sif::printError("MutexHelper: Lock of Mutex failed with code %d\n", status);
 #endif
-		}
-	}
+        }
+    }
 
-	~MutexHelper() {
-		internalMutex->unlockMutex();
-	}
+    ~MutexHelper() {
+        if(internalMutex != nullptr) {
+            internalMutex->unlockMutex();
+        }
+    }
 private:
-	MutexIF* internalMutex;
+    MutexIF* internalMutex;
 };
+
 #endif /* FRAMEWORK_IPC_MUTEXHELPER_H_ */

From f0178a8f73eb726e49e7f42c9be40d7663c6c429 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 23 Feb 2021 14:13:55 +0100
Subject: [PATCH 002/125] preprocessor optimization

---
 ipc/MutexHelper.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/ipc/MutexHelper.h b/ipc/MutexHelper.h
index 7f81f2f4..76eadc00 100644
--- a/ipc/MutexHelper.h
+++ b/ipc/MutexHelper.h
@@ -21,8 +21,8 @@ public:
         }
         ReturnValue_t status = mutex->lockMutex(timeoutType,
                 timeoutMs);
-        if(status == MutexIF::MUTEX_TIMEOUT) {
 #if FSFW_VERBOSE_LEVEL >= 1
+        if(status == MutexIF::MUTEX_TIMEOUT) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::error << "MutexHelper: Lock of mutex failed with timeout of "
                     << timeoutMs << " milliseconds!" << std::endl;
@@ -30,7 +30,7 @@ public:
             sif::printError("MutexHelper: Lock of mutex failed with timeout of %lu milliseconds\n",
                     timeoutMs);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+
         }
         else if(status != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
@@ -39,8 +39,8 @@ public:
 #else
             sif::printError("MutexHelper: Lock of Mutex failed with code %d\n", status);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-#endif /* FSFW_VERBOSE_LEVEL >= 1 */
         }
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
     }
 
     ~MutexHelper() {

From f3cc664d4f9fdadc2f6e2ebf814dd0c924e5ae33 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 23 Feb 2021 22:07:32 +0100
Subject: [PATCH 003/125] small printout tweak

---
 devicehandlers/DeviceHandlerBase.cpp | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 35d34bf9..2471a5a4 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -1494,10 +1494,9 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
 
     if(errorType == sif::OutputTypes::OUT_WARNING) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "DeviceHandlerBase::" << functionName << ": Object ID "
-                << std::hex << std::setw(8) << std::setfill('0')
-                << this->getObjectId() << " | " << errorPrint << std::dec
-                << std::setfill(' ') << std::endl;
+        sif::warning << "DeviceHandlerBase::" << functionName << ": Object ID 0x" << std::hex <<
+                std::setw(8) << std::setfill('0') << this->getObjectId() << " | " << errorPrint <<
+                std::dec << std::setfill(' ') << std::endl;
 #else
         sif::printWarning("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
                 this->getObjectId(), errorPrint);

From 92f249dc62cf02a5052fee4e9877bc89b2be1ab5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 24 Feb 2021 00:23:48 +0100
Subject: [PATCH 004/125] zero size handling

---
 globalfunctions/arrayprinter.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/globalfunctions/arrayprinter.cpp b/globalfunctions/arrayprinter.cpp
index 7dc056b0..1404035f 100644
--- a/globalfunctions/arrayprinter.cpp
+++ b/globalfunctions/arrayprinter.cpp
@@ -5,6 +5,15 @@
 
 void arrayprinter::print(const uint8_t *data, size_t size, OutputType type,
         bool printInfo, size_t maxCharPerLine) {
+    if(size == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::info << "Size is zero, nothing to print" << std::endl;
+#else
+        sif::printInfo("Size is zero, nothing to print\n");
+#endif
+        return;
+    }
+
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     if(printInfo) {
          sif::info << "Printing data with size " << size << ": " << std::endl;

From 17b8d3fed05cae6e208dc3f14b6ba0d44c9ec5ef Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Sat, 6 Mar 2021 18:12:41 +0100
Subject: [PATCH 005/125] printout for trans timeout

---
 devicehandlers/DeviceHandlerBase.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 10de57bd..0649aaa0 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -308,6 +308,14 @@ void DeviceHandlerBase::doStateMachine() {
         uint32_t currentUptime;
         Clock::getUptime(&currentUptime);
         if (currentUptime - timeoutStart >= childTransitionDelay) {
+#if FSFW_VERBOSE_LEVEL >= 1
+            char printout[60];
+            sprintf(printout, "Transition timeout (%lu) occured !",
+                    static_cast<unsigned long>(childTransitionDelay));
+            /* Very common configuration error, so print it */
+            printWarningOrError(sif::OutputTypes::OUT_WARNING, "doStateMachine",
+                    RETURN_FAILED, printout);
+#endif
             triggerEvent(MODE_TRANSITION_FAILED, childTransitionFailure, 0);
             setMode(transitionSourceMode, transitionSourceSubMode);
             break;

From 778ef4ef230019ca4b155a77518156793cf5d308 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Sat, 6 Mar 2021 20:36:54 +0100
Subject: [PATCH 006/125] cleaned up a bit, no functional change

---
 osal/linux/PosixThread.cpp | 336 ++++++++++++++++++-------------------
 1 file changed, 167 insertions(+), 169 deletions(-)

diff --git a/osal/linux/PosixThread.cpp b/osal/linux/PosixThread.cpp
index bd8e7258..bc5cacee 100644
--- a/osal/linux/PosixThread.cpp
+++ b/osal/linux/PosixThread.cpp
@@ -6,252 +6,250 @@
 #include <errno.h>
 
 PosixThread::PosixThread(const char* name_, int priority_, size_t stackSize_):
-		thread(0),priority(priority_),stackSize(stackSize_) {
+        thread(0),priority(priority_),stackSize(stackSize_) {
     name[0] = '\0';
     std::strncat(name, name_, PTHREAD_MAX_NAMELEN - 1);
 }
 
 PosixThread::~PosixThread() {
-	//No deletion and no free of Stack Pointer
+    //No deletion and no free of Stack Pointer
 }
 
 ReturnValue_t PosixThread::sleep(uint64_t ns) {
-	//TODO sleep might be better with timer instead of sleep()
-	timespec time;
-	time.tv_sec = ns/1000000000;
-	time.tv_nsec = ns - time.tv_sec*1e9;
+    //TODO sleep might be better with timer instead of sleep()
+    timespec time;
+    time.tv_sec = ns/1000000000;
+    time.tv_nsec = ns - time.tv_sec*1e9;
 
-	//Remaining Time is not set here
-	int status = nanosleep(&time,NULL);
-	if(status != 0){
-		switch(errno){
-		case EINTR:
-			//The nanosleep() function was interrupted by a signal.
-			return HasReturnvaluesIF::RETURN_FAILED;
-		case EINVAL:
-			//The rqtp argument specified a nanosecond value less than zero or
-			// greater than or equal to 1000 million.
-			return HasReturnvaluesIF::RETURN_FAILED;
-		default:
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
+    //Remaining Time is not set here
+    int status = nanosleep(&time,NULL);
+    if(status != 0){
+        switch(errno){
+        case EINTR:
+            //The nanosleep() function was interrupted by a signal.
+            return HasReturnvaluesIF::RETURN_FAILED;
+        case EINVAL:
+            //The rqtp argument specified a nanosecond value less than zero or
+            // greater than or equal to 1000 million.
+            return HasReturnvaluesIF::RETURN_FAILED;
+        default:
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
 
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void PosixThread::suspend() {
-	//Wait for SIGUSR1
-	int caughtSig = 0;
-	sigset_t waitSignal;
-	sigemptyset(&waitSignal);
-	sigaddset(&waitSignal, SIGUSR1);
-	sigwait(&waitSignal, &caughtSig);
-	if (caughtSig != SIGUSR1) {
+    //Wait for SIGUSR1
+    int caughtSig = 0;
+    sigset_t waitSignal;
+    sigemptyset(&waitSignal);
+    sigaddset(&waitSignal, SIGUSR1);
+    sigwait(&waitSignal, &caughtSig);
+    if (caughtSig != SIGUSR1) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "FixedTimeslotTask: Unknown Signal received: " <<
-				caughtSig << std::endl;
+        sif::error << "FixedTimeslotTask: Unknown Signal received: " <<
+                caughtSig << std::endl;
 #endif
-	}
+    }
 }
 
 void PosixThread::resume(){
-	/* Signal the thread to start. Makes sense to call kill to start or? ;)
-	 *
-	*	According to Posix raise(signal) will call pthread_kill(pthread_self(), sig),
-	*	but as the call must be done from the thread itsself this is not possible here
-	*/
-	pthread_kill(thread,SIGUSR1);
+    /* Signal the thread to start. Makes sense to call kill to start or? ;)
+    According to POSIX raise(signal) will call pthread_kill(pthread_self(), sig),
+    but as the call must be done from the thread itself this is not possible here */
+    pthread_kill(thread,SIGUSR1);
 }
 
 bool PosixThread::delayUntil(uint64_t* const prevoiusWakeTime_ms,
-		const uint64_t delayTime_ms) {
-	uint64_t nextTimeToWake_ms;
-	bool shouldDelay = false;
-	//Get current Time
-	const uint64_t currentTime_ms = getCurrentMonotonicTimeMs();
-	/* Generate the tick time at which the task wants to wake. */
-	nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms;
+        const uint64_t delayTime_ms) {
+    uint64_t nextTimeToWake_ms;
+    bool shouldDelay = false;
+    /* Get current Time */
+    const uint64_t currentTime_ms = getCurrentMonotonicTimeMs();
+    /* Generate the tick time at which the task wants to wake. */
+    nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms;
 
-	if (currentTime_ms < *prevoiusWakeTime_ms) {
-		/* The tick count has overflowed since this function was
-		 lasted called.  In this case the only time we should ever
-		 actually delay is if the wake time has also	overflowed,
-		 and the wake time is greater than the tick time.  When this
+    if (currentTime_ms < *prevoiusWakeTime_ms) {
+        /* The tick count has overflowed since this function was
+		 lasted called. In this case the only time we should ever
+		 actually delay is if the wake time has also overflowed,
+		 and the wake time is greater than the tick time. When this
 		 is the case it is as if neither time had overflowed. */
-		if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
-				&& (nextTimeToWake_ms > currentTime_ms)) {
-			shouldDelay = true;
-		}
-	} else {
-		/* The tick time has not overflowed.  In this case we will
+        if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
+                && (nextTimeToWake_ms > currentTime_ms)) {
+            shouldDelay = true;
+        }
+    } else {
+        /* The tick time has not overflowed.  In this case we will
 		 delay if either the wake time has overflowed, and/or the
 		 tick time is less than the wake time. */
-		if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
-				|| (nextTimeToWake_ms > currentTime_ms)) {
-			shouldDelay = true;
-		}
-	}
+        if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
+                || (nextTimeToWake_ms > currentTime_ms)) {
+            shouldDelay = true;
+        }
+    }
 
-	/* Update the wake time ready for the next call. */
+    /* Update the wake time ready for the next call. */
 
-	(*prevoiusWakeTime_ms) =  nextTimeToWake_ms;
+    (*prevoiusWakeTime_ms) = nextTimeToWake_ms;
 
-	if (shouldDelay) {
-		uint64_t sleepTime = nextTimeToWake_ms - currentTime_ms;
-		PosixThread::sleep(sleepTime * 1000000ull);
-		return true;
-	}
-	//We are shifting the time in case the deadline was missed like rtems
-	(*prevoiusWakeTime_ms) = currentTime_ms;
-	return false;
+    if (shouldDelay) {
+        uint64_t sleepTime = nextTimeToWake_ms - currentTime_ms;
+        PosixThread::sleep(sleepTime * 1000000ull);
+        return true;
+    }
+    /* We are shifting the time in case the deadline was missed like RTEMS */
+    (*prevoiusWakeTime_ms) = currentTime_ms;
+    return false;
 
 }
 
 
 uint64_t PosixThread::getCurrentMonotonicTimeMs(){
-	timespec timeNow;
-	clock_gettime(CLOCK_MONOTONIC_RAW, &timeNow);
-	uint64_t currentTime_ms = (uint64_t) timeNow.tv_sec * 1000
-				+ timeNow.tv_nsec / 1000000;
+    timespec timeNow;
+    clock_gettime(CLOCK_MONOTONIC_RAW, &timeNow);
+    uint64_t currentTime_ms = (uint64_t) timeNow.tv_sec * 1000
+            + timeNow.tv_nsec / 1000000;
 
-	return currentTime_ms;
+    return currentTime_ms;
 }
 
 
 void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	//sif::debug << "PosixThread::createTask" << std::endl;
+    //sif::debug << "PosixThread::createTask" << std::endl;
 #endif
-	/*
-	 * The attr argument points to a pthread_attr_t structure whose contents
-       are used at thread creation time to determine attributes for the new
-       thread; this structure is initialized using pthread_attr_init(3) and
-       related functions.  If attr is NULL, then the thread is created with
-       default attributes.
-	 */
-	pthread_attr_t attributes;
-	int status = pthread_attr_init(&attributes);
-	if(status != 0){
+    /*
+     * The attr argument points to a pthread_attr_t structure whose contents
+     * are used at thread creation time to determine attributes for the new
+     * thread; this structure is initialized using pthread_attr_init(3) and
+     * related functions.  If attr is NULL, then the thread is created with
+     * default attributes.
+     */
+    pthread_attr_t attributes;
+    int status = pthread_attr_init(&attributes);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread attribute init failed with: " <<
-				strerror(status) << std::endl;
+        sif::error << "Posix Thread attribute init failed with: " <<
+                strerror(status) << std::endl;
 #endif
-	}
-	void* stackPointer;
-	status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize);
-	if(status != 0){
+    }
+    void* stackPointer;
+    status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PosixThread::createTask: Stack init failed with: " <<
-				strerror(status) << std::endl;
+        sif::error << "PosixThread::createTask: Stack init failed with: " <<
+                strerror(status) << std::endl;
 #endif
-		if(errno == ENOMEM) {
-			size_t stackMb = stackSize/10e6;
+        if(errno == ENOMEM) {
+            size_t stackMb = stackSize/10e6;
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "PosixThread::createTask: Insufficient memory for"
-					" the requested " << stackMb << " MB" << std::endl;
+            sif::error << "PosixThread::createTask: Insufficient memory for"
+                    " the requested " << stackMb << " MB" << std::endl;
 #else
-			sif::printError("PosixThread::createTask: Insufficient memory for "
-					"the requested %lu MB\n", static_cast<unsigned long>(stackMb));
+            sif::printError("PosixThread::createTask: Insufficient memory for "
+                    "the requested %lu MB\n", static_cast<unsigned long>(stackMb));
 #endif
-		}
-		else if(errno == EINVAL) {
+        }
+        else if(errno == EINVAL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "PosixThread::createTask: Wrong alignment argument!"
-					<< std::endl;
+            sif::error << "PosixThread::createTask: Wrong alignment argument!"
+                    << std::endl;
 #else
-			sif::printError("PosixThread::createTask: "
-					"Wrong alignment argument!\n");
+            sif::printError("PosixThread::createTask: "
+                    "Wrong alignment argument!\n");
 #endif
-		}
-		return;
-	}
+        }
+        return;
+    }
 
-	status = pthread_attr_setstack(&attributes, stackPointer, stackSize);
-	if(status != 0){
+    status = pthread_attr_setstack(&attributes, stackPointer, stackSize);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PosixThread::createTask: pthread_attr_setstack "
-				" failed with: " << strerror(status) <<  std::endl;
-		sif::error << "Make sure the specified stack size is valid and is "
-				"larger than the minimum allowed stack size." << std::endl;
+        sif::error << "PosixThread::createTask: pthread_attr_setstack "
+                " failed with: " << strerror(status) <<  std::endl;
+        sif::error << "Make sure the specified stack size is valid and is "
+                "larger than the minimum allowed stack size." << std::endl;
 #endif
-	}
+    }
 
-	status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);
-	if(status != 0){
+    status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "Posix Thread attribute setinheritsched failed with: " <<
-					strerror(status) << std::endl;
+        sif::error << "Posix Thread attribute setinheritsched failed with: " <<
+                strerror(status) << std::endl;
 #endif
-	}
+    }
 
-	// TODO FIFO -> This needs root privileges for the process
-	status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO);
-	if(status != 0){
+    // TODO FIFO -> This needs root privileges for the process
+    status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread attribute schedule policy failed with: " <<
-				strerror(status) << std::endl;
+        sif::error << "Posix Thread attribute schedule policy failed with: " <<
+                strerror(status) << std::endl;
 #endif
-	}
+    }
 
-	sched_param scheduleParams;
-	scheduleParams.__sched_priority = priority;
-	status = pthread_attr_setschedparam(&attributes, &scheduleParams);
-	if(status != 0){
+    sched_param scheduleParams;
+    scheduleParams.__sched_priority = priority;
+    status = pthread_attr_setschedparam(&attributes, &scheduleParams);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread attribute schedule params failed with: " <<
-				strerror(status) << std::endl;
+        sif::error << "Posix Thread attribute schedule params failed with: " <<
+                strerror(status) << std::endl;
 #endif
-	}
+    }
 
-	//Set Signal Mask for suspend until startTask is called
-	sigset_t waitSignal;
-	sigemptyset(&waitSignal);
-	sigaddset(&waitSignal, SIGUSR1);
-	status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL);
-	if(status != 0){
+    //Set Signal Mask for suspend until startTask is called
+    sigset_t waitSignal;
+    sigemptyset(&waitSignal);
+    sigaddset(&waitSignal, SIGUSR1);
+    status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread sigmask failed failed with: " <<
-				strerror(status) << " errno: " << strerror(errno) << std::endl;
+        sif::error << "Posix Thread sigmask failed failed with: " <<
+                strerror(status) << " errno: " << strerror(errno) << std::endl;
 #endif
-	}
+    }
 
 
-	status = pthread_create(&thread,&attributes,fnc_,arg_);
-	if(status != 0){
+    status = pthread_create(&thread,&attributes,fnc_,arg_);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread create failed with: " <<
-				strerror(status) << std::endl;
+        sif::error << "Posix Thread create failed with: " <<
+                strerror(status) << std::endl;
 #endif
-	}
+    }
 
-	status = pthread_setname_np(thread,name);
-	if(status != 0){
+    status = pthread_setname_np(thread,name);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PosixThread::createTask: setname failed with: " <<
-				strerror(status) << std::endl;
+        sif::error << "PosixThread::createTask: setname failed with: " <<
+                strerror(status) << std::endl;
 #endif
-		if(status == ERANGE) {
+        if(status == ERANGE) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "PosixThread::createTask: Task name length longer"
-					" than 16 chars. Truncating.." << std::endl;
+            sif::error << "PosixThread::createTask: Task name length longer"
+                    " than 16 chars. Truncating.." << std::endl;
 #endif
-			name[15] = '\0';
-			status = pthread_setname_np(thread,name);
-			if(status != 0){
+            name[15] = '\0';
+            status = pthread_setname_np(thread,name);
+            if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-				sif::error << "PosixThread::createTask: Setting name"
-						" did not work.." << std::endl;
+                sif::error << "PosixThread::createTask: Setting name"
+                        " did not work.." << std::endl;
 #endif
-			}
-		}
-	}
+            }
+        }
+    }
 
-	status = pthread_attr_destroy(&attributes);
-	if(status!=0){
+    status = pthread_attr_destroy(&attributes);
+    if(status!=0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread attribute destroy failed with: " <<
-				strerror(status) << std::endl;
+        sif::error << "Posix Thread attribute destroy failed with: " <<
+                strerror(status) << std::endl;
 #endif
-	}
+    }
 }

From 83d0db824289b28dbad81cce0c80276c4fc839c8 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 20 Mar 2021 15:53:43 +0100
Subject: [PATCH 007/125] fixed udp bridge

---
 osal/linux/TmTcUnixUdpBridge.cpp | 10 ----------
 osal/linux/TmTcUnixUdpBridge.h   |  4 ----
 2 files changed, 14 deletions(-)

diff --git a/osal/linux/TmTcUnixUdpBridge.cpp b/osal/linux/TmTcUnixUdpBridge.cpp
index fa7913ea..767c3cfe 100644
--- a/osal/linux/TmTcUnixUdpBridge.cpp
+++ b/osal/linux/TmTcUnixUdpBridge.cpp
@@ -103,11 +103,6 @@ ReturnValue_t TmTcUnixUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
     /* The target address can be set by different threads so this lock ensures thread-safety */
     MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
 
-    if(ipAddrAnySet){
-        clientAddress.sin_addr.s_addr = htons(INADDR_ANY);
-        clientAddressLen = sizeof(clientAddress);
-    }
-
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
     char ipAddress [15];
     sif::debug << "IP Address Sender: "<<
@@ -160,8 +155,3 @@ void TmTcUnixUdpBridge::setMutexProperties(MutexIF::TimeoutType timeoutType,
     this->timeoutType = timeoutType;
     this->mutexTimeoutMs = timeoutMs;
 }
-
-void TmTcUnixUdpBridge::setClientAddressToAny(bool ipAddrAnySet){
-    this->ipAddrAnySet = ipAddrAnySet;
-}
-
diff --git a/osal/linux/TmTcUnixUdpBridge.h b/osal/linux/TmTcUnixUdpBridge.h
index 3ab2118c..c39c43f7 100644
--- a/osal/linux/TmTcUnixUdpBridge.h
+++ b/osal/linux/TmTcUnixUdpBridge.h
@@ -30,8 +30,6 @@ public:
 
     void checkAndSetClientAddress(sockaddr_in& clientAddress);
 
-    void setClientAddressToAny(bool ipAddrAnySet);
-
 protected:
     virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override;
 
@@ -42,8 +40,6 @@ private:
     struct sockaddr_in clientAddress;
     socklen_t clientAddressLen = 0;
 
-    bool ipAddrAnySet = false;
-
     //! Access to the client address is mutex protected as it is set by another task.
     MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
     dur_millis_t mutexTimeoutMs = 20;

From d1a256cbf66223fe47dcb1c7b8c825b6fcfa943c Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 31 Mar 2021 19:53:58 +0200
Subject: [PATCH 008/125] added missing include

---
 osal/linux/tcpipHelpers.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/osal/linux/tcpipHelpers.cpp b/osal/linux/tcpipHelpers.cpp
index 4c1b9a78..3e8f6009 100644
--- a/osal/linux/tcpipHelpers.cpp
+++ b/osal/linux/tcpipHelpers.cpp
@@ -1,5 +1,6 @@
 #include "../common/tcpipHelpers.h"
 
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tasks/TaskFactory.h"
 
 #include <errno.h>

From dea2205908f29cbb700ad85c2614da71b6efccfc Mon Sep 17 00:00:00 2001
From: IRS Cleanroom Laptop <robin.mueller.m@gmail.com>
Date: Thu, 1 Apr 2021 15:20:53 +0200
Subject: [PATCH 009/125] improved output

---
 devicehandlers/DeviceHandlerBase.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 0649aaa0..e46bd5de 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -1516,7 +1516,7 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
     }
     else if(errorType == sif::OutputTypes::OUT_ERROR) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "DeviceHandlerBase::" << functionName << ": Object ID "
+        sif::error << "DeviceHandlerBase::" << functionName << ": Object ID 0x"
                 << std::hex << std::setw(8) << std::setfill('0')
                 << this->getObjectId() << " | " << errorPrint << std::dec
                 << std::setfill(' ') << std::endl;

From 0e76333d3315f2278397935d220c20f64b6e838a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 7 Apr 2021 12:03:28 +0200
Subject: [PATCH 010/125] minor improvements for PUS distributor

---
 tcdistribution/PUSDistributor.cpp | 121 +++++++++++++++++-------------
 1 file changed, 68 insertions(+), 53 deletions(-)

diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 00fd9029..0a95c5ef 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -1,22 +1,24 @@
 #include "CCSDSDistributorIF.h"
 #include "PUSDistributor.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
+#define PUS_DISTRIBUTOR_DEBUGGING     1
+
 PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
-		object_id_t setPacketSource) :
-		TcDistributor(setObjectId), checker(setApid), verifyChannel(),
-		tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
+        object_id_t setPacketSource) :
+        TcDistributor(setObjectId), checker(setApid), verifyChannel(),
+        tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
 
 PUSDistributor::~PUSDistributor() {}
 
 PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    // sif:: debug << "PUSDistributor::handlePacket received: "
-    //          << this->current_packet_id.store_index << ", "
-    //          << this->current_packet_id.packet_index << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
+    store_address_t storeId = this->currentMessage.getStorageId());
+    sif:: debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", " <<
+            storeId.packetIndex << std::endl;
 #endif
     TcMqMapIter queueMapIt = this->queueMap.end();
     if(this->currentPacket == nullptr) {
@@ -25,15 +27,17 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
     this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
     if (currentPacket->getWholeData() != nullptr) {
         tcStatus = checker.checkPacket(currentPacket);
-#ifdef DEBUG
         if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::debug << "PUSDistributor::handlePacket: Packet format "
-                    << "invalid, code "<< static_cast<int>(tcStatus)
-                    << std::endl;
+            sif::debug << "PUSDistributor::handlePacket: Packet format invalid, code " <<
+                    static_cast<int>(tcStatus) << std::endl;
+#else
+            sif::printDebug("PUSDistributor::handlePacket: Packet format invalid, code %d\n",
+                    static_cast<int>(tcStatus));
+#endif
 #endif
         }
-#endif
         uint32_t queue_id = currentPacket->getService();
         queueMapIt = this->queueMap.find(queue_id);
     }
@@ -43,11 +47,12 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
 
     if (queueMapIt == this->queueMap.end()) {
         tcStatus = DESTINATION_NOT_FOUND;
-#ifdef DEBUG
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::debug << "PUSDistributor::handlePacket: Destination not found, "
-                << "code "<< static_cast<int>(tcStatus) << std::endl;
-#endif
+        sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
+#else
+        sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
+#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
 #endif
     }
 
@@ -62,46 +67,54 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
 
 
 ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
-	uint16_t serviceId = service->getIdentifier();
+    uint16_t serviceId = service->getIdentifier();
+#if PUS_DISTRIBUTOR_DEBUGGING == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	// sif::info << "Service ID: " << (int)serviceId << std::endl;
+    sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
+#else
+    sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
 #endif
-	MessageQueueId_t queue = service->getRequestQueue();
-	auto returnPair = queueMap.emplace(serviceId, queue);
-	if (not returnPair.second) {
+#endif
+    MessageQueueId_t queue = service->getRequestQueue();
+    auto returnPair = queueMap.emplace(serviceId, queue);
+    if (not returnPair.second) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PUSDistributor::registerService: Service ID already"
-				" exists in map." << std::endl;
+        sif::error << "PUSDistributor::registerService: Service ID already"
+                " exists in map" << std::endl;
+#else
+        sif::printError("PUSDistributor::registerService: Service ID already exists in map\n");
 #endif
-		return SERVICE_ID_ALREADY_EXISTS;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+#endif
+        return SERVICE_ID_ALREADY_EXISTS;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 MessageQueueId_t PUSDistributor::getRequestQueue() {
-	return tcQueue->getId();
+    return tcQueue->getId();
 }
 
 ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
-	if (queueStatus != RETURN_OK) {
-		tcStatus = queueStatus;
-	}
-	if (tcStatus != RETURN_OK) {
-		this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE,
-				currentPacket, tcStatus);
-		// A failed packet is deleted immediately after reporting,
-		// otherwise it will block memory.
-		currentPacket->deletePacket();
-		return RETURN_FAILED;
-	} else {
-		this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS,
-				currentPacket);
-		return RETURN_OK;
-	}
+    if (queueStatus != RETURN_OK) {
+        tcStatus = queueStatus;
+    }
+    if (tcStatus != RETURN_OK) {
+        this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE,
+                currentPacket, tcStatus);
+        // A failed packet is deleted immediately after reporting,
+        // otherwise it will block memory.
+        currentPacket->deletePacket();
+        return RETURN_FAILED;
+    } else {
+        this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS,
+                currentPacket);
+        return RETURN_OK;
+    }
 }
 
 uint16_t PUSDistributor::getIdentifier() {
-	return checker.getApid();
+    return checker.getApid();
 }
 
 ReturnValue_t PUSDistributor::initialize() {
@@ -111,15 +124,17 @@ ReturnValue_t PUSDistributor::initialize() {
         return ObjectManagerIF::CHILD_INIT_FAILED;
     }
 
-	CCSDSDistributorIF* ccsdsDistributor =
-			objectManager->get<CCSDSDistributorIF>(packetSource);
-	if (ccsdsDistributor == nullptr) {
+    CCSDSDistributorIF* ccsdsDistributor =
+            objectManager->get<CCSDSDistributorIF>(packetSource);
+    if (ccsdsDistributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PUSDistributor::initialize: Packet source invalid."
-		        << " Make sure it exists and implements CCSDSDistributorIF!"
-		        << std::endl;
+        sif::error << "PUSDistributor::initialize: Packet source invalid" << std::endl;
+        sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
+#else
+        sif::printError("PUSDistributor::initialize: Packet source invalid\n");
+        sif::printError("Make sure it exists and implements CCSDSDistributorIF\n");
 #endif
-	    return RETURN_FAILED;
-	}
-	return ccsdsDistributor->registerApplication(this);
+        return RETURN_FAILED;
+    }
+    return ccsdsDistributor->registerApplication(this);
 }

From 7322a7d0f53e0ae72b69cac1a4abffbe74543365 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 7 Apr 2021 12:09:06 +0200
Subject: [PATCH 011/125] improvements for ccsds distributor

---
 tcdistribution/CCSDSDistributor.cpp | 31 ++++++++++++++++++++++-------
 1 file changed, 24 insertions(+), 7 deletions(-)

diff --git a/tcdistribution/CCSDSDistributor.cpp b/tcdistribution/CCSDSDistributor.cpp
index b795854f..f3864066 100644
--- a/tcdistribution/CCSDSDistributor.cpp
+++ b/tcdistribution/CCSDSDistributor.cpp
@@ -1,8 +1,10 @@
 #include "CCSDSDistributor.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/SpacePacketBase.h"
 
+#define CCSDS_DISTRIBUTOR_DEBUGGING     1
+
 CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
 		object_id_t setObjectId):
 		TcDistributor(setObjectId), defaultApid( setDefaultApid ) {
@@ -11,26 +13,36 @@ CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
 CCSDSDistributor::~CCSDSDistributor() {}
 
 TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
+#if CCSDS_DISTRIBUTOR_DEBUGGING == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-//	sif::debug << "CCSDSDistributor::selectDestination received: " <<
-//			this->currentMessage.getStorageId().pool_index << ", " <<
-//			this->currentMessage.getStorageId().packet_index << std::endl;
+	sif::debug << "CCSDSDistributor::selectDestination received: " <<
+			this->currentMessage.getStorageId().poolIndex << ", " <<
+			this->currentMessage.getStorageId().packetIndex << std::endl;
+#else
+	sif::printDebug("CCSDSDistributor::selectDestination received: %d, %d\n",
+	        currentMessage.getStorageId().poolIndex, currentMessage.getStorageId().packetIndex);
+#endif
 #endif
 	const uint8_t* packet = nullptr;
 	size_t size = 0;
 	ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(),
 			&packet, &size );
 	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CCSDSDistributor::selectDestination: Getting data from"
 				" store failed!" << std::endl;
+#else
+		sif::printError("CCSDSDistributor::selectDestination: Getting data from"
+                " store failed!\n");
+#endif
 #endif
 	}
 	SpacePacketBase currentPacket(packet);
 
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-//	sif:: info << "CCSDSDistributor::selectDestination has packet with APID "
-//			<< std::hex << currentPacket.getAPID() << std::dec << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
+	sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex <<
+	        currentPacket.getAPID() << std::dec << std::endl;
 #endif
 	TcMqMapIter position = this->queueMap.find(currentPacket.getAPID());
 	if ( position != this->queueMap.end() ) {
@@ -76,9 +88,14 @@ ReturnValue_t CCSDSDistributor::initialize() {
 	ReturnValue_t status = this->TcDistributor::initialize();
 	this->tcStore = objectManager->get<StorageManagerIF>( objects::TC_STORE );
 	if (this->tcStore == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "CCSDSDistributor::initialize: Could not initialize"
 	            " TC store!" << std::endl;
+#else
+	    sif::printError("CCSDSDistributor::initialize: Could not initialize"
+                " TC store!\n");
+#endif
 #endif
 	    status = RETURN_FAILED;
 	}

From 3ff54844153aa81cbb0c5c6b5546be4bd4a30904 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 7 Apr 2021 13:44:03 +0200
Subject: [PATCH 012/125] disabled debugging mode

---
 tcdistribution/CCSDSDistributor.cpp | 2 +-
 tcdistribution/PUSDistributor.cpp   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/tcdistribution/CCSDSDistributor.cpp b/tcdistribution/CCSDSDistributor.cpp
index f3864066..62cbfbf2 100644
--- a/tcdistribution/CCSDSDistributor.cpp
+++ b/tcdistribution/CCSDSDistributor.cpp
@@ -3,7 +3,7 @@
 #include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/SpacePacketBase.h"
 
-#define CCSDS_DISTRIBUTOR_DEBUGGING     1
+#define CCSDS_DISTRIBUTOR_DEBUGGING     0
 
 CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
 		object_id_t setObjectId):
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 0a95c5ef..abdd1f8d 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -5,7 +5,7 @@
 #include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
-#define PUS_DISTRIBUTOR_DEBUGGING     1
+#define PUS_DISTRIBUTOR_DEBUGGING     0
 
 PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
         object_id_t setPacketSource) :

From 43ddb445737575a8847e895aea279a57f4e0a6a1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 7 Apr 2021 22:12:01 +0200
Subject: [PATCH 013/125] fixed temperature sensor object

---
 thermal/TemperatureSensor.h | 54 +++++++++++++++++++------------------
 1 file changed, 28 insertions(+), 26 deletions(-)

diff --git a/thermal/TemperatureSensor.h b/thermal/TemperatureSensor.h
index 2b1fb1f0..ceb8a861 100644
--- a/thermal/TemperatureSensor.h
+++ b/thermal/TemperatureSensor.h
@@ -1,11 +1,14 @@
 #ifndef TEMPERATURESENSOR_H_
 #define TEMPERATURESENSOR_H_
 
-#include "../thermal/AbstractTemperatureSensor.h"
-#include "../datapoolglob/GlobalDataSet.h"
-#include "../datapoolglob/GlobalPoolVariable.h"
+#include "tcsDefinitions.h"
+#include "AbstractTemperatureSensor.h"
+
+#include "../datapoollocal/LocalPoolDataSetBase.h"
+#include "../datapoollocal/LocalPoolVariable.h"
 #include "../monitoring/LimitMonitor.h"
 
+
 /**
  * @brief 	This building block handles non-linear value conversion and
  * 			range checks for analog temperature sensors.
@@ -57,27 +60,26 @@ public:
 
 	/**
 	 * Instantiate Temperature Sensor Object.
-	 * @param setObjectid objectId of the sensor object
-	 * @param inputValue Input value which is converted to a temperature
-	 * @param poolVariable Pool Variable to store the temperature value
-	 * @param vectorIndex Vector Index for the sensor monitor
-	 * @param parameters Calculation parameters, temperature limits, gradient limit
-	 * @param datapoolId Datapool ID of the output temperature
-	 * @param outputSet Output dataset for the output temperature to fetch it with read()
+	 * @param setObjectid   objectId of the sensor object
+	 * @param inputValue    Pointer to input value which is converted to a temperature
+	 * @param variableGpid  Global Pool ID of the output value
+	 * @param inputVariable Input variable handle
+	 * @param vectorIndex   Vector Index for the sensor monitor
+	 * @param parameters    Calculation parameters, temperature limits, gradient limit
+	 * @param outputSet     Output dataset for the output temperature to fetch it with read()
 	 * @param thermalModule respective thermal module, if it has one
 	 */
 	TemperatureSensor(object_id_t setObjectid,
-			inputType *inputValue, PoolVariableIF *poolVariable,
-			uint8_t vectorIndex, uint32_t datapoolId, Parameters parameters = {0, 0, 0, 0, 0, 0},
-			GlobDataSet *outputSet = NULL, ThermalModuleIF *thermalModule = NULL) :
+			inputType *inputValue, gp_id_t variableGpid, PoolVariableIF* inputVariable,
+			uint8_t vectorIndex, Parameters parameters = {0, 0, 0, 0, 0, 0},
+			LocalPoolDataSetBase *outputSet = NULL, ThermalModuleIF *thermalModule = NULL) :
 			AbstractTemperatureSensor(setObjectid, thermalModule), parameters(parameters),
-			inputValue(inputValue), poolVariable(poolVariable),
-			outputTemperature(datapoolId, outputSet, PoolVariableIF::VAR_WRITE),
-			sensorMonitor(setObjectid, DOMAIN_ID_SENSOR,
-				GlobalDataPool::poolIdAndPositionToPid(poolVariable->getDataPoolId(), vectorIndex),
+			inputValue(inputValue), poolVariable(inputVariable),
+			outputTemperature(variableGpid, outputSet, PoolVariableIF::VAR_WRITE),
+			sensorMonitor(setObjectid, DOMAIN_ID_SENSOR, poolVariable,
 				DEFAULT_CONFIRMATION_COUNT, parameters.lowerLimit, parameters.upperLimit,
 				TEMP_SENSOR_LOW, TEMP_SENSOR_HIGH),
-			oldTemperature(20), uptimeOfOldTemperature( { INVALID_TEMPERATURE, 0 }) {
+			oldTemperature(20), uptimeOfOldTemperature({ thermal::INVALID_TEMPERATURE, 0 }) {
 	}
 
 
@@ -98,7 +100,7 @@ protected:
 
 private:
 	void setInvalid() {
-		outputTemperature = INVALID_TEMPERATURE;
+		outputTemperature = thermal::INVALID_TEMPERATURE;
 		outputTemperature.setValid(false);
 		uptimeOfOldTemperature.tv_sec = INVALID_UPTIME;
 		sensorMonitor.setToInvalid();
@@ -108,11 +110,11 @@ protected:
 
 	UsedParameters parameters;
 
-	inputType * inputValue;
+	inputType* inputValue;
 
-	PoolVariableIF *poolVariable;
+	PoolVariableIF* poolVariable;
 
-	gp_float_t outputTemperature;
+	lp_var_t<float> outputTemperature;
 
 	LimitMonitor<limitType> sensorMonitor;
 
@@ -120,8 +122,8 @@ protected:
 	timeval uptimeOfOldTemperature;
 
 	void doChildOperation() {
-		if (!poolVariable->isValid()
-				|| !healthHelper.healthTable->isHealthy(getObjectId())) {
+		if ((not poolVariable->isValid()) or
+		        (not healthHelper.healthTable->isHealthy(getObjectId()))) {
 			setInvalid();
 			return;
 		}
@@ -152,13 +154,13 @@ protected:
 			}
 		}
 
-		//Check is done against raw limits. SHOULDDO: Why? Using �C would be more easy to handle.
+		//Check is done against raw limits. SHOULDDO: Why? Using C would be more easy to handle.
 		sensorMonitor.doCheck(outputTemperature.value);
 
 		if (sensorMonitor.isOutOfLimits()) {
 			uptimeOfOldTemperature.tv_sec = INVALID_UPTIME;
 			outputTemperature.setValid(PoolVariableIF::INVALID);
-			outputTemperature = INVALID_TEMPERATURE;
+			outputTemperature = thermal::INVALID_TEMPERATURE;
 		} else {
 			oldTemperature = outputTemperature;
 			uptimeOfOldTemperature = uptime;

From 80aab5f461fa0f462aff4743c95413299642ce06 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 15:25:22 +0200
Subject: [PATCH 014/125] super evil bug

---
 osal/linux/MessageQueue.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index 60d15dee..b40ff29b 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -190,13 +190,14 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
 		return HasReturnvaluesIF::RETURN_OK;
-	}else if(status==0){
+	}
+	else if(status==0) {
 		//Success but no message received
 		return MessageQueueIF::EMPTY;
 	} else {
 		//No message was received. Keep lastPartner anyway, I might send
 		//something later. But still, delete packet content.
-		memset(message->getData(), 0, message->getMaximumMessageSize());
+		memset(message->getBuffer(), 0, message->getMaximumMessageSize());
 		switch(errno){
 		case EAGAIN:
 			//O_NONBLOCK or MQ_NONBLOCK was set and there are no messages

From 9ee1bd15c4f82e10edc8aba250686ed70f1699ae Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 15:38:36 +0200
Subject: [PATCH 015/125] now fixed properly

---
 ipc/MessageQueueMessage.cpp | 4 ++++
 ipc/MessageQueueMessage.h   | 1 +
 ipc/MessageQueueMessageIF.h | 1 +
 osal/linux/MessageQueue.cpp | 7 ++++---
 4 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/ipc/MessageQueueMessage.cpp b/ipc/MessageQueueMessage.cpp
index e97778c3..1958af54 100644
--- a/ipc/MessageQueueMessage.cpp
+++ b/ipc/MessageQueueMessage.cpp
@@ -86,3 +86,7 @@ size_t MessageQueueMessage::getMaximumMessageSize() const {
     return this->MAX_MESSAGE_SIZE;
 }
 
+size_t MessageQueueMessage::getMaximumDataSize() const {
+    return this->MAX_DATA_SIZE;
+}
+
diff --git a/ipc/MessageQueueMessage.h b/ipc/MessageQueueMessage.h
index 5234f64f..111056ca 100644
--- a/ipc/MessageQueueMessage.h
+++ b/ipc/MessageQueueMessage.h
@@ -139,6 +139,7 @@ public:
 	virtual void setMessageSize(size_t messageSize) override;
 	virtual size_t getMinimumMessageSize() const override;
 	virtual size_t getMaximumMessageSize() const override;
+	virtual size_t getMaximumDataSize() const override;
 
 	/**
 	 * @brief	This is a debug method that prints the content.
diff --git a/ipc/MessageQueueMessageIF.h b/ipc/MessageQueueMessageIF.h
index 33e01e7d..893c30b5 100644
--- a/ipc/MessageQueueMessageIF.h
+++ b/ipc/MessageQueueMessageIF.h
@@ -72,6 +72,7 @@ public:
     virtual void setMessageSize(size_t messageSize) = 0;
     virtual size_t getMinimumMessageSize() const = 0;
     virtual size_t getMaximumMessageSize() const = 0;
+    virtual size_t getMaximumDataSize() const = 0;
 
 };
 
diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index b40ff29b..8004c045 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -191,13 +191,14 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 		}
 		return HasReturnvaluesIF::RETURN_OK;
 	}
-	else if(status==0) {
+	else if (status==0) {
 		//Success but no message received
 		return MessageQueueIF::EMPTY;
-	} else {
+	}
+	else {
 		//No message was received. Keep lastPartner anyway, I might send
 		//something later. But still, delete packet content.
-		memset(message->getBuffer(), 0, message->getMaximumMessageSize());
+		memset(message->getData(), 0, message->getMaximumDataSize());
 		switch(errno){
 		case EAGAIN:
 			//O_NONBLOCK or MQ_NONBLOCK was set and there are no messages

From e799e45198c19793d99a530e0d496f89d08107b3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 16:20:24 +0200
Subject: [PATCH 016/125] bugfix for RTEMS

---
 osal/rtems/MessageQueue.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/osal/rtems/MessageQueue.cpp b/osal/rtems/MessageQueue.cpp
index bfaf3569..717b80dd 100644
--- a/osal/rtems/MessageQueue.cpp
+++ b/osal/rtems/MessageQueue.cpp
@@ -61,7 +61,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 	} else {
 		//No message was received. Keep lastPartner anyway, I might send something later.
 		//But still, delete packet content.
-		memset(message->getData(), 0, message->getMaximumMessageSize());
+		memset(message->getData(), 0, message->getMaximumDataSize());
 	}
 	return convertReturnCode(status);
 }

From 38d929c2a8df4451129bad3b464f3119fffe96ab Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 18:48:44 +0200
Subject: [PATCH 017/125] coverity fixes

---
 osal/common/TcpTmTcServer.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/osal/common/TcpTmTcServer.cpp b/osal/common/TcpTmTcServer.cpp
index 296afad8..2bfd4876 100644
--- a/osal/common/TcpTmTcServer.cpp
+++ b/osal/common/TcpTmTcServer.cpp
@@ -70,6 +70,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
 #endif
         freeaddrinfo(addrResult);
         handleError(Protocol::TCP, ErrorSources::BIND_CALL);
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
 
     freeaddrinfo(addrResult);

From f988271be4dbae63963755871451dc95d3085efd Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 18:53:22 +0200
Subject: [PATCH 018/125] coverity fixes

---
 container/SharedRingBuffer.cpp |  3 +++
 container/SharedRingBuffer.h   | 23 +++++++++++++----------
 osal/common/TcpTmTcServer.cpp  |  6 ++++--
 osal/common/UdpTmTcBridge.cpp  |  1 +
 4 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/container/SharedRingBuffer.cpp b/container/SharedRingBuffer.cpp
index 5e20bb6f..fe36341d 100644
--- a/container/SharedRingBuffer.cpp
+++ b/container/SharedRingBuffer.cpp
@@ -17,6 +17,9 @@ SharedRingBuffer::SharedRingBuffer(object_id_t objectId, uint8_t *buffer,
     mutex = MutexFactory::instance()->createMutex();
 }
 
+SharedRingBuffer::~SharedRingBuffer() {
+    MutexFactory::instance()->deleteMutex(mutex);
+}
 
 void SharedRingBuffer::setToUseReceiveSizeFIFO(size_t fifoDepth) {
     this->fifoDepth = fifoDepth;
diff --git a/container/SharedRingBuffer.h b/container/SharedRingBuffer.h
index 66a119ab..9d6ea56c 100644
--- a/container/SharedRingBuffer.h
+++ b/container/SharedRingBuffer.h
@@ -26,6 +26,18 @@ public:
      */
     SharedRingBuffer(object_id_t objectId, const size_t size,
             bool overwriteOld, size_t maxExcessBytes);
+    /**
+     * This constructor takes an external buffer with the specified size.
+     * @param buffer
+     * @param size
+     * @param overwriteOld
+     * If the ring buffer is overflowing at a write operartion, the oldest data
+     * will be overwritten.
+     */
+    SharedRingBuffer(object_id_t objectId, uint8_t* buffer, const size_t size,
+            bool overwriteOld, size_t maxExcessBytes);
+
+    virtual~ SharedRingBuffer();
 
     /**
      * @brief    This function can be used to add an optional FIFO to the class
@@ -37,16 +49,7 @@ public:
      */
     void setToUseReceiveSizeFIFO(size_t fifoDepth);
 
-    /**
-     * This constructor takes an external buffer with the specified size.
-     * @param buffer
-     * @param size
-     * @param overwriteOld
-     * If the ring buffer is overflowing at a write operartion, the oldest data
-     * will be overwritten.
-     */
-    SharedRingBuffer(object_id_t objectId, uint8_t* buffer, const size_t size,
-            bool overwriteOld, size_t maxExcessBytes);
+
 
     /**
      * Unless a read-only constant value is read, all operations on the
diff --git a/osal/common/TcpTmTcServer.cpp b/osal/common/TcpTmTcServer.cpp
index 2bfd4876..08a62ffb 100644
--- a/osal/common/TcpTmTcServer.cpp
+++ b/osal/common/TcpTmTcServer.cpp
@@ -85,8 +85,8 @@ TcpTmTcServer::~TcpTmTcServer() {
 ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
     using namespace tcpip;
     /* If a connection is accepted, the corresponding socket will be assigned to the new socket */
-    socket_t clientSocket;
-    sockaddr clientSockAddr;
+    socket_t clientSocket = 0;
+    sockaddr clientSockAddr = {};
     socklen_t connectorSockAddrLen = 0;
     int retval = 0;
 
@@ -102,6 +102,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
 
         if(clientSocket == INVALID_SOCKET) {
             handleError(Protocol::TCP, ErrorSources::ACCEPT_CALL, 500);
+            closeSocket(clientSocket);
             continue;
         };
 
@@ -123,6 +124,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
 
         /* Done, shut down connection */
         retval = shutdown(clientSocket, SHUT_SEND);
+        closeSocket(clientSocket);
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
diff --git a/osal/common/UdpTmTcBridge.cpp b/osal/common/UdpTmTcBridge.cpp
index 7f3dc929..798be6f5 100644
--- a/osal/common/UdpTmTcBridge.cpp
+++ b/osal/common/UdpTmTcBridge.cpp
@@ -103,6 +103,7 @@ ReturnValue_t UdpTmTcBridge::initialize() {
 #endif
         freeaddrinfo(addrResult);
         tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::BIND_CALL);
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
     freeaddrinfo(addrResult);
     return HasReturnvaluesIF::RETURN_OK;

From 00d9a4f3ed44b1bc929271f93f126105aa866616 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 18:57:24 +0200
Subject: [PATCH 019/125] more coverity fixes

---
 osal/host/FixedTimeslotTask.cpp | 1 -
 osal/host/PeriodicTask.cpp      | 1 -
 osal/host/QueueMapManager.cpp   | 4 ++++
 osal/host/QueueMapManager.h     | 2 ++
 4 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp
index 3b169b5a..ee149922 100644
--- a/osal/host/FixedTimeslotTask.cpp
+++ b/osal/host/FixedTimeslotTask.cpp
@@ -38,7 +38,6 @@ FixedTimeslotTask::~FixedTimeslotTask(void) {
     if(mainThread.joinable()) {
         mainThread.join();
     }
-    delete this;
 }
 
 void FixedTimeslotTask::taskEntryPoint(void* argument) {
diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp
index d7abf9d0..631264b7 100644
--- a/osal/host/PeriodicTask.cpp
+++ b/osal/host/PeriodicTask.cpp
@@ -38,7 +38,6 @@ PeriodicTask::~PeriodicTask(void) {
 	if(mainThread.joinable()) {
 		mainThread.join();
 	}
-	delete this;
 }
 
 void PeriodicTask::taskEntryPoint(void* argument) {
diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp
index b50d62dc..c9100fe9 100644
--- a/osal/host/QueueMapManager.cpp
+++ b/osal/host/QueueMapManager.cpp
@@ -10,6 +10,10 @@ QueueMapManager::QueueMapManager() {
     mapLock = MutexFactory::instance()->createMutex();
 }
 
+QueueMapManager::~QueueMapManager() {
+    MutexFactory::instance()->deleteMutex(mapLock);
+}
+
 QueueMapManager* QueueMapManager::instance() {
 	if (mqManagerInstance == nullptr){
 		mqManagerInstance = new QueueMapManager();
diff --git a/osal/host/QueueMapManager.h b/osal/host/QueueMapManager.h
index 3610ca63..90c39c2f 100644
--- a/osal/host/QueueMapManager.h
+++ b/osal/host/QueueMapManager.h
@@ -36,6 +36,8 @@ public:
 private:
 	//! External instantiation is forbidden.
 	QueueMapManager();
+	~QueueMapManager();
+
 	uint32_t queueCounter = 0;
 	MutexIF* mapLock;
 	QueueMap queueMap;

From bddd8720b2898d785f6647d9d383ff65c3d2c4e0 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 18:59:48 +0200
Subject: [PATCH 020/125] another coverity fix

---
 osal/host/FixedTimeslotTask.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp
index ee149922..016b1404 100644
--- a/osal/host/FixedTimeslotTask.cpp
+++ b/osal/host/FixedTimeslotTask.cpp
@@ -118,8 +118,11 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
     }
 
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "Component " << std::hex << componentId <<
-            " not found, not adding it to pst" << std::endl;
+    sif::error << "Component " << std::hex << "0x" << componentId << "not found, "
+            "not adding it to PST.." << std::dec << std::endl;
+#else
+    sif::printError("Component 0x%08x not found, not adding it to PST..\n",
+            static_cast<unsigned int>(componentId));
 #endif
     return HasReturnvaluesIF::RETURN_FAILED;
 }

From efb7c8760a864e45e5686ce450fb8f7b06dc7500 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 19:01:06 +0200
Subject: [PATCH 021/125] coverity action helper fix

---
 action/ActionHelper.cpp | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/action/ActionHelper.cpp b/action/ActionHelper.cpp
index 4b64a40c..b2374ed6 100644
--- a/action/ActionHelper.cpp
+++ b/action/ActionHelper.cpp
@@ -147,11 +147,6 @@ ReturnValue_t ActionHelper::reportData(MessageQueueId_t reportTo,
         return result;
     }
 
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        ipcStore->deleteData(storeAddress);
-        return result;
-    }
-
     /* We don't need to report the objectId, as we receive REQUESTED data before the completion
     success message. True aperiodic replies need to be reported with another dedicated message. */
     ActionMessage::setDataReply(&reply, replyId, storeAddress);

From b30a3aaa382c136df69329e16bae1dfac5f82aa1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 19:07:03 +0200
Subject: [PATCH 022/125] coverity fix

---
 osal/host/MessageQueue.cpp | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index 18272a68..359ce8ec 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -106,6 +106,9 @@ bool MessageQueue::isDefaultDestinationSet() const {
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
         bool ignoreFault) {
+    if(message == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 	message->setSender(sentFrom);
 	if(message->getMessageSize() > message->getMaximumMessageSize()) {
 		// Actually, this should never happen or an error will be emitted
@@ -128,12 +131,12 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	if(targetQueue->messageQueue.size() < targetQueue->messageDepth) {
-		MutexGuard mutexLock(targetQueue->queueLock,
-		        MutexIF::TimeoutType::WAITING, 20);
-		// not ideal, works for now though.
-		MessageQueueMessage* mqmMessage =
-				dynamic_cast<MessageQueueMessage*>(message);
-		if(message != nullptr) {
+		MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);
+		// TODO: Would be nice to support other message types, but this would require
+		// create the message on the heap from an upper layer and simply storing
+		// MessageQueueMessageIF pointers in the queue.
+		MessageQueueMessage* mqmMessage = dynamic_cast<MessageQueueMessage*>(message);
+		if(mqmMessage != nullptr) {
 			targetQueue->messageQueue.push(*mqmMessage);
 		}
 		else {

From 8f4ab6d7edd963166e9f4f0845ff85c9882aeb5a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 19:07:51 +0200
Subject: [PATCH 023/125] coverity: initialize member

---
 serialize/SerializeElement.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/serialize/SerializeElement.h b/serialize/SerializeElement.h
index 47080292..d41098d8 100644
--- a/serialize/SerializeElement.h
+++ b/serialize/SerializeElement.h
@@ -57,7 +57,7 @@ public:
 		return &entry;
 	}
 
-	T entry;
+	T entry = 0;
 };
 
 #endif /* FSFW_SERIALIZE_SERIALIZEELEMENT_H_ */

From dd367bf083422f24f0a2e9aea4ea8b9f213499ce Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 19:09:00 +0200
Subject: [PATCH 024/125] coverity: initialize entry

---
 serialize/SerializeElement.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/serialize/SerializeElement.h b/serialize/SerializeElement.h
index d41098d8..ba34393d 100644
--- a/serialize/SerializeElement.h
+++ b/serialize/SerializeElement.h
@@ -25,7 +25,7 @@ public:
 
 	}
 	SerializeElement() :
-			LinkedElement<SerializeIF>(this) {
+			LinkedElement<SerializeIF>(this), entry(0) {
 	}
 
 	ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize,
@@ -57,7 +57,7 @@ public:
 		return &entry;
 	}
 
-	T entry = 0;
+	T entry;
 };
 
 #endif /* FSFW_SERIALIZE_SERIALIZEELEMENT_H_ */

From e6a1a7cc2d5d5f17f02c70e8bb677d75cde5712e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 19:10:49 +0200
Subject: [PATCH 025/125] coverity fix for event message

---
 events/EventMessage.cpp | 2 +-
 events/EventMessage.h   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/events/EventMessage.cpp b/events/EventMessage.cpp
index bbc41e10..548b4f0f 100644
--- a/events/EventMessage.cpp
+++ b/events/EventMessage.cpp
@@ -109,6 +109,6 @@ bool EventMessage::isClearedEventMessage() {
 	return getEvent() == INVALID_EVENT;
 }
 
-size_t EventMessage::getMinimumMessageSize() {
+size_t EventMessage::getMinimumMessageSize() const {
 	return EVENT_MESSAGE_SIZE;
 }
diff --git a/events/EventMessage.h b/events/EventMessage.h
index 4d003bd7..f2f5ffb5 100644
--- a/events/EventMessage.h
+++ b/events/EventMessage.h
@@ -45,7 +45,7 @@ public:
 
 protected:
 	static const Event INVALID_EVENT = 0;
-	virtual size_t getMinimumMessageSize();
+	virtual size_t getMinimumMessageSize() const override;
 
 };
 

From 905d525aa27069e9948861879a5e104a35e40a4b Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 19:12:21 +0200
Subject: [PATCH 026/125] small fix DHB

---
 devicehandlers/DeviceHandlerBase.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 15eac11f..4a0b3582 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -1483,7 +1483,7 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
         if(errorCode == ObjectManagerIF::CHILD_INIT_FAILED) {
             errorPrint = "Initialization error";
         }
-        if(errorCode == HasReturnvaluesIF::RETURN_FAILED) {
+        else if(errorCode == HasReturnvaluesIF::RETURN_FAILED) {
             if(errorType == sif::OutputTypes::OUT_WARNING) {
                 errorPrint = "Generic Warning";
             }

From 2b84ab018c03774e6a8caff9bb87e9844498ec71 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 20:53:54 +0200
Subject: [PATCH 027/125] implemented mq properly

---
 osal/host/MessageQueue.cpp | 24 ++++++------------------
 osal/host/MessageQueue.h   |  2 +-
 2 files changed, 7 insertions(+), 19 deletions(-)

diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index 359ce8ec..41c55a3d 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -64,9 +64,8 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 		return MessageQueueIF::EMPTY;
 	}
 	MutexGuard mutexLock(queueLock, MutexIF::TimeoutType::WAITING, 20);
-	MessageQueueMessage* currentMessage = &messageQueue.front();
-	std::copy(currentMessage->getBuffer(),
-			currentMessage->getBuffer() + messageSize, message->getBuffer());
+	std::copy(messageQueue.front().data(), messageQueue.front().data() + messageSize,
+	        message->getBuffer());
 	messageQueue.pop();
 	// The last partner is the first uint32_t field in the message
 	this->lastPartner = message->getSender();
@@ -80,7 +79,7 @@ MessageQueueId_t MessageQueue::getLastPartner() const {
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
 	*count = messageQueue.size();
 	// Clears the queue.
-	messageQueue = std::queue<MessageQueueMessage>();
+	messageQueue = std::queue<std::vector<uint8_t>>();
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -132,20 +131,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 	}
 	if(targetQueue->messageQueue.size() < targetQueue->messageDepth) {
 		MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);
-		// TODO: Would be nice to support other message types, but this would require
-		// create the message on the heap from an upper layer and simply storing
-		// MessageQueueMessageIF pointers in the queue.
-		MessageQueueMessage* mqmMessage = dynamic_cast<MessageQueueMessage*>(message);
-		if(mqmMessage != nullptr) {
-			targetQueue->messageQueue.push(*mqmMessage);
-		}
-		else {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::sendMessageFromMessageQueue: Message"
-					"is not MessageQueueMessage!" << std::endl;
-#endif
-		}
-
+		targetQueue->messageQueue.push(std::vector<uint8_t>(message->getMaximumMessageSize()));
+		memcpy(targetQueue->messageQueue.back().data(), message->getBuffer(),
+		        message->getMaximumMessageSize());
 	}
 	else {
 		return MessageQueueIF::FULL;
diff --git a/osal/host/MessageQueue.h b/osal/host/MessageQueue.h
index 97a9e491..e965123d 100644
--- a/osal/host/MessageQueue.h
+++ b/osal/host/MessageQueue.h
@@ -212,7 +212,7 @@ protected:
 	//static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
 
 private:
-	std::queue<MessageQueueMessage> messageQueue;
+	std::queue<std::vector<uint8_t>> messageQueue;
 	/**
 	 * @brief	The class stores the queue id it got assigned.
 	 * 			If initialization fails, the queue id is set to zero.

From c1f4ae08fb4e1c76592735b1dd0741fda14a772d Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 22:28:44 +0200
Subject: [PATCH 028/125] typo fix

---
 ipc/MessageQueueIF.h           | 2 +-
 osal/FreeRTOS/MessageQueue.cpp | 2 +-
 osal/linux/MessageQueue.cpp    | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/ipc/MessageQueueIF.h b/ipc/MessageQueueIF.h
index 1c06521c..74ccb29a 100644
--- a/ipc/MessageQueueIF.h
+++ b/ipc/MessageQueueIF.h
@@ -27,7 +27,7 @@ public:
 	//! Returned if a reply method was called without partner
 	static const ReturnValue_t NO_REPLY_PARTNER = MAKE_RETURN_CODE(3);
 	//! Returned if the target destination is invalid.
-	static constexpr ReturnValue_t DESTINVATION_INVALID = MAKE_RETURN_CODE(4);
+	static constexpr ReturnValue_t DESTINATION_INVALID = MAKE_RETURN_CODE(4);
 
 	virtual ~MessageQueueIF() {}
 	/**
diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index c0c82cf1..3a0f654e 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -135,7 +135,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 	QueueHandle_t destination = nullptr;
 
 	if(sendTo == MessageQueueIF::NO_QUEUE or sendTo == 0x00) {
-		return MessageQueueIF::DESTINVATION_INVALID;
+		return MessageQueueIF::DESTINATION_INVALID;
 	}
 	else  {
 		destination = reinterpret_cast<QueueHandle_t>(sendTo);
diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index 8004c045..12774a58 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -373,7 +373,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 					<<"mq_send to: " << sendTo << " sent from "
 					<< sentFrom << std::endl;
 #endif
-			return DESTINVATION_INVALID;
+			return DESTINATION_INVALID;
 		}
 		case EINTR:
 			//The call was interrupted by a signal.

From d92a20a669d5918422833aaf09b9934a59d8d789 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 8 Apr 2021 22:49:36 +0200
Subject: [PATCH 029/125] coverity

---
 datapool/HkSwitchHelper.cpp      | 2 +-
 globalfunctions/arrayprinter.cpp | 4 ++--
 osal/FreeRTOS/Clock.cpp          | 2 +-
 pus/Service5EventReporting.cpp   | 4 +++-
 4 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/datapool/HkSwitchHelper.cpp b/datapool/HkSwitchHelper.cpp
index 1a2a25eb..21e37f59 100644
--- a/datapool/HkSwitchHelper.cpp
+++ b/datapool/HkSwitchHelper.cpp
@@ -7,7 +7,7 @@ HkSwitchHelper::HkSwitchHelper(EventReportingProxyIF* eventProxy) :
 }
 
 HkSwitchHelper::~HkSwitchHelper() {
-	// TODO Auto-generated destructor stub
+    QueueFactory::instance()->deleteMessageQueue(actionQueue);
 }
 
 ReturnValue_t HkSwitchHelper::initialize() {
diff --git a/globalfunctions/arrayprinter.cpp b/globalfunctions/arrayprinter.cpp
index 20a64f5b..0423360b 100644
--- a/globalfunctions/arrayprinter.cpp
+++ b/globalfunctions/arrayprinter.cpp
@@ -51,7 +51,7 @@ void arrayprinter::printHex(const uint8_t *data, size_t size,
 #else
     // General format: 0x01, 0x02, 0x03 so it is number of chars times 6
     // plus line break plus small safety margin.
-    char printBuffer[(size + 1) * 7 + 1];
+    char printBuffer[(size + 1) * 7 + 1] = {};
     size_t currentPos = 0;
     for(size_t i = 0; i < size; i++) {
         // To avoid buffer overflows.
@@ -94,7 +94,7 @@ void arrayprinter::printDec(const uint8_t *data, size_t size,
 #else
     // General format: 32, 243, -12 so it is number of chars times 5
     // plus line break plus small safety margin.
-    char printBuffer[(size + 1) * 5 + 1];
+    char printBuffer[(size + 1) * 5 + 1] = {};
     size_t currentPos = 0;
     for(size_t i = 0; i < size; i++) {
         // To avoid buffer overflows.
diff --git a/osal/FreeRTOS/Clock.cpp b/osal/FreeRTOS/Clock.cpp
index 806edcc7..c15971fe 100644
--- a/osal/FreeRTOS/Clock.cpp
+++ b/osal/FreeRTOS/Clock.cpp
@@ -111,7 +111,7 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 
 ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from,
         timeval* to) {
-    struct tm time_tm;
+    struct tm time_tm = {};
 
     time_tm.tm_year = from->year - 1900;
     time_tm.tm_mon = from->month - 1;
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index 29eb7f20..965a27ad 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -15,7 +15,9 @@ Service5EventReporting::Service5EventReporting(object_id_t objectId,
 	eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
 
-Service5EventReporting::~Service5EventReporting(){}
+Service5EventReporting::~Service5EventReporting() {
+    QueueFactory::instance()->deleteMessageQueue(eventQueue);
+}
 
 ReturnValue_t Service5EventReporting::performService() {
 	EventMessage message;

From 40cc3c383bca725d162638fa9885ec3f4d83f8fb Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 00:45:04 +0200
Subject: [PATCH 030/125] this should work

---
 serialize/SerializeElement.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/serialize/SerializeElement.h b/serialize/SerializeElement.h
index ba34393d..db66f9cc 100644
--- a/serialize/SerializeElement.h
+++ b/serialize/SerializeElement.h
@@ -25,7 +25,7 @@ public:
 
 	}
 	SerializeElement() :
-			LinkedElement<SerializeIF>(this), entry(0) {
+			LinkedElement<SerializeIF>(this), entry() {
 	}
 
 	ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize,

From d3c3a9147a875f34a4fe95358b5e41cecfa4093f Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 08:29:56 +0200
Subject: [PATCH 031/125] bumped version number

---
 FSFWVersion.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/FSFWVersion.h b/FSFWVersion.h
index 11a60891..df2d49a5 100644
--- a/FSFWVersion.h
+++ b/FSFWVersion.h
@@ -3,9 +3,9 @@
 
 const char* const FSFW_VERSION_NAME = "ASTP";
 
-#define FSFW_VERSION 		0
-#define	FSFW_SUBVERSION 	0
-#define	FSFW_REVISION 	1
+#define FSFW_VERSION        1
+#define	FSFW_SUBVERSION     0
+#define	FSFW_REVISION       0
 
 
 

From b8c7a6570915df561df260ec6e83b03521b9114e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 08:59:32 +0200
Subject: [PATCH 032/125] coverity fix

---
 datapoollocal/LocalDataPoolManager.cpp | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/datapoollocal/LocalDataPoolManager.cpp b/datapoollocal/LocalDataPoolManager.cpp
index 94ee9c26..dbe68ff1 100644
--- a/datapoollocal/LocalDataPoolManager.cpp
+++ b/datapoollocal/LocalDataPoolManager.cpp
@@ -909,27 +909,29 @@ void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
 			errorPrint = "Unknown error";
 		}
 	}
+	object_id_t objectId = 0xffffffff;
+	if(owner != nullptr) {
+	    objectId = owner->getObjectId();
+	}
 
 	if(outputType == sif::OutputTypes::OUT_WARNING) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "LocalDataPoolManager::" << functionName
-				<< ": Object ID 0x" << std::setw(8) << std::setfill('0')
-				<< std::hex << owner->getObjectId() << " | " << errorPrint
-				<< std::dec << std::setfill(' ') << std::endl;
+		sif::warning << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+		        std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+		        std::dec << std::setfill(' ') << std::endl;
 #else
 		sif::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
-				functionName, owner->getObjectId(), errorPrint);
+				functionName, objectId, errorPrint);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 	}
 	else if(outputType == sif::OutputTypes::OUT_ERROR) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "LocalDataPoolManager::" << functionName
-				<< ": Object ID 0x" << std::setw(8) << std::setfill('0')
-				<< std::hex << owner->getObjectId() << " | " << errorPrint
-				<< std::dec << std::setfill(' ') << std::endl;
+		sif::error << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+		        std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+		        std::dec << std::setfill(' ') << std::endl;
 #else
 		sif::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
-				functionName, owner->getObjectId(), errorPrint);
+				functionName, objectId, errorPrint);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 	}
 #endif /* #if FSFW_VERBOSE_LEVEL >= 1 */

From d9a0a4f2ea75f62035287c3e75f2272b995d77b5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 09:14:42 +0200
Subject: [PATCH 033/125] coverity

---
 pus/Service1TelecommandVerification.cpp | 4 +++-
 tmtcservices/TmTcBridge.cpp             | 4 +++-
 2 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index 9e86c752..7ce75478 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -15,7 +15,9 @@ Service1TelecommandVerification::Service1TelecommandVerification(
 	tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
 
-Service1TelecommandVerification::~Service1TelecommandVerification() {}
+Service1TelecommandVerification::~Service1TelecommandVerification() {
+    QueueFactory::instance()->deleteMessageQueue(tmQueue);
+}
 
 MessageQueueId_t Service1TelecommandVerification::getVerificationQueue(){
 	return tmQueue->getId();
diff --git a/tmtcservices/TmTcBridge.cpp b/tmtcservices/TmTcBridge.cpp
index f99b9051..dcffac41 100644
--- a/tmtcservices/TmTcBridge.cpp
+++ b/tmtcservices/TmTcBridge.cpp
@@ -16,7 +16,9 @@ TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination,
             createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
 }
 
-TmTcBridge::~TmTcBridge() {}
+TmTcBridge::~TmTcBridge() {
+    QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue);
+}
 
 ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(
 		uint8_t sentPacketsPerCycle) {

From e0d39b1feb04aafdb3fbe9e57a2f84ed9923300a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 09:17:11 +0200
Subject: [PATCH 034/125] coverity

---
 pus/Service8FunctionManagement.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/pus/Service8FunctionManagement.cpp b/pus/Service8FunctionManagement.cpp
index a2202abc..54187a82 100644
--- a/pus/Service8FunctionManagement.cpp
+++ b/pus/Service8FunctionManagement.cpp
@@ -53,12 +53,14 @@ ReturnValue_t Service8FunctionManagement::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t Service8FunctionManagement::prepareCommand(
 		CommandMessage* message, uint8_t subservice, const uint8_t* tcData,
 		size_t tcDataLen, uint32_t* state, object_id_t objectId) {
-	return prepareDirectCommand(dynamic_cast<CommandMessage*>(message),
-				tcData, tcDataLen);
+	return prepareDirectCommand(message, tcData, tcDataLen);
 }
 
 ReturnValue_t Service8FunctionManagement::prepareDirectCommand(
 		CommandMessage *message, const uint8_t *tcData, size_t tcDataLen) {
+    if(message == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
     if(tcDataLen < sizeof(object_id_t) + sizeof(ActionId_t)) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::debug << "Service8FunctionManagement::prepareDirectCommand:"

From 6db0725aa4293cfafdf9cdea87d701a3892cd2eb Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 15:39:48 +0200
Subject: [PATCH 035/125] coverity fix

---
 health/HealthTable.cpp | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/health/HealthTable.cpp b/health/HealthTable.cpp
index 5d720b19..8b19d8e3 100644
--- a/health/HealthTable.cpp
+++ b/health/HealthTable.cpp
@@ -68,8 +68,18 @@ void HealthTable::printAll(uint8_t* pointer, size_t maxSize) {
     MutexGuard(mutex, timeoutType, mutexTimeoutMs);
 	size_t size = 0;
 	uint16_t count = healthMap.size();
-	SerializeAdapter::serialize(&count,
+	ReturnValue_t result = SerializeAdapter::serialize(&count,
 			&pointer, &size, maxSize, SerializeIF::Endianness::BIG);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::warning << "HealthTable::printAll: Serialization of health table failed" << std::endl;
+#else
+	    sif::printWarning("HealthTable::printAll: Serialization of health table failed\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+	    return;
+	}
 	for (const auto& health: healthMap) {
 		SerializeAdapter::serialize(&health.first,
 				&pointer, &size, maxSize, SerializeIF::Endianness::BIG);

From 316310e99301790d9674d69e3c17e9ff782a34e9 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 15:49:33 +0200
Subject: [PATCH 036/125] coverity

---
 devicehandlers/DeviceHandlerBase.cpp | 2 +-
 pus/CService200ModeCommanding.cpp    | 3 +--
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 4a0b3582..8c8919ce 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -1515,7 +1515,7 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
                 << std::setfill(' ') << std::endl;
 #else
         sif::printError("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
-                this->getObjectId(), errorPrint);
+                functionName, this->getObjectId(), errorPrint);
 #endif
     }
 
diff --git a/pus/CService200ModeCommanding.cpp b/pus/CService200ModeCommanding.cpp
index c4e99359..70caadd1 100644
--- a/pus/CService200ModeCommanding.cpp
+++ b/pus/CService200ModeCommanding.cpp
@@ -61,8 +61,7 @@ ReturnValue_t CService200ModeCommanding::prepareCommand(
 		return result;
 	}
 
-	ModeMessage::setModeMessage(dynamic_cast<CommandMessage*>(message),
-	        ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
+	ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
 	        modeCommandPacket.getSubmode());
 	return result;
 }

From 2e417c787d8fa38f9ff0bc9a73b44f900f796c75 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 16:14:14 +0200
Subject: [PATCH 037/125] coverity

---
 devicehandlers/DeviceHandlerBase.cpp | 5 ++++-
 pus/Service20ParameterManagement.cpp | 5 ++---
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 8c8919ce..531a0642 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -1495,6 +1495,9 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
             errorPrint = "Unknown error";
         }
     }
+    if(functionName == nullptr) {
+        functionName = "unknown function";
+    }
 
     if(errorType == sif::OutputTypes::OUT_WARNING) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -1504,7 +1507,7 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
                 << std::setfill(' ') << std::endl;
 #else
         sif::printWarning("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
-                this->getObjectId(), errorPrint);
+                functionName, this->getObjectId(), errorPrint);
 #endif
     }
     else if(errorType == sif::OutputTypes::OUT_ERROR) {
diff --git a/pus/Service20ParameterManagement.cpp b/pus/Service20ParameterManagement.cpp
index bc3a9119..90e96650 100644
--- a/pus/Service20ParameterManagement.cpp
+++ b/pus/Service20ParameterManagement.cpp
@@ -75,9 +75,8 @@ ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue
 #else
         sif::printError("Service20ParameterManagement::checkInterfaceAndAcquire"
                 "MessageQueue: Can't access object\n");
-        sif::printError("Object ID: 0x%08x\n", objectId);
-        sif::printError("Make sure it implements "
-                "ReceivesParameterMessagesIF!\n");
+        sif::printError("Object ID: 0x%08x\n", *objectId);
+        sif::printError("Make sure it implements ReceivesParameterMessagesIF!\n");
 #endif
 
         return CommandingServiceBase::INVALID_OBJECT;

From 83e7dbb1f0ab91fa7068e628d66d70633afd5238 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Fri, 9 Apr 2021 16:17:21 +0200
Subject: [PATCH 038/125] Coveritx fixes

---
 pus/servicepackets/Service8Packets.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/pus/servicepackets/Service8Packets.h b/pus/servicepackets/Service8Packets.h
index b026edf5..a27cf8bb 100644
--- a/pus/servicepackets/Service8Packets.h
+++ b/pus/servicepackets/Service8Packets.h
@@ -43,8 +43,8 @@ public:
 
 private:
 	DirectCommand(const DirectCommand &command);
-	object_id_t objectId;
-	ActionId_t actionId;
+	object_id_t objectId = 0;
+	ActionId_t actionId = 0;
 	uint32_t parametersSize; //!< [EXPORT] : [IGNORE]
 	const uint8_t * parameterBuffer; //!< [EXPORT] : [MAXSIZE] 65535 Bytes
 

From f1ffa88e07718e14581e71e9e53f74f471a54946 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 10 Apr 2021 14:29:00 +0200
Subject: [PATCH 039/125] small bugfix

---
 osal/host/FixedTimeslotTask.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp
index 016b1404..89daa278 100644
--- a/osal/host/FixedTimeslotTask.cpp
+++ b/osal/host/FixedTimeslotTask.cpp
@@ -3,7 +3,7 @@
 #include "../../ipc/MutexFactory.h"
 #include "../../osal/host/Mutex.h"
 #include "../../osal/host/FixedTimeslotTask.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
 #include <thread>

From 5144cbd789f2fe30cf02891d29301204dcdf7818 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 11 Apr 2021 02:26:11 +0200
Subject: [PATCH 040/125] additional inc path now in IF

---
 CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ff060d7b..8ba6a187 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -184,6 +184,7 @@ endif()
 target_include_directories(${LIB_FSFW_NAME} INTERFACE 
 	${CMAKE_SOURCE_DIR}
 	${FSFW_CONFIG_PATH_ABSOLUTE}
+	${FSFW_ADD_INC_PATHS_ABS}
 )
 
 # Includes path required to compile FSFW itself as well

From fbe860c6a51247104a84fdc9f94064810ca2e2f8 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 11 Apr 2021 14:54:02 +0200
Subject: [PATCH 041/125] refactored addr print, using AI_PASSIVE now

---
 osal/common/UdpTcPollingTask.cpp |  9 +++++---
 osal/common/UdpTmTcBridge.cpp    | 21 ++++++++++---------
 osal/common/tcpipCommon.cpp      | 35 ++++++++++++++++++++++++++++++++
 osal/common/tcpipCommon.h        | 11 ++++++++--
 4 files changed, 61 insertions(+), 15 deletions(-)

diff --git a/osal/common/UdpTcPollingTask.cpp b/osal/common/UdpTcPollingTask.cpp
index 759cee05..47f67b29 100644
--- a/osal/common/UdpTcPollingTask.cpp
+++ b/osal/common/UdpTcPollingTask.cpp
@@ -15,7 +15,7 @@
 #endif
 
 //! Debugging preprocessor define.
-#define FSFW_UDP_RCV_WIRETAPPING_ENABLED    0
+#define FSFW_UDP_RECV_WIRETAPPING_ENABLED    0
 
 UdpTcPollingTask::UdpTcPollingTask(object_id_t objectId,
         object_id_t tmtcUnixUdpBridge, size_t maxRecvSize,
@@ -66,10 +66,13 @@ ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) {
             tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::RECVFROM_CALL, 1000);
             continue;
         }
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1
+#if FSFW_UDP_RECV_WIRETAPPING_ENABLED == 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::debug << "UdpTcPollingTask::performOperation: " << bytesReceived <<
                 " bytes received" << std::endl;
+#else
 #endif
+#endif /* FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1 */
 
         ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
         if(result != HasReturnvaluesIF::RETURN_FAILED) {
@@ -84,7 +87,7 @@ ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) {
 ReturnValue_t UdpTcPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
     store_address_t storeId;
 
-#if FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1
+#if FSFW_UDP_RECV_WIRETAPPING_ENABLED == 1
     arrayprinter::print(receptionBuffer.data(), bytesRead);
 #endif
 
diff --git a/osal/common/UdpTmTcBridge.cpp b/osal/common/UdpTmTcBridge.cpp
index 798be6f5..ba23f521 100644
--- a/osal/common/UdpTmTcBridge.cpp
+++ b/osal/common/UdpTmTcBridge.cpp
@@ -70,6 +70,7 @@ ReturnValue_t UdpTmTcBridge::initialize() {
     hints.ai_family = AF_INET;
     hints.ai_socktype = SOCK_DGRAM;
     hints.ai_protocol = IPPROTO_UDP;
+    hints.ai_flags = AI_PASSIVE;
 
     /* Set up UDP socket:
     https://en.wikipedia.org/wiki/Getaddrinfo
@@ -95,6 +96,10 @@ ReturnValue_t UdpTmTcBridge::initialize() {
         return HasReturnvaluesIF::RETURN_FAILED;
     }
 
+#if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
+    tcpip::printAddress(addrResult->ai_addr);
+#endif
+
     retval = bind(serverSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
     if(retval != 0) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -121,10 +126,8 @@ ReturnValue_t UdpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) {
     /* The target address can be set by different threads so this lock ensures thread-safety */
     MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
 
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
-    char ipAddress [15];
-    sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
-            &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+#if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
+    tcpip::printAddress(&clientAddress);
 #endif
 
     int bytesSent = sendto(
@@ -152,13 +155,11 @@ void UdpTmTcBridge::checkAndSetClientAddress(sockaddr& newAddress) {
     /* The target address can be set by different threads so this lock ensures thread-safety */
     MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
 
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
-    char ipAddress [15];
-    sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
-            &newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
-    sif::debug << "IP Address Old: " <<  inet_ntop(AF_INET,
-            &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+#if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
+    tcpip::printAddress(&newAddress);
+    tcpip::printAddress(&clientAddress);
 #endif
+
     registerCommConnect();
 
     /* Set new IP address to reply to */
diff --git a/osal/common/tcpipCommon.cpp b/osal/common/tcpipCommon.cpp
index 9a5e4647..ab4eda29 100644
--- a/osal/common/tcpipCommon.cpp
+++ b/osal/common/tcpipCommon.cpp
@@ -1,4 +1,5 @@
 #include "tcpipCommon.h"
+#include <fsfw/serviceinterface/ServiceInterface.h>
 
 void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string &protStr,
         std::string &srcString) {
@@ -34,3 +35,37 @@ void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std:
         srcString = "unknown call";
     }
 }
+
+void tcpip::printAddress(struct sockaddr* addr) {
+    char ipAddress[INET6_ADDRSTRLEN] = {};
+    const char* stringPtr = NULL;
+    switch(addr->sa_family) {
+    case AF_INET: {
+        struct sockaddr_in *addrIn = reinterpret_cast<struct sockaddr_in*>(addr);
+        stringPtr = inet_ntop(AF_INET, &(addrIn->sin_addr), ipAddress, INET_ADDRSTRLEN);
+        break;
+    }
+    case AF_INET6: {
+        struct sockaddr_in6 *addrIn = reinterpret_cast<struct sockaddr_in6*>(addr);
+        stringPtr = inet_ntop(AF_INET6, &(addrIn->sin6_addr), ipAddress, INET6_ADDRSTRLEN);
+        break;
+    }
+    }
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    if(stringPtr == NULL) {
+        sif::debug << "Could not convert IP address to text representation, error code "
+                << errno << std::endl;
+    }
+    else {
+        sif::debug << "IP Address Sender: " <<  ipAddress << std::endl;
+    }
+#else
+    if(stringPtr == NULL) {
+        sif::printDebug("Could not convert IP address to text representation, error code %d\n",
+                errno);
+    }
+    else {
+        sif::printDebug("IP Address Sender: %s\n", ipAddress);
+    }
+#endif
+}
diff --git a/osal/common/tcpipCommon.h b/osal/common/tcpipCommon.h
index dc5ada52..22b914dc 100644
--- a/osal/common/tcpipCommon.h
+++ b/osal/common/tcpipCommon.h
@@ -4,6 +4,13 @@
 #include "../../timemanager/clockDefinitions.h"
 #include <string>
 
+#ifdef _WIN32
+#include <winsock2.h>
+#else
+#include <netdb.h>
+#include <arpa/inet.h>
+#endif
+
 namespace tcpip {
 
 const char* const DEFAULT_SERVER_PORT = "7301";
@@ -28,8 +35,8 @@ enum class ErrorSources {
 void determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string& protStr,
         std::string& srcString);
 
+void printAddress(struct sockaddr* addr);
+
 }
 
-
-
 #endif /* FSFW_OSAL_COMMON_TCPIPCOMMON_H_ */

From 924ea420a9f72d188000dc73bfe2500635e2ddad Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 11 Apr 2021 14:58:53 +0200
Subject: [PATCH 042/125] missing include for windows

---
 osal/common/tcpipCommon.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/osal/common/tcpipCommon.cpp b/osal/common/tcpipCommon.cpp
index ab4eda29..551e2a42 100644
--- a/osal/common/tcpipCommon.cpp
+++ b/osal/common/tcpipCommon.cpp
@@ -1,6 +1,10 @@
 #include "tcpipCommon.h"
 #include <fsfw/serviceinterface/ServiceInterface.h>
 
+#ifdef _WIN32
+#include <ws2tcpip.h>
+#endif
+
 void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string &protStr,
         std::string &srcString) {
     if(protocol == Protocol::TCP) {

From 6e6fb62b3cb66561b76fc86b320e05d5559c2be9 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 11 Apr 2021 18:36:24 +0200
Subject: [PATCH 043/125] last coverity fixes

---
 devicehandlers/HealthDevice.cpp | 4 ++--
 health/HealthTable.cpp          | 2 +-
 timemanager/TimeMessage.cpp     | 2 +-
 timemanager/TimeMessage.h       | 2 +-
 tmtcservices/TmTcMessage.cpp    | 2 +-
 tmtcservices/TmTcMessage.h      | 2 +-
 6 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/devicehandlers/HealthDevice.cpp b/devicehandlers/HealthDevice.cpp
index 418ed257..e23dd5b6 100644
--- a/devicehandlers/HealthDevice.cpp
+++ b/devicehandlers/HealthDevice.cpp
@@ -16,9 +16,9 @@ ReturnValue_t HealthDevice::performOperation(uint8_t opCode) {
 	CommandMessage command;
 	ReturnValue_t result = commandQueue->receiveMessage(&command);
 	if (result == HasReturnvaluesIF::RETURN_OK) {
-		healthHelper.handleHealthCommand(&command);
+		result = healthHelper.handleHealthCommand(&command);
 	}
-	return HasReturnvaluesIF::RETURN_OK;
+	return result;
 }
 
 ReturnValue_t HealthDevice::initialize() {
diff --git a/health/HealthTable.cpp b/health/HealthTable.cpp
index 8b19d8e3..5717fe9f 100644
--- a/health/HealthTable.cpp
+++ b/health/HealthTable.cpp
@@ -96,7 +96,7 @@ ReturnValue_t HealthTable::iterate(HealthEntry *value, bool reset) {
 		mapIterator = healthMap.begin();
 	}
 	if (mapIterator == healthMap.end()) {
-		result = HasReturnvaluesIF::RETURN_FAILED;
+		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	*value = *mapIterator;
 	mapIterator++;
diff --git a/timemanager/TimeMessage.cpp b/timemanager/TimeMessage.cpp
index a1042efe..66aea0f4 100644
--- a/timemanager/TimeMessage.cpp
+++ b/timemanager/TimeMessage.cpp
@@ -25,6 +25,6 @@ uint32_t TimeMessage::getCounterValue() {
 	return temp;
 }
 
-size_t TimeMessage::getMinimumMessageSize() {
+size_t TimeMessage::getMinimumMessageSize() const {
 	return this->MAX_SIZE;
 }
diff --git a/timemanager/TimeMessage.h b/timemanager/TimeMessage.h
index f5ac3e14..00778fb7 100644
--- a/timemanager/TimeMessage.h
+++ b/timemanager/TimeMessage.h
@@ -11,7 +11,7 @@ protected:
 	 * @brief	This call always returns the same fixed size of the message.
 	 * @return	Returns HEADER_SIZE + \c sizeof(timeval) + sizeof(uint32_t).
 	 */
-	size_t getMinimumMessageSize();
+	size_t getMinimumMessageSize() const override;
 public:
 
 	/**
diff --git a/tmtcservices/TmTcMessage.cpp b/tmtcservices/TmTcMessage.cpp
index ae028315..c0f32aaa 100644
--- a/tmtcservices/TmTcMessage.cpp
+++ b/tmtcservices/TmTcMessage.cpp
@@ -21,7 +21,7 @@ TmTcMessage::TmTcMessage(store_address_t storeId) {
 	this->setStorageId(storeId);
 }
 
-size_t TmTcMessage::getMinimumMessageSize() {
+size_t TmTcMessage::getMinimumMessageSize() const {
 	return this->HEADER_SIZE + sizeof(store_address_t);
 }
 
diff --git a/tmtcservices/TmTcMessage.h b/tmtcservices/TmTcMessage.h
index 41fe198a..7d2a7bdb 100644
--- a/tmtcservices/TmTcMessage.h
+++ b/tmtcservices/TmTcMessage.h
@@ -18,7 +18,7 @@ protected:
 	 * @brief	This call always returns the same fixed size of the message.
 	 * @return	Returns HEADER_SIZE + @c sizeof(store_address_t).
 	 */
-	size_t getMinimumMessageSize();
+	size_t getMinimumMessageSize() const override;
 public:
 	/**
 	 * @brief	In the default constructor, only the message_size is set.

From 438049bb803eb1ecb770bdd8ad941538ec7f179a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 11 Apr 2021 18:50:48 +0200
Subject: [PATCH 044/125] another coverity fix

---
 thermal/Heater.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/thermal/Heater.cpp b/thermal/Heater.cpp
index 782ce296..d148c871 100644
--- a/thermal/Heater.cpp
+++ b/thermal/Heater.cpp
@@ -286,7 +286,10 @@ void Heater::handleQueue() {
 		if (result == HasReturnvaluesIF::RETURN_OK) {
 			return;
 		}
-		parameterHelper.handleParameterMessage(&command);
+		result = parameterHelper.handleParameterMessage(&command);
+        if (result == HasReturnvaluesIF::RETURN_OK) {
+            return;
+        }
 	}
 }
 

From 5d93cf12f771506d75a99df015f4d892d84765c5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 11 Apr 2021 18:55:10 +0200
Subject: [PATCH 045/125] another coverity  fix

---
 health/HealthTable.cpp | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/health/HealthTable.cpp b/health/HealthTable.cpp
index 5717fe9f..3fed1deb 100644
--- a/health/HealthTable.cpp
+++ b/health/HealthTable.cpp
@@ -81,11 +81,17 @@ void HealthTable::printAll(uint8_t* pointer, size_t maxSize) {
 	    return;
 	}
 	for (const auto& health: healthMap) {
-		SerializeAdapter::serialize(&health.first,
+		result = SerializeAdapter::serialize(&health.first,
 				&pointer, &size, maxSize, SerializeIF::Endianness::BIG);
+		if(result != HasReturnvaluesIF::RETURN_OK) {
+		    return;
+		}
 		uint8_t healthValue = health.second;
-		SerializeAdapter::serialize(&healthValue, &pointer, &size,
+		result = SerializeAdapter::serialize(&healthValue, &pointer, &size,
 				maxSize, SerializeIF::Endianness::BIG);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return;
+        }
 	}
 }
 

From d7d52439d76a114bebaa3a67e4a59385eac91540 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 12 Apr 2021 20:40:41 +0200
Subject: [PATCH 046/125] added new option

---
 defaultcfg/fsfwconfig/FSFWConfig.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index ed86e6e1..9a07b4ea 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -17,6 +17,8 @@
 	#define FSFW_DISABLE_PRINTOUT 	0
 #endif
 
+#define FSFW_USE_PUS_C_TELEMETRY    1
+
 //! Can be used to disable the ANSI color sequences for C stdio.
 #define FSFW_COLORED_OUTPUT 		1
 

From 9a5f717169069f23330b47ed73c62cfd8139e96e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 12 Apr 2021 21:17:53 +0200
Subject: [PATCH 047/125] refactoring to allow PUS c implementation

---
 tmtcpacket/pus/CMakeLists.txt     |   2 +
 tmtcpacket/pus/TmPacketBase.cpp   | 106 +++-----------
 tmtcpacket/pus/TmPacketBase.h     | 234 +++++++++++-------------------
 tmtcpacket/pus/TmPacketIF.h       |   8 +
 tmtcpacket/pus/TmPacketPusA.cpp   | 121 +++++++++++++++
 tmtcpacket/pus/TmPacketPusA.h     | 137 +++++++++++++++++
 tmtcpacket/pus/TmPacketPusC.cpp   |   2 +
 tmtcpacket/pus/TmPacketPusC.h     |   8 +
 tmtcpacket/pus/TmPacketStored.cpp |  10 +-
 tmtcpacket/pus/TmPacketStored.h   |   4 +-
 10 files changed, 398 insertions(+), 234 deletions(-)
 create mode 100644 tmtcpacket/pus/TmPacketIF.h
 create mode 100644 tmtcpacket/pus/TmPacketPusA.cpp
 create mode 100644 tmtcpacket/pus/TmPacketPusA.h
 create mode 100644 tmtcpacket/pus/TmPacketPusC.cpp
 create mode 100644 tmtcpacket/pus/TmPacketPusC.h

diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index fcfc82d2..1fdd9dd8 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -5,4 +5,6 @@ target_sources(${LIB_FSFW_NAME}
 		TmPacketBase.cpp
 		TmPacketMinimal.cpp
 		TmPacketStored.cpp
+		TmPacketPusA.cpp
+		TmPacketPusC.cpp
 )
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp
index c8e4b430..10f37b00 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/TmPacketBase.cpp
@@ -11,111 +11,53 @@
 TimeStamperIF* TmPacketBase::timeStamper = nullptr;
 object_id_t TmPacketBase::timeStamperId = 0;
 
-TmPacketBase::TmPacketBase(uint8_t* setData) :
-		SpacePacketBase(setData) {
-	tmData = reinterpret_cast<TmPacketPointer*>(setData);
+TmPacketBase::TmPacketBase(uint8_t* setData):
+        SpacePacketBase(setData) {
 }
 
 TmPacketBase::~TmPacketBase() {
-	//Nothing to do.
+    //Nothing to do.
 }
 
-uint8_t TmPacketBase::getService() {
-	return tmData->data_field.service_type;
-}
-
-uint8_t TmPacketBase::getSubService() {
-	return tmData->data_field.service_subtype;
-}
-
-uint8_t* TmPacketBase::getSourceData() {
-	return &tmData->data;
-}
 
 uint16_t TmPacketBase::getSourceDataSize() {
-	return getPacketDataLength() - sizeof(tmData->data_field)
-			- CRC_SIZE + 1;
+    return getPacketDataLength() - getDataFieldSize() - CRC_SIZE + 1;
 }
 
 uint16_t TmPacketBase::getErrorControl() {
-	uint32_t size = getSourceDataSize() + CRC_SIZE;
-	uint8_t* p_to_buffer = &tmData->data;
-	return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
+    uint32_t size = getSourceDataSize() + CRC_SIZE;
+    uint8_t* p_to_buffer = getSourceData();
+    return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
 }
 
 void TmPacketBase::setErrorControl() {
-	uint32_t full_size = getFullSize();
-	uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
-	uint32_t size = getSourceDataSize();
-	getSourceData()[size] = (crc & 0XFF00) >> 8;	// CRCH
-	getSourceData()[size + 1] = (crc) & 0X00FF; 		// CRCL
+    uint32_t full_size = getFullSize();
+    uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
+    uint32_t size = getSourceDataSize();
+    getSourceData()[size] = (crc & 0XFF00) >> 8;	// CRCH
+    getSourceData()[size + 1] = (crc) & 0X00FF; 		// CRCL
 }
 
-void TmPacketBase::setData(const uint8_t* p_Data) {
-	SpacePacketBase::setData(p_Data);
-	tmData = (TmPacketPointer*) p_Data;
-}
+
 
 void TmPacketBase::print() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	 sif::debug << "TmPacketBase::print: " << std::endl;
+    sif::debug << "TmPacketBase::print: " << std::endl;
 #endif
-	 arrayprinter::print(getWholeData(), getFullSize());
+    arrayprinter::print(getWholeData(), getFullSize());
 }
 
 bool TmPacketBase::checkAndSetStamper() {
-	if (timeStamper == NULL) {
-		timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
-		if (timeStamper == NULL) {
+    if (timeStamper == NULL) {
+        timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
+        if (timeStamper == NULL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TmPacketBase::checkAndSetStamper: Stamper not found!"
-					<< std::endl;
+            sif::error << "TmPacketBase::checkAndSetStamper: Stamper not found!"
+                    << std::endl;
 #endif
-			return false;
-		}
-	}
-	return true;
+            return false;
+        }
+    }
+    return true;
 }
 
-ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const {
-	size_t tempSize = 0;
-	return CCSDSTime::convertFromCcsds(timestamp, tmData->data_field.time,
-			&tempSize, sizeof(tmData->data_field.time));
-}
-
-uint8_t* TmPacketBase::getPacketTimeRaw() const{
-	return tmData->data_field.time;
-
-}
-
-void TmPacketBase::initializeTmPacket(uint16_t apid, uint8_t service,
-        uint8_t subservice, uint8_t packetSubcounter) {
-	//Set primary header:
-	initSpacePacketHeader(false, true, apid);
-	//Set data Field Header:
-	//First, set to zero.
-	memset(&tmData->data_field, 0, sizeof(tmData->data_field));
-
-	// NOTE: In PUS-C, the PUS Version is 2 and specified for the first 4 bits.
-	// The other 4 bits of the first byte are the spacecraft time reference
-	// status. To change to PUS-C, set 0b00100000.
-    // Set CCSDS_secondary header flag to 0, version number to 001 and ack
-    // to 0000
-	tmData->data_field.version_type_ack = 0b00010000;
-	tmData->data_field.service_type = service;
-	tmData->data_field.service_subtype = subservice;
-	tmData->data_field.subcounter = packetSubcounter;
-	//Timestamp packet
-	if (checkAndSetStamper()) {
-		timeStamper->addTimeStamp(tmData->data_field.time,
-		        sizeof(tmData->data_field.time));
-	}
-}
-
-void TmPacketBase::setSourceDataSize(uint16_t size) {
-	setPacketDataLength(size + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1);
-}
-
-size_t TmPacketBase::getTimestampSize() const {
-	return sizeof(tmData->data_field.time);
-}
diff --git a/tmtcpacket/pus/TmPacketBase.h b/tmtcpacket/pus/TmPacketBase.h
index 6efc0165..72cdbb69 100644
--- a/tmtcpacket/pus/TmPacketBase.h
+++ b/tmtcpacket/pus/TmPacketBase.h
@@ -10,32 +10,6 @@ namespace Factory{
 void setStaticFrameworkObjectIds();
 }
 
-/**
- * This struct defines a byte-wise structured PUS TM Data Field Header.
- * Any optional fields in the header must be added or removed here.
- * Currently, no Destination field is present, but an eigth-byte representation
- * for a time tag.
- * @ingroup tmtcpackets
- */
-struct PUSTmDataFieldHeader {
-	uint8_t version_type_ack;
-	uint8_t service_type;
-	uint8_t service_subtype;
-	uint8_t subcounter;
-//	uint8_t destination;
-	uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
-};
-
-/**
- * This struct defines the data structure of a PUS Telecommand Packet when
- * accessed via a pointer.
- * @ingroup tmtcpackets
- */
-struct TmPacketPointer {
-	CCSDSPrimaryHeader primary;
-	PUSTmDataFieldHeader data_field;
-	uint8_t data;
-};
 
 /**
  * This class is the basic data handler for any ECSS PUS Telemetry packet.
@@ -49,61 +23,84 @@ struct TmPacketPointer {
  * @ingroup tmtcpackets
  */
 class TmPacketBase : public SpacePacketBase {
-	friend void (Factory::setStaticFrameworkObjectIds)();
+    friend void (Factory::setStaticFrameworkObjectIds)();
 public:
-	/**
-	 * This constant defines the minimum size of a valid PUS Telemetry Packet.
-	 */
-	static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
-	        sizeof(PUSTmDataFieldHeader) + 2);
-	//! Maximum size of a TM Packet in this mission.
-	//! TODO: Make this dependant on a config variable.
-	static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
-	//! First byte of secondary header for PUS-A packets.
-	//! TODO: Maybe also support PUS-C via config?
-	static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 0b00010000;
 
-	/**
-	 * This is the default constructor.
-	 * It sets its internal data pointer to the address passed and also
-	 * forwards the data pointer to the parent SpacePacketBase class.
-	 * @param set_address	The position where the packet data lies.
-	 */
-	TmPacketBase( uint8_t* setData );
-	/**
-	 * This is the empty default destructor.
-	 */
-	virtual ~TmPacketBase();
+    //! Maximum size of a TM Packet in this mission.
+    //! TODO: Make this dependant on a config variable.
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
+    //! First byte of secondary header for PUS-A packets.
+    //! TODO: Maybe also support PUS-C via config?
+    static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 0b00010000;
 
-	/**
-	 * This is a getter for the packet's PUS Service ID, which is the second
-	 * byte of the Data Field Header.
-	 * @return	The packet's PUS Service ID.
-	 */
-	uint8_t getService();
-	/**
-	 * This is a getter for the packet's PUS Service Subtype, which is the
-	 * third byte of the Data Field Header.
-	 * @return	The packet's PUS Service Subtype.
-	 */
-	uint8_t getSubService();
-	/**
-	 * This is a getter for a pointer to the packet's Source data.
-	 *
-	 * These are the bytes that follow after the Data Field Header. They form
-	 * the packet's source data.
-	 * @return	A pointer to the PUS Source Data.
-	 */
-	uint8_t* getSourceData();
-	/**
-	 * This method calculates the size of the PUS Source data field.
-	 *
-	 * It takes the information stored in the CCSDS Packet Data Length field
-	 * and subtracts the Data Field Header size and the CRC size.
-	 * @return	The size of the PUS Source Data (without Error Control field)
-	 */
-	uint16_t getSourceDataSize();
+    /**
+     * This is the default constructor.
+     * It sets its internal data pointer to the address passed and also
+     * forwards the data pointer to the parent SpacePacketBase class.
+     * @param set_address	The position where the packet data lies.
+     */
+    TmPacketBase( uint8_t* setData );
+    /**
+     * This is the empty default destructor.
+     */
+    virtual ~TmPacketBase();
 
+    /**
+     * This is a getter for the packet's PUS Service ID, which is the second
+     * byte of the Data Field Header.
+     * @return	The packet's PUS Service ID.
+     */
+    virtual uint8_t getService() = 0;
+    /**
+     * This is a getter for the packet's PUS Service Subtype, which is the
+     * third byte of the Data Field Header.
+     * @return	The packet's PUS Service Subtype.
+     */
+    virtual uint8_t getSubService() = 0;
+    /**
+     * This is a getter for a pointer to the packet's Source data.
+     *
+     * These are the bytes that follow after the Data Field Header. They form
+     * the packet's source data.
+     * @return	A pointer to the PUS Source Data.
+     */
+    virtual uint8_t* getSourceData() = 0;
+    /**
+     * This method calculates the size of the PUS Source data field.
+     *
+     * It takes the information stored in the CCSDS Packet Data Length field
+     * and subtracts the Data Field Header size and the CRC size.
+     * @return	The size of the PUS Source Data (without Error Control field)
+     */
+    virtual uint16_t getSourceDataSize() = 0;
+
+    /**
+     * Get size of data field which can differ based on implementation
+     * @return
+     */
+    virtual uint16_t getDataFieldSize() = 0;
+
+    virtual size_t getPacketMinimumSize() const = 0;
+
+    /**
+     * Interprets the "time"-field in the secondary header and returns it in
+     * timeval format.
+     * @return Converted timestamp of packet.
+     */
+    virtual ReturnValue_t getPacketTime(timeval* timestamp) const = 0;
+    /**
+     * Returns a raw pointer to the beginning of the time field.
+     * @return Raw pointer to time field.
+     */
+    virtual uint8_t* getPacketTimeRaw() const = 0;
+
+    virtual size_t getTimestampSize() const = 0;
+
+    /**
+     * This is a debugging helper method that prints the whole packet content
+     * to the screen.
+     */
+    void print();
     /**
      * With this method, the Error Control Field is updated to match the
      * current content of the packet. This method is not protected because
@@ -111,79 +108,24 @@ public:
      * like the sequence count.
      */
     void setErrorControl();
-
-	/**
-	 * This getter returns the Error Control Field of the packet.
-	 *
-	 * The field is placed after any possible Source Data. If no
-	 * Source Data is present there's still an Error Control field. It is
-	 * supposed to be a 16bit-CRC.
-	 * @return	The PUS Error Control
-	 */
-	uint16_t getErrorControl();
-
-	/**
-	 * This is a debugging helper method that prints the whole packet content
-	 * to the screen.
-	 */
-	void print();
-	/**
-	 * Interprets the "time"-field in the secondary header and returns it in
-	 * timeval format.
-	 * @return Converted timestamp of packet.
-	 */
-	ReturnValue_t getPacketTime(timeval* timestamp) const;
-	/**
-	 * Returns a raw pointer to the beginning of the time field.
-	 * @return Raw pointer to time field.
-	 */
-	uint8_t* getPacketTimeRaw() const;
-
-	size_t getTimestampSize() const;
+    /**
+     * This getter returns the Error Control Field of the packet.
+     *
+     * The field is placed after any possible Source Data. If no
+     * Source Data is present there's still an Error Control field. It is
+     * supposed to be a 16bit-CRC.
+     * @return  The PUS Error Control
+     */
+    uint16_t getErrorControl();
 
 protected:
-	/**
-	 * A pointer to a structure which defines the data structure of
-	 * the packet's data.
-	 *
-	 * To be hardware-safe, all elements are of byte size.
-	 */
-	TmPacketPointer* tmData;
-	/**
-	 * The timeStamper is responsible for adding a timestamp to the packet.
-	 * It is initialized lazy.
-	 */
-	static TimeStamperIF* timeStamper;
-	//! The ID to use when looking for a time stamper.
-	static object_id_t timeStamperId;
-
     /**
-     * Initializes the Tm Packet header.
-     * Does set the timestamp (to now), but not the error control field.
-     * @param apid APID used.
-     * @param service   PUS Service
-     * @param subservice PUS Subservice
-     * @param packetSubcounter Additional subcounter used.
+     * The timeStamper is responsible for adding a timestamp to the packet.
+     * It is initialized lazy.
      */
-    void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
-            uint8_t packetSubcounter);
-
-    /**
-     * With this method, the packet data pointer can be redirected to another
-     * location.
-     *
-     * This call overwrites the parent's setData method to set both its
-     * @c tc_data pointer and the parent's @c data pointer.
-     *
-     * @param p_data    A pointer to another PUS Telemetry Packet.
-     */
-    void setData( const uint8_t* pData );
-
-    /**
-     * In case data was filled manually (almost never the case).
-     * @param size Size of source data (without CRC and data filed header!).
-     */
-    void setSourceDataSize(uint16_t size);
+    static TimeStamperIF* timeStamper;
+    //! The ID to use when looking for a time stamper.
+    static object_id_t timeStamperId;
 
     /**
      * Checks if a time stamper is available and tries to set it if not.
diff --git a/tmtcpacket/pus/TmPacketIF.h b/tmtcpacket/pus/TmPacketIF.h
new file mode 100644
index 00000000..bd66523b
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketIF.h
@@ -0,0 +1,8 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETIF_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETIF_H_
+
+
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETIF_H_ */
diff --git a/tmtcpacket/pus/TmPacketPusA.cpp b/tmtcpacket/pus/TmPacketPusA.cpp
new file mode 100644
index 00000000..630aac0f
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketPusA.cpp
@@ -0,0 +1,121 @@
+#include "TmPacketPusA.h"
+#include "TmPacketBase.h"
+
+#include "../../globalfunctions/CRC.h"
+#include "../../globalfunctions/arrayprinter.h"
+#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../timemanager/CCSDSTime.h"
+
+#include <cstring>
+
+
+TmPacketPusA::TmPacketPusA(uint8_t* setData) : TmPacketBase(setData) {
+    tmData = reinterpret_cast<TmPacketPointer*>(setData);
+}
+
+TmPacketPusA::~TmPacketPusA() {
+    //Nothing to do.
+}
+
+uint8_t TmPacketPusA::getService() {
+    return tmData->data_field.service_type;
+}
+
+uint8_t TmPacketPusA::getSubService() {
+    return tmData->data_field.service_subtype;
+}
+
+uint8_t* TmPacketPusA::getSourceData() {
+    return &tmData->data;
+}
+
+uint16_t TmPacketPusA::getSourceDataSize() {
+    return getPacketDataLength() - sizeof(tmData->data_field)
+            - CRC_SIZE + 1;
+}
+
+//uint16_t TmPacketPusA::getErrorControl() {
+//    uint32_t size = getSourceDataSize() + CRC_SIZE;
+//    uint8_t* p_to_buffer = &tmData->data;
+//    return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
+//}
+//
+//void TmPacketPusA::setErrorControl() {
+//    uint32_t full_size = getFullSize();
+//    uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
+//    uint32_t size = getSourceDataSize();
+//    getSourceData()[size] = (crc & 0XFF00) >> 8;    // CRCH
+//    getSourceData()[size + 1] = (crc) & 0X00FF;         // CRCL
+//}
+
+void TmPacketPusA::setData(const uint8_t* p_Data) {
+    SpacePacketBase::setData(p_Data);
+    tmData = (TmPacketPointer*) p_Data;
+}
+
+
+size_t TmPacketPusA::getPacketMinimumSize() const {
+    return TM_PACKET_MIN_SIZE;
+}
+
+uint16_t TmPacketPusA::getDataFieldSize() {
+    return sizeof(PUSTmDataFieldHeader);
+}
+
+bool TmPacketPusA::checkAndSetStamper() {
+    if (timeStamper == NULL) {
+        timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
+        if (timeStamper == NULL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "TmPacketPusA::checkAndSetStamper: Stamper not found!"
+                    << std::endl;
+#endif
+            return false;
+        }
+    }
+    return true;
+}
+
+ReturnValue_t TmPacketPusA::getPacketTime(timeval* timestamp) const {
+    size_t tempSize = 0;
+    return CCSDSTime::convertFromCcsds(timestamp, tmData->data_field.time,
+            &tempSize, sizeof(tmData->data_field.time));
+}
+
+uint8_t* TmPacketPusA::getPacketTimeRaw() const{
+    return tmData->data_field.time;
+
+}
+
+void TmPacketPusA::initializeTmPacket(uint16_t apid, uint8_t service,
+        uint8_t subservice, uint8_t packetSubcounter) {
+    //Set primary header:
+    initSpacePacketHeader(false, true, apid);
+    //Set data Field Header:
+    //First, set to zero.
+    memset(&tmData->data_field, 0, sizeof(tmData->data_field));
+
+    // NOTE: In PUS-C, the PUS Version is 2 and specified for the first 4 bits.
+    // The other 4 bits of the first byte are the spacecraft time reference
+    // status. To change to PUS-C, set 0b00100000.
+    // Set CCSDS_secondary header flag to 0, version number to 001 and ack
+    // to 0000
+    tmData->data_field.version_type_ack = 0b00010000;
+    tmData->data_field.service_type = service;
+    tmData->data_field.service_subtype = subservice;
+    tmData->data_field.subcounter = packetSubcounter;
+    //Timestamp packet
+    if (checkAndSetStamper()) {
+        timeStamper->addTimeStamp(tmData->data_field.time,
+                sizeof(tmData->data_field.time));
+    }
+}
+
+void TmPacketPusA::setSourceDataSize(uint16_t size) {
+    setPacketDataLength(size + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1);
+}
+
+size_t TmPacketPusA::getTimestampSize() const {
+    return sizeof(tmData->data_field.time);
+}
diff --git a/tmtcpacket/pus/TmPacketPusA.h b/tmtcpacket/pus/TmPacketPusA.h
new file mode 100644
index 00000000..4db634c2
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketPusA.h
@@ -0,0 +1,137 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_
+
+#include "TmPacketBase.h"
+#include "../SpacePacketBase.h"
+#include "../../timemanager/TimeStamperIF.h"
+#include "../../timemanager/Clock.h"
+#include "../../objectmanager/SystemObjectIF.h"
+
+namespace Factory{
+void setStaticFrameworkObjectIds();
+}
+
+/**
+ * This struct defines a byte-wise structured PUS TM Data Field Header.
+ * Any optional fields in the header must be added or removed here.
+ * Currently, no Destination field is present, but an eigth-byte representation
+ * for a time tag.
+ * @ingroup tmtcpackets
+ */
+struct PUSTmDataFieldHeader {
+    uint8_t version_type_ack;
+    uint8_t service_type;
+    uint8_t service_subtype;
+    uint8_t subcounter;
+//  uint8_t destination;
+    uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
+};
+
+/**
+ * This struct defines the data structure of a PUS Telecommand Packet when
+ * accessed via a pointer.
+ * @ingroup tmtcpackets
+ */
+struct TmPacketPointer {
+    CCSDSPrimaryHeader primary;
+    PUSTmDataFieldHeader data_field;
+    uint8_t data;
+};
+
+/**
+ * PUS A packet implementation
+ * @ingroup tmtcpackets
+ */
+class TmPacketPusA: public TmPacketBase {
+    friend void (Factory::setStaticFrameworkObjectIds)();
+public:
+    /**
+     * This constant defines the minimum size of a valid PUS Telemetry Packet.
+     */
+    static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
+            sizeof(PUSTmDataFieldHeader) + 2);
+    //! Maximum size of a TM Packet in this mission.
+    //! TODO: Make this dependant on a config variable.
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
+    //! First byte of secondary header for PUS-A packets.
+    //! TODO: Maybe also support PUS-C via config?
+    static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 0b00010000;
+
+    /**
+     * This is the default constructor.
+     * It sets its internal data pointer to the address passed and also
+     * forwards the data pointer to the parent SpacePacketBase class.
+     * @param set_address   The position where the packet data lies.
+     */
+    TmPacketPusA( uint8_t* setData );
+    /**
+     * This is the empty default destructor.
+     */
+    virtual ~TmPacketPusA();
+
+    /* TmPacketBase implementations */
+    uint8_t getService() override;
+    uint8_t getSubService() override;
+    uint8_t* getSourceData() override;
+    uint16_t getSourceDataSize() override;
+    uint16_t getDataFieldSize() override;
+    /**
+     * Interprets the "time"-field in the secondary header and returns it in
+     * timeval format.
+     * @return Converted timestamp of packet.
+     */
+    ReturnValue_t getPacketTime(timeval* timestamp) const override;
+    /**
+     * Returns a raw pointer to the beginning of the time field.
+     * @return Raw pointer to time field.
+     */
+    uint8_t* getPacketTimeRaw() const override;
+
+    size_t getTimestampSize() const override;
+    size_t getPacketMinimumSize() const override;
+
+protected:
+    /**
+     * A pointer to a structure which defines the data structure of
+     * the packet's data.
+     *
+     * To be hardware-safe, all elements are of byte size.
+     */
+    TmPacketPointer* tmData;
+
+    /**
+     * Initializes the Tm Packet header.
+     * Does set the timestamp (to now), but not the error control field.
+     * @param apid APID used.
+     * @param service   PUS Service
+     * @param subservice PUS Subservice
+     * @param packetSubcounter Additional subcounter used.
+     */
+    void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
+            uint8_t packetSubcounter);
+
+    /**
+     * With this method, the packet data pointer can be redirected to another
+     * location.
+     *
+     * This call overwrites the parent's setData method to set both its
+     * @c tc_data pointer and the parent's @c data pointer.
+     *
+     * @param p_data    A pointer to another PUS Telemetry Packet.
+     */
+    void setData( const uint8_t* pData );
+
+    /**
+     * In case data was filled manually (almost never the case).
+     * @param size Size of source data (without CRC and data filed header!).
+     */
+    void setSourceDataSize(uint16_t size);
+
+    /**
+     * Checks if a time stamper is available and tries to set it if not.
+     * @return Returns false if setting failed.
+     */
+    bool checkAndSetStamper();
+};
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_ */
diff --git a/tmtcpacket/pus/TmPacketPusC.cpp b/tmtcpacket/pus/TmPacketPusC.cpp
new file mode 100644
index 00000000..99f50825
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketPusC.cpp
@@ -0,0 +1,2 @@
+#include "TmPacketPusC.h"
+
diff --git a/tmtcpacket/pus/TmPacketPusC.h b/tmtcpacket/pus/TmPacketPusC.h
new file mode 100644
index 00000000..fec435c4
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketPusC.h
@@ -0,0 +1,8 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
+
+
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_ */
diff --git a/tmtcpacket/pus/TmPacketStored.cpp b/tmtcpacket/pus/TmPacketStored.cpp
index 0fd2a4a0..d69e2419 100644
--- a/tmtcpacket/pus/TmPacketStored.cpp
+++ b/tmtcpacket/pus/TmPacketStored.cpp
@@ -10,21 +10,21 @@ StorageManagerIF *TmPacketStored::store = nullptr;
 InternalErrorReporterIF *TmPacketStored::internalErrorReporter = nullptr;
 
 TmPacketStored::TmPacketStored(store_address_t setAddress) :
-		TmPacketBase(nullptr), storeAddress(setAddress) {
+		TmPacketPusA(nullptr), storeAddress(setAddress) {
 	setStoreAddress(storeAddress);
 }
 
 TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
 		uint8_t subservice, uint8_t packetSubcounter, const uint8_t *data,
 		uint32_t size, const uint8_t *headerData, uint32_t headerSize) :
-		TmPacketBase(NULL) {
+		TmPacketPusA(nullptr) {
 	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
 	if (not checkAndSetStore()) {
 		return;
 	}
 	uint8_t *pData = nullptr;
 	ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
-			(TmPacketBase::TM_PACKET_MIN_SIZE + size + headerSize), &pData);
+			(getPacketMinimumSize() + size + headerSize), &pData);
 
 	if (returnValue != store->RETURN_OK) {
 		checkAndReportLostTm();
@@ -41,7 +41,7 @@ TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
 TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
 		uint8_t subservice, uint8_t packetSubcounter, SerializeIF *content,
 		SerializeIF *header) :
-		TmPacketBase(NULL) {
+		TmPacketPusA(nullptr) {
 	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
 	if (not checkAndSetStore()) {
 		return;
@@ -55,7 +55,7 @@ TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
 	}
 	uint8_t *p_data = NULL;
 	ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
-			(TmPacketBase::TM_PACKET_MIN_SIZE + sourceDataSize), &p_data);
+			(getPacketMinimumSize() + sourceDataSize), &p_data);
 	if (returnValue != store->RETURN_OK) {
 		checkAndReportLostTm();
 	}
diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/TmPacketStored.h
index b231407d..1cc8e6ba 100644
--- a/tmtcpacket/pus/TmPacketStored.h
+++ b/tmtcpacket/pus/TmPacketStored.h
@@ -2,7 +2,9 @@
 #define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
 
 #include "TmPacketBase.h"
+#include <FSFWConfig.h>
 
+#include "../../tmtcpacket/pus/TmPacketPusA.h"
 #include "../../serialize/SerializeIF.h"
 #include "../../storagemanager/StorageManagerIF.h"
 #include "../../internalError/InternalErrorReporterIF.h"
@@ -18,7 +20,7 @@
  *	packets in a store with the help of a storeAddress.
  *	@ingroup tmtcpackets
  */
-class TmPacketStored : public TmPacketBase {
+class TmPacketStored : public TmPacketPusA {
 public:
 	/**
 	 * This is a default constructor which does not set the data pointer.

From ae1dab1fce187d18d4a37cf416dcbc0cbfcfb491 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 12 Apr 2021 21:53:08 +0200
Subject: [PATCH 048/125] sth broke

---
 pus/Service17Test.cpp                        |  4 +-
 pus/Service1TelecommandVerification.cpp      |  4 +-
 pus/Service5EventReporting.cpp               |  2 +-
 tmtcpacket/pus/CMakeLists.txt                |  1 +
 tmtcpacket/pus/TmPacketBase.h                |  2 +-
 tmtcpacket/pus/TmPacketPusA.cpp              | 22 +----
 tmtcpacket/pus/TmPacketPusA.h                | 11 ++-
 tmtcpacket/pus/TmPacketStored.cpp            | 96 ++++----------------
 tmtcpacket/pus/TmPacketStored.h              | 50 +++-------
 tmtcpacket/pus/TmPacketStoredBase.cpp        | 94 +++++++++++++++++++
 tmtcpacket/pus/TmPacketStoredBase.h          | 89 ++++++++++++++++++
 tmtcservices/CommandingServiceBase.cpp       |  6 +-
 unittest/user/unittest/core/CatchFactory.cpp |  2 +-
 13 files changed, 233 insertions(+), 150 deletions(-)
 create mode 100644 tmtcpacket/pus/TmPacketStoredBase.cpp
 create mode 100644 tmtcpacket/pus/TmPacketStoredBase.h

diff --git a/pus/Service17Test.cpp b/pus/Service17Test.cpp
index 85a32e1e..28370171 100644
--- a/pus/Service17Test.cpp
+++ b/pus/Service17Test.cpp
@@ -17,14 +17,14 @@ Service17Test::~Service17Test() {
 ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
 	switch(subservice) {
 	case Subservice::CONNECTION_TEST: {
-		TmPacketStored connectionPacket(apid, serviceId,
+		TmPacketStoredPusA connectionPacket(apid, serviceId,
 		        Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
 		connectionPacket.sendPacket(requestQueue->getDefaultDestination(),
 				requestQueue->getId());
 		return HasReturnvaluesIF::RETURN_OK;
 	}
     case Subservice::EVENT_TRIGGER_TEST: {
-        TmPacketStored connectionPacket(apid, serviceId,
+        TmPacketStoredPusA connectionPacket(apid, serviceId,
                 Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
         connectionPacket.sendPacket(requestQueue->getDefaultDestination(),
                 requestQueue->getId());
diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index 7ce75478..5e9d0e42 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -68,7 +68,7 @@ ReturnValue_t Service1TelecommandVerification::generateFailureReport(
 			message->getTcSequenceControl(), message->getStep(),
 			message->getErrorCode(), message->getParameter1(),
 			message->getParameter2());
-	TmPacketStored tmPacket(apid, serviceId, message->getReportId(),
+	TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
 	        packetSubCounter++, &report);
 	ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
 			tmQueue->getId());
@@ -79,7 +79,7 @@ ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
         PusVerificationMessage *message) {
 	SuccessReport report(message->getReportId(),message->getTcPacketId(),
 			message->getTcSequenceControl(),message->getStep());
-	TmPacketStored tmPacket(apid, serviceId, message->getReportId(),
+	TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
 	        packetSubCounter++, &report);
 	ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
 			tmQueue->getId());
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index 965a27ad..8632f8d7 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -52,7 +52,7 @@ ReturnValue_t Service5EventReporting::generateEventReport(
 {
 	EventReport report(message.getEventId(),message.getReporter(),
 			message.getParameter1(),message.getParameter2());
-	TmPacketStored tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
+	TmPacketStoredPusA tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
 			message.getSeverity(), packetSubCounter++, &report);
 	ReturnValue_t result = tmPacket.sendPacket(
 	        requestQueue->getDefaultDestination(),requestQueue->getId());
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index 1fdd9dd8..a2ce20e2 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -7,4 +7,5 @@ target_sources(${LIB_FSFW_NAME}
 		TmPacketStored.cpp
 		TmPacketPusA.cpp
 		TmPacketPusC.cpp
+		TmPacketStoredBase.cpp
 )
diff --git a/tmtcpacket/pus/TmPacketBase.h b/tmtcpacket/pus/TmPacketBase.h
index 72cdbb69..09a6ca54 100644
--- a/tmtcpacket/pus/TmPacketBase.h
+++ b/tmtcpacket/pus/TmPacketBase.h
@@ -31,7 +31,7 @@ public:
     static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
     //! First byte of secondary header for PUS-A packets.
     //! TODO: Maybe also support PUS-C via config?
-    static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 0b00010000;
+    static const uint8_t VERSION_NUMBER_BYTE = 0b00010000;
 
     /**
      * This is the default constructor.
diff --git a/tmtcpacket/pus/TmPacketPusA.cpp b/tmtcpacket/pus/TmPacketPusA.cpp
index 630aac0f..841f7e40 100644
--- a/tmtcpacket/pus/TmPacketPusA.cpp
+++ b/tmtcpacket/pus/TmPacketPusA.cpp
@@ -11,7 +11,7 @@
 
 
 TmPacketPusA::TmPacketPusA(uint8_t* setData) : TmPacketBase(setData) {
-    tmData = reinterpret_cast<TmPacketPointer*>(setData);
+    tmData = reinterpret_cast<TmPacketPointerPusA*>(setData);
 }
 
 TmPacketPusA::~TmPacketPusA() {
@@ -35,23 +35,9 @@ uint16_t TmPacketPusA::getSourceDataSize() {
             - CRC_SIZE + 1;
 }
 
-//uint16_t TmPacketPusA::getErrorControl() {
-//    uint32_t size = getSourceDataSize() + CRC_SIZE;
-//    uint8_t* p_to_buffer = &tmData->data;
-//    return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
-//}
-//
-//void TmPacketPusA::setErrorControl() {
-//    uint32_t full_size = getFullSize();
-//    uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
-//    uint32_t size = getSourceDataSize();
-//    getSourceData()[size] = (crc & 0XFF00) >> 8;    // CRCH
-//    getSourceData()[size + 1] = (crc) & 0X00FF;         // CRCL
-//}
-
 void TmPacketPusA::setData(const uint8_t* p_Data) {
     SpacePacketBase::setData(p_Data);
-    tmData = (TmPacketPointer*) p_Data;
+    tmData = (TmPacketPointerPusA*) p_Data;
 }
 
 
@@ -60,7 +46,7 @@ size_t TmPacketPusA::getPacketMinimumSize() const {
 }
 
 uint16_t TmPacketPusA::getDataFieldSize() {
-    return sizeof(PUSTmDataFieldHeader);
+    return sizeof(PUSTmDataFieldHeaderPusA);
 }
 
 bool TmPacketPusA::checkAndSetStamper() {
@@ -113,7 +99,7 @@ void TmPacketPusA::initializeTmPacket(uint16_t apid, uint8_t service,
 }
 
 void TmPacketPusA::setSourceDataSize(uint16_t size) {
-    setPacketDataLength(size + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1);
+    setPacketDataLength(size + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1);
 }
 
 size_t TmPacketPusA::getTimestampSize() const {
diff --git a/tmtcpacket/pus/TmPacketPusA.h b/tmtcpacket/pus/TmPacketPusA.h
index 4db634c2..21bdfd95 100644
--- a/tmtcpacket/pus/TmPacketPusA.h
+++ b/tmtcpacket/pus/TmPacketPusA.h
@@ -18,7 +18,7 @@ void setStaticFrameworkObjectIds();
  * for a time tag.
  * @ingroup tmtcpackets
  */
-struct PUSTmDataFieldHeader {
+struct PUSTmDataFieldHeaderPusA {
     uint8_t version_type_ack;
     uint8_t service_type;
     uint8_t service_subtype;
@@ -32,9 +32,9 @@ struct PUSTmDataFieldHeader {
  * accessed via a pointer.
  * @ingroup tmtcpackets
  */
-struct TmPacketPointer {
+struct TmPacketPointerPusA {
     CCSDSPrimaryHeader primary;
-    PUSTmDataFieldHeader data_field;
+    PUSTmDataFieldHeaderPusA data_field;
     uint8_t data;
 };
 
@@ -49,7 +49,7 @@ public:
      * This constant defines the minimum size of a valid PUS Telemetry Packet.
      */
     static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
-            sizeof(PUSTmDataFieldHeader) + 2);
+            sizeof(PUSTmDataFieldHeaderPusA) + 2);
     //! Maximum size of a TM Packet in this mission.
     //! TODO: Make this dependant on a config variable.
     static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
@@ -75,6 +75,7 @@ public:
     uint8_t* getSourceData() override;
     uint16_t getSourceDataSize() override;
     uint16_t getDataFieldSize() override;
+
     /**
      * Interprets the "time"-field in the secondary header and returns it in
      * timeval format.
@@ -97,7 +98,7 @@ protected:
      *
      * To be hardware-safe, all elements are of byte size.
      */
-    TmPacketPointer* tmData;
+    TmPacketPointerPusA* tmData;
 
     /**
      * Initializes the Tm Packet header.
diff --git a/tmtcpacket/pus/TmPacketStored.cpp b/tmtcpacket/pus/TmPacketStored.cpp
index d69e2419..4a33c504 100644
--- a/tmtcpacket/pus/TmPacketStored.cpp
+++ b/tmtcpacket/pus/TmPacketStored.cpp
@@ -6,20 +6,19 @@
 
 #include <cstring>
 
-StorageManagerIF *TmPacketStored::store = nullptr;
-InternalErrorReporterIF *TmPacketStored::internalErrorReporter = nullptr;
+StorageManagerIF *TmPacketStoredPusA::store = nullptr;
+InternalErrorReporterIF *TmPacketStoredPusA::internalErrorReporter = nullptr;
 
-TmPacketStored::TmPacketStored(store_address_t setAddress) :
-		TmPacketPusA(nullptr), storeAddress(setAddress) {
-	setStoreAddress(storeAddress);
+TmPacketStoredPusA::TmPacketStoredPusA(store_address_t setAddress) :
+		TmPacketStoredBase(setAddress), TmPacketPusA(nullptr){
 }
 
-TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
+TmPacketStoredPusA::TmPacketStoredPusA(uint16_t apid, uint8_t service,
 		uint8_t subservice, uint8_t packetSubcounter, const uint8_t *data,
 		uint32_t size, const uint8_t *headerData, uint32_t headerSize) :
 		TmPacketPusA(nullptr) {
 	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	if (not checkAndSetStore()) {
+	if (not TmPacketStoredBase::checkAndSetStore()) {
 		return;
 	}
 	uint8_t *pData = nullptr;
@@ -27,7 +26,7 @@ TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
 			(getPacketMinimumSize() + size + headerSize), &pData);
 
 	if (returnValue != store->RETURN_OK) {
-		checkAndReportLostTm();
+	    TmPacketStoredBase::checkAndReportLostTm();
 		return;
 	}
 	setData(pData);
@@ -35,15 +34,15 @@ TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
 	memcpy(getSourceData(), headerData, headerSize);
 	memcpy(getSourceData() + headerSize, data, size);
 	setPacketDataLength(
-			size + headerSize + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1);
+			size + headerSize + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1);
 }
 
-TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
+TmPacketStoredPusA::TmPacketStoredPusA(uint16_t apid, uint8_t service,
 		uint8_t subservice, uint8_t packetSubcounter, SerializeIF *content,
 		SerializeIF *header) :
 		TmPacketPusA(nullptr) {
 	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	if (not checkAndSetStore()) {
+	if (not  TmPacketStoredBase::checkAndSetStore()) {
 		return;
 	}
 	size_t sourceDataSize = 0;
@@ -57,7 +56,7 @@ TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
 	ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
 			(getPacketMinimumSize() + sourceDataSize), &p_data);
 	if (returnValue != store->RETURN_OK) {
-		checkAndReportLostTm();
+	    TmPacketStoredBase::checkAndReportLostTm();
 	}
 	setData(p_data);
 	initializeTmPacket(apid, service, subservice, packetSubcounter);
@@ -72,76 +71,13 @@ TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
 				SerializeIF::Endianness::BIG);
 	}
 	setPacketDataLength(
-			sourceDataSize + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1);
+			sourceDataSize + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1);
 }
 
-store_address_t TmPacketStored::getStoreAddress() {
-	return storeAddress;
+uint8_t* TmPacketStoredPusA::getAllTmData() {
+    return getWholeData();
 }
 
-void TmPacketStored::deletePacket() {
-	store->deleteData(storeAddress);
-	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	setData(nullptr);
-}
-
-void TmPacketStored::setStoreAddress(store_address_t setAddress) {
-	storeAddress = setAddress;
-	const uint8_t* tempData = nullptr;
-	size_t tempSize;
-	if (not checkAndSetStore()) {
-		return;
-	}
-	ReturnValue_t status = store->getData(storeAddress, &tempData, &tempSize);
-	if (status == StorageManagerIF::RETURN_OK) {
-		setData(tempData);
-	} else {
-		setData(nullptr);
-		storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	}
-}
-
-bool TmPacketStored::checkAndSetStore() {
-	if (store == nullptr) {
-		store = objectManager->get<StorageManagerIF>(objects::TM_STORE);
-		if (store == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TmPacketStored::TmPacketStored: TM Store not found!"
-					<< std::endl;
-#endif
-			return false;
-		}
-	}
-	return true;
-}
-
-ReturnValue_t TmPacketStored::sendPacket(MessageQueueId_t destination,
-		MessageQueueId_t sentFrom, bool doErrorReporting) {
-	if (getWholeData() == nullptr) {
-		//SHOULDDO: More decent code.
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	TmTcMessage tmMessage(getStoreAddress());
-	ReturnValue_t result = MessageQueueSenderIF::sendMessage(destination,
-			&tmMessage, sentFrom);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		deletePacket();
-		if (doErrorReporting) {
-			checkAndReportLostTm();
-		}
-		return result;
-	}
-	//SHOULDDO: In many cases, some counter is incremented for successfully sent packets. The check is often not done, but just incremented.
-	return HasReturnvaluesIF::RETURN_OK;
-
-}
-
-void TmPacketStored::checkAndReportLostTm() {
-	if (internalErrorReporter == nullptr) {
-		internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
-				objects::INTERNAL_ERROR_REPORTER);
-	}
-	if (internalErrorReporter != nullptr) {
-		internalErrorReporter->lostTm();
-	}
+void TmPacketStoredPusA::setDataPointer(const uint8_t *newPointer) {
+    setData(newPointer);
 }
diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/TmPacketStored.h
index 1cc8e6ba..17b81e9f 100644
--- a/tmtcpacket/pus/TmPacketStored.h
+++ b/tmtcpacket/pus/TmPacketStored.h
@@ -1,7 +1,8 @@
-#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
-#define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
 
 #include "TmPacketBase.h"
+#include "TmPacketStoredBase.h"
 #include <FSFWConfig.h>
 
 #include "../../tmtcpacket/pus/TmPacketPusA.h"
@@ -20,13 +21,15 @@
  *	packets in a store with the help of a storeAddress.
  *	@ingroup tmtcpackets
  */
-class TmPacketStored : public TmPacketPusA {
+class TmPacketStoredPusA :
+        public TmPacketStoredBase,
+        public TmPacketPusA {
 public:
 	/**
 	 * This is a default constructor which does not set the data pointer.
 	 * However, it does try to set the packet store.
 	 */
-	TmPacketStored( store_address_t setAddress );
+	TmPacketStoredPusA( store_address_t setAddress );
 	/**
 	 * With this constructor, new space is allocated in the packet store and
 	 * a new PUS Telemetry Packet is created there.
@@ -47,7 +50,7 @@ public:
 	 * 						 will be copied in front of data
 	 * @param headerSize	The size of the headerDataF
 	 */
-	TmPacketStored( uint16_t apid, uint8_t service,	uint8_t subservice,
+	TmPacketStoredPusA( uint16_t apid, uint8_t service,	uint8_t subservice,
 			uint8_t packet_counter = 0, const uint8_t* data = nullptr,
 			uint32_t size = 0, const uint8_t* headerData = nullptr,
 			uint32_t headerSize = 0);
@@ -55,30 +58,13 @@ public:
 	 * Another ctor to directly pass structured content and header data to the
 	 * packet to avoid additional buffers.
 	 */
-	TmPacketStored( uint16_t apid, uint8_t service,	uint8_t subservice,
+	TmPacketStoredPusA( uint16_t apid, uint8_t service,	uint8_t subservice,
 			uint8_t packet_counter, SerializeIF* content,
 			SerializeIF* header = nullptr);
-	/**
-	 * This is a getter for the current store address of the packet.
-	 * @return	The current store address. The (raw) value is
-	 * 			@c StorageManagerIF::INVALID_ADDRESS if
-	 * 			the packet is not linked.
-	 */
-	store_address_t getStoreAddress();
-	/**
-	 * With this call, the packet is deleted.
-	 * It removes itself from the store and sets its data pointer to NULL.
-	 */
-	void deletePacket();
-	/**
-	 * With this call, a packet can be linked to another store. This is useful
-	 * if the packet is a class member and used for more than one packet.
-	 * @param setAddress	The new packet id to link to.
-	 */
-	void setStoreAddress( store_address_t setAddress );
 
-	ReturnValue_t sendPacket( MessageQueueId_t destination,
-			MessageQueueId_t sentFrom, bool doErrorReporting = true );
+    uint8_t* getAllTmData() override;
+    void setDataPointer(const uint8_t* newPointer) override;
+
 private:
 	/**
 	 * This is a pointer to the store all instances of the class use.
@@ -94,17 +80,7 @@ private:
 	 * The address where the packet data of the object instance is stored.
 	 */
 	store_address_t storeAddress;
-	/**
-	 * A helper method to check if a store is assigned to the class.
-	 * If not, the method tries to retrieve the store from the global
-	 * ObjectManager.
-	 * @return	@li	@c true if the store is linked or could be created.
-	 * 			@li @c false otherwise.
-	 */
-	bool checkAndSetStore();
-
-	void checkAndReportLostTm();
 };
 
 
-#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_ */
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_ */
diff --git a/tmtcpacket/pus/TmPacketStoredBase.cpp b/tmtcpacket/pus/TmPacketStoredBase.cpp
new file mode 100644
index 00000000..3ab31a80
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketStoredBase.cpp
@@ -0,0 +1,94 @@
+#include "TmPacketStoredBase.h"
+
+#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../tmtcservices/TmTcMessage.h"
+
+#include <cstring>
+
+StorageManagerIF *TmPacketStoredBase::store = nullptr;
+InternalErrorReporterIF *TmPacketStoredBase::internalErrorReporter = nullptr;
+
+TmPacketStoredBase::TmPacketStoredBase(store_address_t setAddress): storeAddress(setAddress) {
+    setStoreAddress(storeAddress);
+}
+
+TmPacketStoredBase::TmPacketStoredBase() {
+}
+
+
+TmPacketStoredBase::~TmPacketStoredBase() {
+}
+
+store_address_t TmPacketStoredBase::getStoreAddress() {
+    return storeAddress;
+}
+
+void TmPacketStoredBase::deletePacket() {
+    store->deleteData(storeAddress);
+    storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    setDataPointer(nullptr);
+}
+
+void TmPacketStoredBase::setStoreAddress(store_address_t setAddress) {
+    storeAddress = setAddress;
+    const uint8_t* tempData = nullptr;
+    size_t tempSize;
+    if (not checkAndSetStore()) {
+        return;
+    }
+    ReturnValue_t status = store->getData(storeAddress, &tempData, &tempSize);
+    if (status == StorageManagerIF::RETURN_OK) {
+        setDataPointer(tempData);
+    } else {
+        setDataPointer(nullptr);
+        storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    }
+}
+
+bool TmPacketStoredBase::checkAndSetStore() {
+    if (store == nullptr) {
+        store = objectManager->get<StorageManagerIF>(objects::TM_STORE);
+        if (store == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "TmPacketStored::TmPacketStored: TM Store not found!"
+                    << std::endl;
+#endif
+            return false;
+        }
+    }
+    return true;
+}
+
+ReturnValue_t TmPacketStoredBase::sendPacket(MessageQueueId_t destination,
+        MessageQueueId_t sentFrom, bool doErrorReporting) {
+    if (getAllTmData() == nullptr) {
+        //SHOULDDO: More decent code.
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    TmTcMessage tmMessage(getStoreAddress());
+    ReturnValue_t result = MessageQueueSenderIF::sendMessage(destination,
+            &tmMessage, sentFrom);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        deletePacket();
+        if (doErrorReporting) {
+            checkAndReportLostTm();
+        }
+        return result;
+    }
+    //SHOULDDO: In many cases, some counter is incremented for successfully sent packets. The check is often not done, but just incremented.
+    return HasReturnvaluesIF::RETURN_OK;
+
+}
+
+void TmPacketStoredBase::checkAndReportLostTm() {
+    if (internalErrorReporter == nullptr) {
+        internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
+                objects::INTERNAL_ERROR_REPORTER);
+    }
+    if (internalErrorReporter != nullptr) {
+        internalErrorReporter->lostTm();
+    }
+}
+
+
diff --git a/tmtcpacket/pus/TmPacketStoredBase.h b/tmtcpacket/pus/TmPacketStoredBase.h
new file mode 100644
index 00000000..58a231a5
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketStoredBase.h
@@ -0,0 +1,89 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
+
+#include "TmPacketBase.h"
+#include "TmPacketStoredBase.h"
+#include <FSFWConfig.h>
+
+#include "../../tmtcpacket/pus/TmPacketPusA.h"
+#include "../../serialize/SerializeIF.h"
+#include "../../storagemanager/StorageManagerIF.h"
+#include "../../internalError/InternalErrorReporterIF.h"
+#include "../../ipc/MessageQueueSenderIF.h"
+
+/**
+ *  This class generates a ECSS PUS Telemetry packet within a given
+ *  intermediate storage.
+ *  As most packets are passed between tasks with the help of a storage
+ *  anyway, it seems logical to create a Packet-In-Storage access class
+ *  which saves the user almost all storage handling operation.
+ *  Packets can both be newly created with the class and be "linked" to
+ *  packets in a store with the help of a storeAddress.
+ *  @ingroup tmtcpackets
+ */
+class TmPacketStoredBase {
+public:
+    /**
+     * This is a default constructor which does not set the data pointer.
+     * However, it does try to set the packet store.
+     */
+    TmPacketStoredBase( store_address_t setAddress );
+    TmPacketStoredBase();
+
+    virtual ~TmPacketStoredBase();
+
+    virtual uint8_t* getAllTmData() = 0;
+    virtual void setDataPointer(const uint8_t* newPointer) = 0;
+
+    /**
+     * This is a getter for the current store address of the packet.
+     * @return  The current store address. The (raw) value is
+     *          @c StorageManagerIF::INVALID_ADDRESS if
+     *          the packet is not linked.
+     */
+    store_address_t getStoreAddress();
+    /**
+     * With this call, the packet is deleted.
+     * It removes itself from the store and sets its data pointer to NULL.
+     */
+    void deletePacket();
+    /**
+     * With this call, a packet can be linked to another store. This is useful
+     * if the packet is a class member and used for more than one packet.
+     * @param setAddress    The new packet id to link to.
+     */
+    void setStoreAddress( store_address_t setAddress );
+
+    ReturnValue_t sendPacket( MessageQueueId_t destination,
+            MessageQueueId_t sentFrom, bool doErrorReporting = true );
+
+protected:
+    /**
+     * This is a pointer to the store all instances of the class use.
+     * If the store is not yet set (i.e. @c store is NULL), every constructor
+     * call tries to set it and throws an error message in case of failures.
+     * The default store is objects::TM_STORE.
+     */
+    static StorageManagerIF* store;
+
+    static InternalErrorReporterIF *internalErrorReporter;
+
+    /**
+     * The address where the packet data of the object instance is stored.
+     */
+    store_address_t storeAddress;
+    /**
+     * A helper method to check if a store is assigned to the class.
+     * If not, the method tries to retrieve the store from the global
+     * ObjectManager.
+     * @return  @li @c true if the store is linked or could be created.
+     *          @li @c false otherwise.
+     */
+    bool checkAndSetStore();
+
+    void checkAndReportLostTm();
+};
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_ */
+
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index 8b6f7a09..bbcdcc80 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -293,7 +293,7 @@ void CommandingServiceBase::handleRequestQueue() {
 ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
 		const uint8_t* data, size_t dataLen, const uint8_t* headerData,
 		size_t headerSize) {
-	TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+	TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
 			this->tmPacketCounter, data, dataLen, headerData, headerSize);
 	ReturnValue_t result = tmPacketStored.sendPacket(
 			requestQueue->getDefaultDestination(), requestQueue->getId());
@@ -311,7 +311,7 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
     size_t size = 0;
     SerializeAdapter::serialize(&objectId, &pBuffer, &size,
                 sizeof(object_id_t), SerializeIF::Endianness::BIG);
-    TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+    TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
             this->tmPacketCounter, data, dataLen, buffer, size);
     ReturnValue_t result = tmPacketStored.sendPacket(
             requestQueue->getDefaultDestination(), requestQueue->getId());
@@ -324,7 +324,7 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
 
 ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
         SerializeIF* content, SerializeIF* header) {
-    TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+    TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
             this->tmPacketCounter, content, header);
     ReturnValue_t result = tmPacketStored.sendPacket(
             requestQueue->getDefaultDestination(), requestQueue->getId());
diff --git a/unittest/user/unittest/core/CatchFactory.cpp b/unittest/user/unittest/core/CatchFactory.cpp
index eabaa21d..2c4eaf24 100644
--- a/unittest/user/unittest/core/CatchFactory.cpp
+++ b/unittest/user/unittest/core/CatchFactory.cpp
@@ -74,7 +74,7 @@ void Factory::setStaticFrameworkObjectIds() {
 
 	DeviceHandlerFailureIsolation::powerConfirmationId = objects::NO_OBJECT;
 
-	TmPacketStored::timeStamperId = objects::NO_OBJECT;
+	TmPacketStoredPusA::timeStamperId = objects::NO_OBJECT;
 }
 
 

From 4faa5b0685fc894b643979d54bdf5d8736257e75 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 12 Apr 2021 22:02:16 +0200
Subject: [PATCH 049/125] fixes

---
 tmtcpacket/pus/TmPacketStored.cpp | 5 +----
 tmtcpacket/pus/TmPacketStored.h   | 9 ---------
 2 files changed, 1 insertion(+), 13 deletions(-)

diff --git a/tmtcpacket/pus/TmPacketStored.cpp b/tmtcpacket/pus/TmPacketStored.cpp
index 4a33c504..b09f505d 100644
--- a/tmtcpacket/pus/TmPacketStored.cpp
+++ b/tmtcpacket/pus/TmPacketStored.cpp
@@ -6,9 +6,6 @@
 
 #include <cstring>
 
-StorageManagerIF *TmPacketStoredPusA::store = nullptr;
-InternalErrorReporterIF *TmPacketStoredPusA::internalErrorReporter = nullptr;
-
 TmPacketStoredPusA::TmPacketStoredPusA(store_address_t setAddress) :
 		TmPacketStoredBase(setAddress), TmPacketPusA(nullptr){
 }
@@ -42,7 +39,7 @@ TmPacketStoredPusA::TmPacketStoredPusA(uint16_t apid, uint8_t service,
 		SerializeIF *header) :
 		TmPacketPusA(nullptr) {
 	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	if (not  TmPacketStoredBase::checkAndSetStore()) {
+	if (not TmPacketStoredBase::checkAndSetStore()) {
 		return;
 	}
 	size_t sourceDataSize = 0;
diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/TmPacketStored.h
index 17b81e9f..f1722f4e 100644
--- a/tmtcpacket/pus/TmPacketStored.h
+++ b/tmtcpacket/pus/TmPacketStored.h
@@ -66,15 +66,6 @@ public:
     void setDataPointer(const uint8_t* newPointer) override;
 
 private:
-	/**
-	 * This is a pointer to the store all instances of the class use.
-	 * If the store is not yet set (i.e. @c store is NULL), every constructor
-	 * call tries to set it and throws an error message in case of failures.
-	 * The default store is objects::TM_STORE.
-	 */
-	static StorageManagerIF* store;
-
-	static InternalErrorReporterIF *internalErrorReporter;
 
 	/**
 	 * The address where the packet data of the object instance is stored.

From 07f121631609209a828b6f1df5bdc602bec72b68 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 12 Apr 2021 22:12:18 +0200
Subject: [PATCH 050/125] error found

---
 tmtcpacket/pus/TmPacketStored.h     | 6 ------
 tmtcpacket/pus/TmPacketStoredBase.h | 6 +++---
 2 files changed, 3 insertions(+), 9 deletions(-)

diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/TmPacketStored.h
index f1722f4e..aeeba275 100644
--- a/tmtcpacket/pus/TmPacketStored.h
+++ b/tmtcpacket/pus/TmPacketStored.h
@@ -65,12 +65,6 @@ public:
     uint8_t* getAllTmData() override;
     void setDataPointer(const uint8_t* newPointer) override;
 
-private:
-
-	/**
-	 * The address where the packet data of the object instance is stored.
-	 */
-	store_address_t storeAddress;
 };
 
 
diff --git a/tmtcpacket/pus/TmPacketStoredBase.h b/tmtcpacket/pus/TmPacketStoredBase.h
index 58a231a5..dd7e31eb 100644
--- a/tmtcpacket/pus/TmPacketStoredBase.h
+++ b/tmtcpacket/pus/TmPacketStoredBase.h
@@ -52,10 +52,10 @@ public:
      * if the packet is a class member and used for more than one packet.
      * @param setAddress    The new packet id to link to.
      */
-    void setStoreAddress( store_address_t setAddress );
+    void setStoreAddress(store_address_t setAddress);
 
-    ReturnValue_t sendPacket( MessageQueueId_t destination,
-            MessageQueueId_t sentFrom, bool doErrorReporting = true );
+    ReturnValue_t sendPacket(MessageQueueId_t destination, MessageQueueId_t sentFrom,
+            bool doErrorReporting = true);
 
 protected:
     /**

From 9a2fbefc9ff5c9fbdd64fa4dbe7eaef293704b05 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 12 Apr 2021 22:24:11 +0200
Subject: [PATCH 051/125] refactoring continued

---
 pus/Service17Test.cpp                         |  2 +-
 pus/Service1TelecommandVerification.cpp       |  2 +-
 pus/Service5EventReporting.cpp                |  2 +-
 tmtcpacket/pus/CMakeLists.txt                 |  2 +-
 tmtcpacket/pus/TmPacketBase.cpp               |  8 +-
 tmtcpacket/pus/TmPacketStored.h               | 73 ++-----------------
 ...acketStored.cpp => TmPacketStoredPusA.cpp} |  2 +-
 tmtcpacket/pus/TmPacketStoredPusA.h           | 71 ++++++++++++++++++
 tmtcservices/CommandingServiceBase.cpp        |  2 +-
 9 files changed, 89 insertions(+), 75 deletions(-)
 rename tmtcpacket/pus/{TmPacketStored.cpp => TmPacketStoredPusA.cpp} (98%)
 create mode 100644 tmtcpacket/pus/TmPacketStoredPusA.h

diff --git a/pus/Service17Test.cpp b/pus/Service17Test.cpp
index 28370171..cdd8817c 100644
--- a/pus/Service17Test.cpp
+++ b/pus/Service17Test.cpp
@@ -2,7 +2,7 @@
 
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../objectmanager/SystemObject.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/TmPacketStoredPusA.h"
 
 
 Service17Test::Service17Test(object_id_t objectId,
diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index 5e9d0e42..358d1faa 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -3,7 +3,7 @@
 
 #include "../ipc/QueueFactory.h"
 #include "../tmtcservices/PusVerificationReport.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/TmPacketStoredPusA.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../tmtcservices/AcceptsTelemetryIF.h"
 
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index 8632f8d7..f6281fe9 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -4,7 +4,7 @@
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../events/EventManagerIF.h"
 #include "../ipc/QueueFactory.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/TmPacketStoredPusA.h"
 
 
 Service5EventReporting::Service5EventReporting(object_id_t objectId,
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index a2ce20e2..747ed070 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -4,7 +4,7 @@ target_sources(${LIB_FSFW_NAME}
 		TcPacketStored.cpp
 		TmPacketBase.cpp
 		TmPacketMinimal.cpp
-		TmPacketStored.cpp
+		TmPacketStoredPusA.cpp
 		TmPacketPusA.cpp
 		TmPacketPusC.cpp
 		TmPacketStoredBase.cpp
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp
index 10f37b00..2dfef258 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/TmPacketBase.cpp
@@ -3,13 +3,13 @@
 #include "../../globalfunctions/CRC.h"
 #include "../../globalfunctions/arrayprinter.h"
 #include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../timemanager/CCSDSTime.h"
 
 #include <cstring>
 
 TimeStamperIF* TmPacketBase::timeStamper = nullptr;
-object_id_t TmPacketBase::timeStamperId = 0;
+object_id_t TmPacketBase::timeStamperId = objects::NO_OBJECT;
 
 TmPacketBase::TmPacketBase(uint8_t* setData):
         SpacePacketBase(setData) {
@@ -52,8 +52,10 @@ bool TmPacketBase::checkAndSetStamper() {
         timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
         if (timeStamper == NULL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "TmPacketBase::checkAndSetStamper: Stamper not found!"
+            sif::Warning << "TmPacketBase::checkAndSetStamper: Stamper not found!"
                     << std::endl;
+#else
+            sif::printWarning("TmPacketBase::checkAndSetStamper: Stamper not found!\n");
 #endif
             return false;
         }
diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/TmPacketStored.h
index aeeba275..53ef3f4d 100644
--- a/tmtcpacket/pus/TmPacketStored.h
+++ b/tmtcpacket/pus/TmPacketStored.h
@@ -1,71 +1,12 @@
-#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
-#define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
 
-#include "TmPacketBase.h"
-#include "TmPacketStoredBase.h"
 #include <FSFWConfig.h>
 
-#include "../../tmtcpacket/pus/TmPacketPusA.h"
-#include "../../serialize/SerializeIF.h"
-#include "../../storagemanager/StorageManagerIF.h"
-#include "../../internalError/InternalErrorReporterIF.h"
-#include "../../ipc/MessageQueueSenderIF.h"
-
-/**
- *	This class generates a ECSS PUS Telemetry packet within a given
- *	intermediate storage.
- *	As most packets are passed between tasks with the help of a storage
- *	anyway, it seems logical to create a Packet-In-Storage access class
- *	which saves the user almost all storage handling operation.
- *	Packets can both be newly created with the class and be "linked" to
- *	packets in a store with the help of a storeAddress.
- *	@ingroup tmtcpackets
- */
-class TmPacketStoredPusA :
-        public TmPacketStoredBase,
-        public TmPacketPusA {
-public:
-	/**
-	 * This is a default constructor which does not set the data pointer.
-	 * However, it does try to set the packet store.
-	 */
-	TmPacketStoredPusA( store_address_t setAddress );
-	/**
-	 * With this constructor, new space is allocated in the packet store and
-	 * a new PUS Telemetry Packet is created there.
-	 * Packet Application Data passed in data is copied into the packet.
-	 * The Application data is passed in two parts, first a header, then a
-	 * data field. This allows building a Telemetry Packet from two separate
-	 * data sources.
-	 * @param apid			Sets the packet's APID field.
-	 * @param service		Sets the packet's Service ID field.
-	 * 		This specifies the source service.
-	 * @param subservice	Sets the packet's Service Subtype field.
-	 * 		This specifies the source sub-service.
-	 * @param packet_counter	Sets the Packet counter field of this packet
-	 * @param data			The payload data to be copied to the
-	 * 						Application Data Field
-	 * @param size			The amount of data to be copied.
-	 * @param headerData 	The header Data of the Application field,
-	 * 						 will be copied in front of data
-	 * @param headerSize	The size of the headerDataF
-	 */
-	TmPacketStoredPusA( uint16_t apid, uint8_t service,	uint8_t subservice,
-			uint8_t packet_counter = 0, const uint8_t* data = nullptr,
-			uint32_t size = 0, const uint8_t* headerData = nullptr,
-			uint32_t headerSize = 0);
-	/**
-	 * Another ctor to directly pass structured content and header data to the
-	 * packet to avoid additional buffers.
-	 */
-	TmPacketStoredPusA( uint16_t apid, uint8_t service,	uint8_t subservice,
-			uint8_t packet_counter, SerializeIF* content,
-			SerializeIF* header = nullptr);
-
-    uint8_t* getAllTmData() override;
-    void setDataPointer(const uint8_t* newPointer) override;
-
-};
+#if FSFW_USE_PUS_C_TELEMETRY == 1
+#else
+#include "TmPacketStoredPusA.h"
+#endif
 
 
-#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_ */
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_ */
diff --git a/tmtcpacket/pus/TmPacketStored.cpp b/tmtcpacket/pus/TmPacketStoredPusA.cpp
similarity index 98%
rename from tmtcpacket/pus/TmPacketStored.cpp
rename to tmtcpacket/pus/TmPacketStoredPusA.cpp
index b09f505d..4931f8d9 100644
--- a/tmtcpacket/pus/TmPacketStored.cpp
+++ b/tmtcpacket/pus/TmPacketStoredPusA.cpp
@@ -1,4 +1,4 @@
-#include "TmPacketStored.h"
+#include "TmPacketStoredPusA.h"
 
 #include "../../objectmanager/ObjectManagerIF.h"
 #include "../../serviceinterface/ServiceInterfaceStream.h"
diff --git a/tmtcpacket/pus/TmPacketStoredPusA.h b/tmtcpacket/pus/TmPacketStoredPusA.h
new file mode 100644
index 00000000..aeeba275
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketStoredPusA.h
@@ -0,0 +1,71 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
+
+#include "TmPacketBase.h"
+#include "TmPacketStoredBase.h"
+#include <FSFWConfig.h>
+
+#include "../../tmtcpacket/pus/TmPacketPusA.h"
+#include "../../serialize/SerializeIF.h"
+#include "../../storagemanager/StorageManagerIF.h"
+#include "../../internalError/InternalErrorReporterIF.h"
+#include "../../ipc/MessageQueueSenderIF.h"
+
+/**
+ *	This class generates a ECSS PUS Telemetry packet within a given
+ *	intermediate storage.
+ *	As most packets are passed between tasks with the help of a storage
+ *	anyway, it seems logical to create a Packet-In-Storage access class
+ *	which saves the user almost all storage handling operation.
+ *	Packets can both be newly created with the class and be "linked" to
+ *	packets in a store with the help of a storeAddress.
+ *	@ingroup tmtcpackets
+ */
+class TmPacketStoredPusA :
+        public TmPacketStoredBase,
+        public TmPacketPusA {
+public:
+	/**
+	 * This is a default constructor which does not set the data pointer.
+	 * However, it does try to set the packet store.
+	 */
+	TmPacketStoredPusA( store_address_t setAddress );
+	/**
+	 * With this constructor, new space is allocated in the packet store and
+	 * a new PUS Telemetry Packet is created there.
+	 * Packet Application Data passed in data is copied into the packet.
+	 * The Application data is passed in two parts, first a header, then a
+	 * data field. This allows building a Telemetry Packet from two separate
+	 * data sources.
+	 * @param apid			Sets the packet's APID field.
+	 * @param service		Sets the packet's Service ID field.
+	 * 		This specifies the source service.
+	 * @param subservice	Sets the packet's Service Subtype field.
+	 * 		This specifies the source sub-service.
+	 * @param packet_counter	Sets the Packet counter field of this packet
+	 * @param data			The payload data to be copied to the
+	 * 						Application Data Field
+	 * @param size			The amount of data to be copied.
+	 * @param headerData 	The header Data of the Application field,
+	 * 						 will be copied in front of data
+	 * @param headerSize	The size of the headerDataF
+	 */
+	TmPacketStoredPusA( uint16_t apid, uint8_t service,	uint8_t subservice,
+			uint8_t packet_counter = 0, const uint8_t* data = nullptr,
+			uint32_t size = 0, const uint8_t* headerData = nullptr,
+			uint32_t headerSize = 0);
+	/**
+	 * Another ctor to directly pass structured content and header data to the
+	 * packet to avoid additional buffers.
+	 */
+	TmPacketStoredPusA( uint16_t apid, uint8_t service,	uint8_t subservice,
+			uint8_t packet_counter, SerializeIF* content,
+			SerializeIF* header = nullptr);
+
+    uint8_t* getAllTmData() override;
+    void setDataPointer(const uint8_t* newPointer) override;
+
+};
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_ */
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index bbcdcc80..147c8796 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -6,7 +6,7 @@
 #include "../objectmanager/ObjectManagerIF.h"
 #include "../ipc/QueueFactory.h"
 #include "../tmtcpacket/pus/TcPacketStored.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/TmPacketStoredPusA.h"
 #include "../serviceinterface/ServiceInterface.h"
 
 object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;

From f906605097465daa0f3d300de5bfa7ade1a11384 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 12 Apr 2021 23:55:33 +0200
Subject: [PATCH 052/125] addes pus packet c implementation

---
 timemanager/TimeStamperIF.h           |   5 +-
 tmtcpacket/pus/TmPacketBase.cpp       |   9 +-
 tmtcpacket/pus/TmPacketBase.h         |   5 +-
 tmtcpacket/pus/TmPacketPusA.cpp       |  24 +----
 tmtcpacket/pus/TmPacketPusA.h         |  11 +--
 tmtcpacket/pus/TmPacketPusC.cpp       |  86 +++++++++++++++++
 tmtcpacket/pus/TmPacketPusC.h         | 127 ++++++++++++++++++++++++++
 tmtcpacket/pus/TmPacketStoredPusC.cpp |   2 +
 tmtcpacket/pus/TmPacketStoredPusC.h   |   8 ++
 9 files changed, 237 insertions(+), 40 deletions(-)
 create mode 100644 tmtcpacket/pus/TmPacketStoredPusC.cpp
 create mode 100644 tmtcpacket/pus/TmPacketStoredPusC.h

diff --git a/timemanager/TimeStamperIF.h b/timemanager/TimeStamperIF.h
index 57b7f014..1c4ada60 100644
--- a/timemanager/TimeStamperIF.h
+++ b/timemanager/TimeStamperIF.h
@@ -1,6 +1,7 @@
 #ifndef FSFW_TIMEMANAGER_TIMESTAMPERIF_H_
 #define FSFW_TIMEMANAGER_TIMESTAMPERIF_H_
 
+#include <FSFWConfig.h>
 #include "../returnvalues/HasReturnvaluesIF.h"
 
 /**
@@ -16,8 +17,8 @@ public:
 
 	//! This is a mission-specific constant and determines the total
 	//! size reserved for timestamps.
-	//! TODO: Default define in FSFWConfig ?
-	static const uint8_t MISSION_TIMESTAMP_SIZE = 8;
+	static const uint8_t MISSION_TIMESTAMP_SIZE = fsfwconfig::FSFW_MISSION_TIMESTAMP_SIZE;
+
 	virtual ReturnValue_t addTimeStamp(uint8_t* buffer,
 	        const uint8_t maxSize) = 0;
 	virtual ~TimeStamperIF() {}
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp
index 2dfef258..25193c92 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/TmPacketBase.cpp
@@ -38,7 +38,11 @@ void TmPacketBase::setErrorControl() {
     getSourceData()[size + 1] = (crc) & 0X00FF; 		// CRCL
 }
 
-
+ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const {
+    size_t tempSize = 0;
+    return CCSDSTime::convertFromCcsds(timestamp, getPacketTimeRaw(),
+            &tempSize, getTimestampSize());
+}
 
 void TmPacketBase::print() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -52,8 +56,7 @@ bool TmPacketBase::checkAndSetStamper() {
         timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
         if (timeStamper == NULL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::Warning << "TmPacketBase::checkAndSetStamper: Stamper not found!"
-                    << std::endl;
+            sif::warning << "TmPacketBase::checkAndSetStamper: Stamper not found!" << std::endl;
 #else
             sif::printWarning("TmPacketBase::checkAndSetStamper: Stamper not found!\n");
 #endif
diff --git a/tmtcpacket/pus/TmPacketBase.h b/tmtcpacket/pus/TmPacketBase.h
index 09a6ca54..6925e99b 100644
--- a/tmtcpacket/pus/TmPacketBase.h
+++ b/tmtcpacket/pus/TmPacketBase.h
@@ -29,8 +29,7 @@ public:
     //! Maximum size of a TM Packet in this mission.
     //! TODO: Make this dependant on a config variable.
     static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
-    //! First byte of secondary header for PUS-A packets.
-    //! TODO: Maybe also support PUS-C via config?
+    //! First four bits of first byte of secondary header
     static const uint8_t VERSION_NUMBER_BYTE = 0b00010000;
 
     /**
@@ -87,7 +86,7 @@ public:
      * timeval format.
      * @return Converted timestamp of packet.
      */
-    virtual ReturnValue_t getPacketTime(timeval* timestamp) const = 0;
+    virtual ReturnValue_t getPacketTime(timeval* timestamp) const;
     /**
      * Returns a raw pointer to the beginning of the time field.
      * @return Raw pointer to time field.
diff --git a/tmtcpacket/pus/TmPacketPusA.cpp b/tmtcpacket/pus/TmPacketPusA.cpp
index 841f7e40..d96f6aa7 100644
--- a/tmtcpacket/pus/TmPacketPusA.cpp
+++ b/tmtcpacket/pus/TmPacketPusA.cpp
@@ -49,27 +49,7 @@ uint16_t TmPacketPusA::getDataFieldSize() {
     return sizeof(PUSTmDataFieldHeaderPusA);
 }
 
-bool TmPacketPusA::checkAndSetStamper() {
-    if (timeStamper == NULL) {
-        timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
-        if (timeStamper == NULL) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "TmPacketPusA::checkAndSetStamper: Stamper not found!"
-                    << std::endl;
-#endif
-            return false;
-        }
-    }
-    return true;
-}
-
-ReturnValue_t TmPacketPusA::getPacketTime(timeval* timestamp) const {
-    size_t tempSize = 0;
-    return CCSDSTime::convertFromCcsds(timestamp, tmData->data_field.time,
-            &tempSize, sizeof(tmData->data_field.time));
-}
-
-uint8_t* TmPacketPusA::getPacketTimeRaw() const{
+uint8_t* TmPacketPusA::getPacketTimeRaw() const {
     return tmData->data_field.time;
 
 }
@@ -92,7 +72,7 @@ void TmPacketPusA::initializeTmPacket(uint16_t apid, uint8_t service,
     tmData->data_field.service_subtype = subservice;
     tmData->data_field.subcounter = packetSubcounter;
     //Timestamp packet
-    if (checkAndSetStamper()) {
+    if (TmPacketBase::checkAndSetStamper()) {
         timeStamper->addTimeStamp(tmData->data_field.time,
                 sizeof(tmData->data_field.time));
     }
diff --git a/tmtcpacket/pus/TmPacketPusA.h b/tmtcpacket/pus/TmPacketPusA.h
index 21bdfd95..dd9a5d09 100644
--- a/tmtcpacket/pus/TmPacketPusA.h
+++ b/tmtcpacket/pus/TmPacketPusA.h
@@ -53,9 +53,6 @@ public:
     //! Maximum size of a TM Packet in this mission.
     //! TODO: Make this dependant on a config variable.
     static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
-    //! First byte of secondary header for PUS-A packets.
-    //! TODO: Maybe also support PUS-C via config?
-    static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 0b00010000;
 
     /**
      * This is the default constructor.
@@ -76,19 +73,13 @@ public:
     uint16_t getSourceDataSize() override;
     uint16_t getDataFieldSize() override;
 
-    /**
-     * Interprets the "time"-field in the secondary header and returns it in
-     * timeval format.
-     * @return Converted timestamp of packet.
-     */
-    ReturnValue_t getPacketTime(timeval* timestamp) const override;
     /**
      * Returns a raw pointer to the beginning of the time field.
      * @return Raw pointer to time field.
      */
     uint8_t* getPacketTimeRaw() const override;
-
     size_t getTimestampSize() const override;
+
     size_t getPacketMinimumSize() const override;
 
 protected:
diff --git a/tmtcpacket/pus/TmPacketPusC.cpp b/tmtcpacket/pus/TmPacketPusC.cpp
index 99f50825..e6aa50dd 100644
--- a/tmtcpacket/pus/TmPacketPusC.cpp
+++ b/tmtcpacket/pus/TmPacketPusC.cpp
@@ -1,2 +1,88 @@
 #include "TmPacketPusC.h"
+#include "TmPacketBase.h"
 
+#include "../../globalfunctions/CRC.h"
+#include "../../globalfunctions/arrayprinter.h"
+#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../timemanager/CCSDSTime.h"
+
+#include <cstring>
+
+
+TmPacketPusC::TmPacketPusC(uint8_t* setData) : TmPacketBase(setData) {
+    tmData = reinterpret_cast<TmPacketPointerPusC*>(setData);
+}
+
+TmPacketPusC::~TmPacketPusC() {
+    //Nothing to do.
+}
+
+uint8_t TmPacketPusC::getService() {
+    return tmData->dataField.serviceType;
+}
+
+uint8_t TmPacketPusC::getSubService() {
+    return tmData->dataField.serviceSubtype;
+}
+
+uint8_t* TmPacketPusC::getSourceData() {
+    return &tmData->data;
+}
+
+uint16_t TmPacketPusC::getSourceDataSize() {
+    return getPacketDataLength() - sizeof(tmData->dataField) - CRC_SIZE + 1;
+}
+
+void TmPacketPusC::setData(const uint8_t* p_Data) {
+    SpacePacketBase::setData(p_Data);
+    tmData = (TmPacketPointerPusC*) p_Data;
+}
+
+
+size_t TmPacketPusC::getPacketMinimumSize() const {
+    return TM_PACKET_MIN_SIZE;
+}
+
+uint16_t TmPacketPusC::getDataFieldSize() {
+    return sizeof(PUSTmDataFieldHeaderPusC);
+}
+
+uint8_t* TmPacketPusC::getPacketTimeRaw() const{
+    return tmData->dataField.time;
+
+}
+
+void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
+        uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId = 0,
+        uint8_t timeRefField = 0) {
+    //Set primary header:
+    initSpacePacketHeader(false, true, apid);
+    //Set data Field Header:
+    //First, set to zero.
+    memset(&tmData->dataField, 0, sizeof(tmData->dataField));
+
+    // NOTE: In PUS-C, the PUS Version is 2 and specified for the first 4 bits.
+    // The other 4 bits of the first byte are the spacecraft time reference
+    // status. To change to PUS-C, set 0b00100000.
+    // Set CCSDS_secondary header flag to 0, version number to 001 and ack
+    // to 0000
+    tmData->dataField.versionTimeReferenceField = VERSION_NUMBER_BYTE | timeRefField;
+    tmData->dataField.serviceType = service;
+    tmData->dataField.serviceSubtype = subservice;
+    tmData->dataField.subcounter = packetSubcounter;
+    tmData->dataField.destinationId = destinationId;
+    //Timestamp packet
+    if (checkAndSetStamper()) {
+        timeStamper->addTimeStamp(tmData->dataField.time,
+                sizeof(tmData->dataField.time));
+    }
+}
+
+void TmPacketPusC::setSourceDataSize(uint16_t size) {
+    setPacketDataLength(size + sizeof(PUSTmDataFieldHeaderPusC) + CRC_SIZE - 1);
+}
+
+size_t TmPacketPusC::getTimestampSize() const {
+    return sizeof(tmData->dataField.time);
+}
diff --git a/tmtcpacket/pus/TmPacketPusC.h b/tmtcpacket/pus/TmPacketPusC.h
index fec435c4..975d134f 100644
--- a/tmtcpacket/pus/TmPacketPusC.h
+++ b/tmtcpacket/pus/TmPacketPusC.h
@@ -1,8 +1,135 @@
 #ifndef FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
 #define FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
 
+#include "TmPacketBase.h"
+#include "../SpacePacketBase.h"
+#include "../../timemanager/TimeStamperIF.h"
+#include "../../timemanager/Clock.h"
+#include "../../objectmanager/SystemObjectIF.h"
 
+namespace Factory{
+void setStaticFrameworkObjectIds();
+}
 
+/**
+ * This struct defines a byte-wise structured PUS TM Data Field Header.
+ * Any optional fields in the header must be added or removed here.
+ * Currently, no Destination field is present, but an eigth-byte representation
+ * for a time tag.
+ * @ingroup tmtcpackets
+ */
+struct PUSTmDataFieldHeaderPusC {
+    uint8_t versionTimeReferenceField;
+    uint8_t serviceType;
+    uint8_t serviceSubtype;
+    uint16_t subcounter;
+    uint16_t destinationId;
+    uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
+};
 
+/**
+ * This struct defines the data structure of a PUS Telecommand Packet when
+ * accessed via a pointer.
+ * @ingroup tmtcpackets
+ */
+struct TmPacketPointerPusC {
+    CCSDSPrimaryHeader primary;
+    PUSTmDataFieldHeaderPusC dataField;
+    uint8_t data;
+};
+
+/**
+ * PUS A packet implementation
+ * @ingroup tmtcpackets
+ */
+class TmPacketPusC: public TmPacketBase {
+    friend void (Factory::setStaticFrameworkObjectIds)();
+public:
+    /**
+     * This constant defines the minimum size of a valid PUS Telemetry Packet.
+     */
+    static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
+            sizeof(PUSTmDataFieldHeaderPusC) + 2);
+    //! Maximum size of a TM Packet in this mission.
+    //! TODO: Make this dependant on a config variable.
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
+
+    /**
+     * This is the default constructor.
+     * It sets its internal data pointer to the address passed and also
+     * forwards the data pointer to the parent SpacePacketBase class.
+     * @param set_address   The position where the packet data lies.
+     */
+    TmPacketPusC( uint8_t* setData );
+    /**
+     * This is the empty default destructor.
+     */
+    virtual ~TmPacketPusC();
+
+    /* TmPacketBase implementations */
+    uint8_t getService() override;
+    uint8_t getSubService() override;
+    uint8_t* getSourceData() override;
+    uint16_t getSourceDataSize() override;
+    uint16_t getDataFieldSize() override;
+
+    /**
+     * Interprets the "time"-field in the secondary header and returns it in
+     * timeval format.
+     * @return Converted timestamp of packet.
+     */
+    ReturnValue_t getPacketTime(timeval* timestamp) const override;
+    /**
+     * Returns a raw pointer to the beginning of the time field.
+     * @return Raw pointer to time field.
+     */
+    uint8_t* getPacketTimeRaw() const override;
+
+    size_t getTimestampSize() const override;
+    size_t getPacketMinimumSize() const override;
+
+protected:
+    /**
+     * A pointer to a structure which defines the data structure of
+     * the packet's data.
+     *
+     * To be hardware-safe, all elements are of byte size.
+     */
+    TmPacketPointerPusC* tmData;
+
+    /**
+     * Initializes the Tm Packet header.
+     * Does set the timestamp (to now), but not the error control field.
+     * @param apid APID used.
+     * @param service   PUS Service
+     * @param subservice PUS Subservice
+     * @param packetSubcounter Additional subcounter used.
+     */
+    void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
+            uint16_t packetSubcounter, uint16_t destinationId, uint8_t timeRefField);
+
+    /**
+     * With this method, the packet data pointer can be redirected to another
+     * location.
+     *
+     * This call overwrites the parent's setData method to set both its
+     * @c tc_data pointer and the parent's @c data pointer.
+     *
+     * @param p_data    A pointer to another PUS Telemetry Packet.
+     */
+    void setData( const uint8_t* pData );
+
+    /**
+     * In case data was filled manually (almost never the case).
+     * @param size Size of source data (without CRC and data filed header!).
+     */
+    void setSourceDataSize(uint16_t size);
+
+    /**
+     * Checks if a time stamper is available and tries to set it if not.
+     * @return Returns false if setting failed.
+     */
+    bool checkAndSetStamper();
+};
 
 #endif /* FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_ */
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.cpp b/tmtcpacket/pus/TmPacketStoredPusC.cpp
new file mode 100644
index 00000000..bb27b80f
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketStoredPusC.cpp
@@ -0,0 +1,2 @@
+#include "TmPacketStoredPusC.h"
+
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.h b/tmtcpacket/pus/TmPacketStoredPusC.h
new file mode 100644
index 00000000..53b39414
--- /dev/null
+++ b/tmtcpacket/pus/TmPacketStoredPusC.h
@@ -0,0 +1,8 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
+
+
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_ */

From ed186b04dfdfb41f5574fac1d01fddd3a9162511 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 13 Apr 2021 00:19:09 +0200
Subject: [PATCH 053/125] finalized PUS C TM support

---
 pus/Service17Test.cpp                        | 15 +++-
 pus/Service1TelecommandVerification.cpp      | 12 ++-
 pus/Service5EventReporting.cpp               |  7 +-
 tmtcpacket/pus/CMakeLists.txt                |  1 +
 tmtcpacket/pus/TmPacketPusC.cpp              |  8 +-
 tmtcpacket/pus/TmPacketPusC.h                | 15 +---
 tmtcpacket/pus/TmPacketStored.h              |  1 +
 tmtcpacket/pus/TmPacketStoredPusA.cpp        |  3 +-
 tmtcpacket/pus/TmPacketStoredPusA.h          | 10 +--
 tmtcpacket/pus/TmPacketStoredPusC.cpp        | 78 ++++++++++++++++++++
 tmtcpacket/pus/TmPacketStoredPusC.h          | 57 ++++++++++++++
 tmtcservices/CommandingServiceBase.cpp       | 18 ++++-
 unittest/user/unittest/core/CatchFactory.cpp |  4 +-
 13 files changed, 196 insertions(+), 33 deletions(-)

diff --git a/pus/Service17Test.cpp b/pus/Service17Test.cpp
index cdd8817c..daed987a 100644
--- a/pus/Service17Test.cpp
+++ b/pus/Service17Test.cpp
@@ -1,8 +1,9 @@
 #include "Service17Test.h"
+#include <FSFWConfig.h>
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/SystemObject.h"
-#include "../tmtcpacket/pus/TmPacketStoredPusA.h"
+#include "../tmtcpacket/pus/TmPacketStored.h"
 
 
 Service17Test::Service17Test(object_id_t objectId,
@@ -17,15 +18,25 @@ Service17Test::~Service17Test() {
 ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
 	switch(subservice) {
 	case Subservice::CONNECTION_TEST: {
+#if FSFW_USE_PUS_C_TELEMETRY == 0
 		TmPacketStoredPusA connectionPacket(apid, serviceId,
 		        Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#else
+		TmPacketStoredPusC connectionPacket(apid, serviceId,
+		        Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#endif
 		connectionPacket.sendPacket(requestQueue->getDefaultDestination(),
 				requestQueue->getId());
 		return HasReturnvaluesIF::RETURN_OK;
 	}
     case Subservice::EVENT_TRIGGER_TEST: {
+#if FSFW_USE_PUS_C_TELEMETRY == 0
         TmPacketStoredPusA connectionPacket(apid, serviceId,
                 Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#else
+        TmPacketStoredPusC connectionPacket(apid, serviceId,
+                Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#endif
         connectionPacket.sendPacket(requestQueue->getDefaultDestination(),
                 requestQueue->getId());
         triggerEvent(TEST, 1234, 5678);
diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index 358d1faa..7ef08de7 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -3,7 +3,7 @@
 
 #include "../ipc/QueueFactory.h"
 #include "../tmtcservices/PusVerificationReport.h"
-#include "../tmtcpacket/pus/TmPacketStoredPusA.h"
+#include "../tmtcpacket/pus/TmPacketStored.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../tmtcservices/AcceptsTelemetryIF.h"
 
@@ -68,8 +68,13 @@ ReturnValue_t Service1TelecommandVerification::generateFailureReport(
 			message->getTcSequenceControl(), message->getStep(),
 			message->getErrorCode(), message->getParameter1(),
 			message->getParameter2());
+#if FSFW_USE_PUS_C_TELEMETRY == 0
 	TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
 	        packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(),
+            packetSubCounter++, &report);
+#endif
 	ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
 			tmQueue->getId());
 	return result;
@@ -79,8 +84,13 @@ ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
         PusVerificationMessage *message) {
 	SuccessReport report(message->getReportId(),message->getTcPacketId(),
 			message->getTcSequenceControl(),message->getStep());
+#if FSFW_USE_PUS_C_TELEMETRY == 0
 	TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
 	        packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(),
+            packetSubCounter++, &report);
+#endif
 	ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
 			tmQueue->getId());
 	return result;
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index f6281fe9..62eefcb3 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -4,7 +4,7 @@
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../events/EventManagerIF.h"
 #include "../ipc/QueueFactory.h"
-#include "../tmtcpacket/pus/TmPacketStoredPusA.h"
+#include "../tmtcpacket/pus/TmPacketStored.h"
 
 
 Service5EventReporting::Service5EventReporting(object_id_t objectId,
@@ -52,8 +52,13 @@ ReturnValue_t Service5EventReporting::generateEventReport(
 {
 	EventReport report(message.getEventId(),message.getReporter(),
 			message.getParameter1(),message.getParameter2());
+#if FSFW_USE_PUS_C_TELEMETRY == 0
 	TmPacketStoredPusA tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
 			message.getSeverity(), packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
+            message.getSeverity(), packetSubCounter++, &report);
+#endif
 	ReturnValue_t result = tmPacket.sendPacket(
 	        requestQueue->getDefaultDestination(),requestQueue->getId());
 	if(result != HasReturnvaluesIF::RETURN_OK) {
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index 747ed070..cd4f49f1 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -5,6 +5,7 @@ target_sources(${LIB_FSFW_NAME}
 		TmPacketBase.cpp
 		TmPacketMinimal.cpp
 		TmPacketStoredPusA.cpp
+		TmPacketStoredPusC.cpp
 		TmPacketPusA.cpp
 		TmPacketPusC.cpp
 		TmPacketStoredBase.cpp
diff --git a/tmtcpacket/pus/TmPacketPusC.cpp b/tmtcpacket/pus/TmPacketPusC.cpp
index e6aa50dd..a6b4c428 100644
--- a/tmtcpacket/pus/TmPacketPusC.cpp
+++ b/tmtcpacket/pus/TmPacketPusC.cpp
@@ -54,8 +54,8 @@ uint8_t* TmPacketPusC::getPacketTimeRaw() const{
 }
 
 void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
-        uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId = 0,
-        uint8_t timeRefField = 0) {
+        uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId,
+        uint8_t timeRefField) {
     //Set primary header:
     initSpacePacketHeader(false, true, apid);
     //Set data Field Header:
@@ -67,13 +67,15 @@ void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
     // status. To change to PUS-C, set 0b00100000.
     // Set CCSDS_secondary header flag to 0, version number to 001 and ack
     // to 0000
+    /* Only account for last 4 bytes */
+    timeRefField &= 0b1111;
     tmData->dataField.versionTimeReferenceField = VERSION_NUMBER_BYTE | timeRefField;
     tmData->dataField.serviceType = service;
     tmData->dataField.serviceSubtype = subservice;
     tmData->dataField.subcounter = packetSubcounter;
     tmData->dataField.destinationId = destinationId;
     //Timestamp packet
-    if (checkAndSetStamper()) {
+    if (TmPacketBase::checkAndSetStamper()) {
         timeStamper->addTimeStamp(tmData->dataField.time,
                 sizeof(tmData->dataField.time));
     }
diff --git a/tmtcpacket/pus/TmPacketPusC.h b/tmtcpacket/pus/TmPacketPusC.h
index 975d134f..d072d5b9 100644
--- a/tmtcpacket/pus/TmPacketPusC.h
+++ b/tmtcpacket/pus/TmPacketPusC.h
@@ -73,19 +73,13 @@ public:
     uint16_t getSourceDataSize() override;
     uint16_t getDataFieldSize() override;
 
-    /**
-     * Interprets the "time"-field in the secondary header and returns it in
-     * timeval format.
-     * @return Converted timestamp of packet.
-     */
-    ReturnValue_t getPacketTime(timeval* timestamp) const override;
     /**
      * Returns a raw pointer to the beginning of the time field.
      * @return Raw pointer to time field.
      */
     uint8_t* getPacketTimeRaw() const override;
-
     size_t getTimestampSize() const override;
+
     size_t getPacketMinimumSize() const override;
 
 protected:
@@ -106,7 +100,7 @@ protected:
      * @param packetSubcounter Additional subcounter used.
      */
     void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
-            uint16_t packetSubcounter, uint16_t destinationId, uint8_t timeRefField);
+            uint16_t packetSubcounter, uint16_t destinationId = 0, uint8_t timeRefField = 0);
 
     /**
      * With this method, the packet data pointer can be redirected to another
@@ -125,11 +119,6 @@ protected:
      */
     void setSourceDataSize(uint16_t size);
 
-    /**
-     * Checks if a time stamper is available and tries to set it if not.
-     * @return Returns false if setting failed.
-     */
-    bool checkAndSetStamper();
 };
 
 #endif /* FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_ */
diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/TmPacketStored.h
index 53ef3f4d..fadda561 100644
--- a/tmtcpacket/pus/TmPacketStored.h
+++ b/tmtcpacket/pus/TmPacketStored.h
@@ -4,6 +4,7 @@
 #include <FSFWConfig.h>
 
 #if FSFW_USE_PUS_C_TELEMETRY == 1
+#include "TmPacketStoredPusC.h"
 #else
 #include "TmPacketStoredPusA.h"
 #endif
diff --git a/tmtcpacket/pus/TmPacketStoredPusA.cpp b/tmtcpacket/pus/TmPacketStoredPusA.cpp
index 4931f8d9..68102b62 100644
--- a/tmtcpacket/pus/TmPacketStoredPusA.cpp
+++ b/tmtcpacket/pus/TmPacketStoredPusA.cpp
@@ -1,7 +1,6 @@
 #include "TmPacketStoredPusA.h"
 
-#include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tmtcservices/TmTcMessage.h"
 
 #include <cstring>
diff --git a/tmtcpacket/pus/TmPacketStoredPusA.h b/tmtcpacket/pus/TmPacketStoredPusA.h
index aeeba275..0cfcf0b8 100644
--- a/tmtcpacket/pus/TmPacketStoredPusA.h
+++ b/tmtcpacket/pus/TmPacketStoredPusA.h
@@ -1,18 +1,12 @@
 #ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
 #define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
 
-#include "TmPacketBase.h"
 #include "TmPacketStoredBase.h"
+#include "TmPacketPusA.h"
 #include <FSFWConfig.h>
 
-#include "../../tmtcpacket/pus/TmPacketPusA.h"
-#include "../../serialize/SerializeIF.h"
-#include "../../storagemanager/StorageManagerIF.h"
-#include "../../internalError/InternalErrorReporterIF.h"
-#include "../../ipc/MessageQueueSenderIF.h"
-
 /**
- *	This class generates a ECSS PUS Telemetry packet within a given
+ *	This class generates a ECSS PUS A Telemetry packet within a given
  *	intermediate storage.
  *	As most packets are passed between tasks with the help of a storage
  *	anyway, it seems logical to create a Packet-In-Storage access class
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.cpp b/tmtcpacket/pus/TmPacketStoredPusC.cpp
index bb27b80f..f006412e 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.cpp
+++ b/tmtcpacket/pus/TmPacketStoredPusC.cpp
@@ -1,2 +1,80 @@
 #include "TmPacketStoredPusC.h"
 
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../tmtcservices/TmTcMessage.h"
+
+#include <cstring>
+
+TmPacketStoredPusC::TmPacketStoredPusC(store_address_t setAddress) :
+        TmPacketStoredBase(setAddress), TmPacketPusC(nullptr){
+}
+
+TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
+        uint8_t subservice, uint16_t packetSubcounter, const uint8_t *data,
+        uint32_t size, const uint8_t *headerData, uint32_t headerSize, uint16_t destinationId,
+        uint8_t timeRefField) :
+        TmPacketPusC(nullptr) {
+    storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    if (not TmPacketStoredBase::checkAndSetStore()) {
+        return;
+    }
+    uint8_t *pData = nullptr;
+    ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
+            (getPacketMinimumSize() + size + headerSize), &pData);
+
+    if (returnValue != store->RETURN_OK) {
+        TmPacketStoredBase::checkAndReportLostTm();
+        return;
+    }
+    setData(pData);
+    initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField);
+    memcpy(getSourceData(), headerData, headerSize);
+    memcpy(getSourceData() + headerSize, data, size);
+    setPacketDataLength(
+            size + headerSize + sizeof(PUSTmDataFieldHeaderPusC) + CRC_SIZE - 1);
+}
+
+TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
+        uint8_t subservice, uint16_t packetSubcounter, SerializeIF *content,
+        SerializeIF *header, uint16_t destinationId, uint8_t timeRefField) :
+        TmPacketPusC(nullptr) {
+    storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    if (not TmPacketStoredBase::checkAndSetStore()) {
+        return;
+    }
+    size_t sourceDataSize = 0;
+    if (content != NULL) {
+        sourceDataSize += content->getSerializedSize();
+    }
+    if (header != NULL) {
+        sourceDataSize += header->getSerializedSize();
+    }
+    uint8_t *p_data = NULL;
+    ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
+            (getPacketMinimumSize() + sourceDataSize), &p_data);
+    if (returnValue != store->RETURN_OK) {
+        TmPacketStoredBase::checkAndReportLostTm();
+    }
+    setData(p_data);
+    initializeTmPacket(apid, service, subservice, packetSubcounter);
+    uint8_t *putDataHere = getSourceData();
+    size_t size = 0;
+    if (header != NULL) {
+        header->serialize(&putDataHere, &size, sourceDataSize,
+                SerializeIF::Endianness::BIG);
+    }
+    if (content != NULL) {
+        content->serialize(&putDataHere, &size, sourceDataSize,
+                SerializeIF::Endianness::BIG);
+    }
+    setPacketDataLength(
+            sourceDataSize + sizeof(PUSTmDataFieldHeaderPusC) + CRC_SIZE - 1);
+}
+
+uint8_t* TmPacketStoredPusC::getAllTmData() {
+    return getWholeData();
+}
+
+void TmPacketStoredPusC::setDataPointer(const uint8_t *newPointer) {
+    setData(newPointer);
+}
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.h b/tmtcpacket/pus/TmPacketStoredPusC.h
index 53b39414..83478a94 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.h
+++ b/tmtcpacket/pus/TmPacketStoredPusC.h
@@ -1,7 +1,64 @@
 #ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
 #define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
 
+#include <fsfw/tmtcpacket/pus/TmPacketPusC.h>
+#include <fsfw/tmtcpacket/pus/TmPacketStoredBase.h>
 
+/**
+ *  This class generates a ECSS PUS A Telemetry packet within a given
+ *  intermediate storage.
+ *  As most packets are passed between tasks with the help of a storage
+ *  anyway, it seems logical to create a Packet-In-Storage access class
+ *  which saves the user almost all storage handling operation.
+ *  Packets can both be newly created with the class and be "linked" to
+ *  packets in a store with the help of a storeAddress.
+ *  @ingroup tmtcpackets
+ */
+class TmPacketStoredPusC:
+        public TmPacketStoredBase,
+        public TmPacketPusC {
+public:
+    /**
+     * This is a default constructor which does not set the data pointer.
+     * However, it does try to set the packet store.
+     */
+    TmPacketStoredPusC( store_address_t setAddress );
+    /**
+     * With this constructor, new space is allocated in the packet store and
+     * a new PUS Telemetry Packet is created there.
+     * Packet Application Data passed in data is copied into the packet.
+     * The Application data is passed in two parts, first a header, then a
+     * data field. This allows building a Telemetry Packet from two separate
+     * data sources.
+     * @param apid          Sets the packet's APID field.
+     * @param service       Sets the packet's Service ID field.
+     *      This specifies the source service.
+     * @param subservice    Sets the packet's Service Subtype field.
+     *      This specifies the source sub-service.
+     * @param packet_counter    Sets the Packet counter field of this packet
+     * @param data          The payload data to be copied to the
+     *                      Application Data Field
+     * @param size          The amount of data to be copied.
+     * @param headerData    The header Data of the Application field,
+     *                       will be copied in front of data
+     * @param headerSize    The size of the headerDataF
+     */
+    TmPacketStoredPusC( uint16_t apid, uint8_t service, uint8_t subservice,
+            uint16_t packetCounter = 0, const uint8_t* data = nullptr,
+            uint32_t size = 0, const uint8_t* headerData = nullptr,
+            uint32_t headerSize = 0, uint16_t destinationId = 0, uint8_t timeRefField = 0);
+    /**
+     * Another ctor to directly pass structured content and header data to the
+     * packet to avoid additional buffers.
+     */
+    TmPacketStoredPusC( uint16_t apid, uint8_t service, uint8_t subservice,
+            uint16_t packetCounter, SerializeIF* content,
+            SerializeIF* header = nullptr, uint16_t destinationId = 0, uint8_t timeRefField = 0);
+
+    uint8_t* getAllTmData() override;
+    void setDataPointer(const uint8_t* newPointer) override;
+
+};
 
 
 
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index 147c8796..fbd29468 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -1,12 +1,13 @@
 #include "AcceptsTelemetryIF.h"
 #include "CommandingServiceBase.h"
 #include "TmTcMessage.h"
+#include <FSFWConfig.h>
 
 #include "../tcdistribution/PUSDistributorIF.h"
 #include "../objectmanager/ObjectManagerIF.h"
 #include "../ipc/QueueFactory.h"
 #include "../tmtcpacket/pus/TcPacketStored.h"
-#include "../tmtcpacket/pus/TmPacketStoredPusA.h"
+#include "../tmtcpacket/pus/TmPacketStored.h"
 #include "../serviceinterface/ServiceInterface.h"
 
 object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
@@ -293,8 +294,13 @@ void CommandingServiceBase::handleRequestQueue() {
 ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
 		const uint8_t* data, size_t dataLen, const uint8_t* headerData,
 		size_t headerSize) {
+#if FSFW_USE_PUS_C_TELEMETRY == 0
 	TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
 			this->tmPacketCounter, data, dataLen, headerData, headerSize);
+#else
+	TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice,
+            this->tmPacketCounter, data, dataLen, headerData, headerSize);
+#endif
 	ReturnValue_t result = tmPacketStored.sendPacket(
 			requestQueue->getDefaultDestination(), requestQueue->getId());
 	if (result == HasReturnvaluesIF::RETURN_OK) {
@@ -311,8 +317,13 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
     size_t size = 0;
     SerializeAdapter::serialize(&objectId, &pBuffer, &size,
                 sizeof(object_id_t), SerializeIF::Endianness::BIG);
+#if FSFW_USE_PUS_C_TELEMETRY == 0
     TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
             this->tmPacketCounter, data, dataLen, buffer, size);
+#else
+    TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice,
+            this->tmPacketCounter, data, dataLen, buffer, size);
+#endif
     ReturnValue_t result = tmPacketStored.sendPacket(
             requestQueue->getDefaultDestination(), requestQueue->getId());
     if (result == HasReturnvaluesIF::RETURN_OK) {
@@ -324,8 +335,13 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
 
 ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
         SerializeIF* content, SerializeIF* header) {
+#if FSFW_USE_PUS_C_TELEMETRY == 0
     TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
             this->tmPacketCounter, content, header);
+#else
+    TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice,
+            this->tmPacketCounter, content, header);
+#endif
     ReturnValue_t result = tmPacketStored.sendPacket(
             requestQueue->getDefaultDestination(), requestQueue->getId());
     if (result == HasReturnvaluesIF::RETURN_OK) {
diff --git a/unittest/user/unittest/core/CatchFactory.cpp b/unittest/user/unittest/core/CatchFactory.cpp
index 2c4eaf24..9afb4fdd 100644
--- a/unittest/user/unittest/core/CatchFactory.cpp
+++ b/unittest/user/unittest/core/CatchFactory.cpp
@@ -1,6 +1,6 @@
+#include "CatchFactory.h"
 #include <fsfw/datapoollocal/LocalDataPoolManager.h>
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-#include "CatchFactory.h"
 
 #include <fsfw/events/EventManager.h>
 #include <fsfw/health/HealthTable.h>
@@ -74,7 +74,7 @@ void Factory::setStaticFrameworkObjectIds() {
 
 	DeviceHandlerFailureIsolation::powerConfirmationId = objects::NO_OBJECT;
 
-	TmPacketStoredPusA::timeStamperId = objects::NO_OBJECT;
+	TmPacketBase::timeStamperId = objects::NO_OBJECT;
 }
 
 

From fc7e401ddc86b744e6e2bdb42291f67ca8302073 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 13 Apr 2021 01:24:26 +0200
Subject: [PATCH 054/125] pus c working

---
 defaultcfg/fsfwconfig/FSFWConfig.h    | 2 +-
 tmtcpacket/pus/TmPacketPusC.cpp       | 6 ++++--
 tmtcpacket/pus/TmPacketPusC.h         | 6 ++++--
 tmtcpacket/pus/TmPacketStoredPusC.cpp | 2 +-
 4 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index 9a07b4ea..23129402 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -45,7 +45,7 @@
 namespace fsfwconfig {
 //! Default timestamp size. The default timestamp will be an eight byte CDC
 //! short timestamp.
-static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 8;
+static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 7;
 
 //! Configure the allocated pool sizes for the event manager.
 static constexpr size_t FSFW_EVENTMGMR_MATCHTREE_NODES = 240;
diff --git a/tmtcpacket/pus/TmPacketPusC.cpp b/tmtcpacket/pus/TmPacketPusC.cpp
index a6b4c428..12f0d3f8 100644
--- a/tmtcpacket/pus/TmPacketPusC.cpp
+++ b/tmtcpacket/pus/TmPacketPusC.cpp
@@ -72,8 +72,10 @@ void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
     tmData->dataField.versionTimeReferenceField = VERSION_NUMBER_BYTE | timeRefField;
     tmData->dataField.serviceType = service;
     tmData->dataField.serviceSubtype = subservice;
-    tmData->dataField.subcounter = packetSubcounter;
-    tmData->dataField.destinationId = destinationId;
+    tmData->dataField.subcounterMsb = packetSubcounter << 8 & 0xff;
+    tmData->dataField.subcounterLsb = packetSubcounter & 0xff;
+    tmData->dataField.destinationIdMsb = destinationId << 8 & 0xff;
+    tmData->dataField.destinationIdLsb = destinationId & 0xff;
     //Timestamp packet
     if (TmPacketBase::checkAndSetStamper()) {
         timeStamper->addTimeStamp(tmData->dataField.time,
diff --git a/tmtcpacket/pus/TmPacketPusC.h b/tmtcpacket/pus/TmPacketPusC.h
index d072d5b9..fdc27548 100644
--- a/tmtcpacket/pus/TmPacketPusC.h
+++ b/tmtcpacket/pus/TmPacketPusC.h
@@ -22,8 +22,10 @@ struct PUSTmDataFieldHeaderPusC {
     uint8_t versionTimeReferenceField;
     uint8_t serviceType;
     uint8_t serviceSubtype;
-    uint16_t subcounter;
-    uint16_t destinationId;
+    uint8_t subcounterMsb;
+    uint8_t subcounterLsb;
+    uint8_t destinationIdMsb;
+    uint8_t destinationIdLsb;
     uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
 };
 
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.cpp b/tmtcpacket/pus/TmPacketStoredPusC.cpp
index f006412e..7f774411 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.cpp
+++ b/tmtcpacket/pus/TmPacketStoredPusC.cpp
@@ -56,7 +56,7 @@ TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
         TmPacketStoredBase::checkAndReportLostTm();
     }
     setData(p_data);
-    initializeTmPacket(apid, service, subservice, packetSubcounter);
+    initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField);
     uint8_t *putDataHere = getSourceData();
     size_t size = 0;
     if (header != NULL) {

From cf8a9996a75c41af890e3d51599814f75aa1fe40 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 13 Apr 2021 10:42:49 +0200
Subject: [PATCH 055/125] added new test stub

---
 unittest/tests/tmtcpacket/CMakeLists.txt | 3 +++
 unittest/tests/tmtcpacket/PusTmTest.cpp  | 3 +++
 2 files changed, 6 insertions(+)
 create mode 100644 unittest/tests/tmtcpacket/CMakeLists.txt
 create mode 100644 unittest/tests/tmtcpacket/PusTmTest.cpp

diff --git a/unittest/tests/tmtcpacket/CMakeLists.txt b/unittest/tests/tmtcpacket/CMakeLists.txt
new file mode 100644
index 00000000..a1a4c1b6
--- /dev/null
+++ b/unittest/tests/tmtcpacket/CMakeLists.txt
@@ -0,0 +1,3 @@
+target_sources(${TARGET_NAME} PRIVATE
+	PusTmTest.cpp
+)
diff --git a/unittest/tests/tmtcpacket/PusTmTest.cpp b/unittest/tests/tmtcpacket/PusTmTest.cpp
new file mode 100644
index 00000000..b28b04f6
--- /dev/null
+++ b/unittest/tests/tmtcpacket/PusTmTest.cpp
@@ -0,0 +1,3 @@
+
+
+

From 62e409a9f22092fdd650a1fa776d3b33a18694c2 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 15 Apr 2021 10:07:39 +0200
Subject: [PATCH 056/125] doc fix

---
 tmtcpacket/pus/TmPacketPusC.cpp | 7 +------
 1 file changed, 1 insertion(+), 6 deletions(-)

diff --git a/tmtcpacket/pus/TmPacketPusC.cpp b/tmtcpacket/pus/TmPacketPusC.cpp
index 12f0d3f8..ca2bccdb 100644
--- a/tmtcpacket/pus/TmPacketPusC.cpp
+++ b/tmtcpacket/pus/TmPacketPusC.cpp
@@ -62,12 +62,7 @@ void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
     //First, set to zero.
     memset(&tmData->dataField, 0, sizeof(tmData->dataField));
 
-    // NOTE: In PUS-C, the PUS Version is 2 and specified for the first 4 bits.
-    // The other 4 bits of the first byte are the spacecraft time reference
-    // status. To change to PUS-C, set 0b00100000.
-    // Set CCSDS_secondary header flag to 0, version number to 001 and ack
-    // to 0000
-    /* Only account for last 4 bytes */
+    /* Only account for last 4 bytes for time reference field */
     timeRefField &= 0b1111;
     tmData->dataField.versionTimeReferenceField = VERSION_NUMBER_BYTE | timeRefField;
     tmData->dataField.serviceType = service;

From 770356f8b68f2d897852b811d1ed61e0f57e6f69 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 19 Apr 2021 14:38:56 +0200
Subject: [PATCH 057/125] fix for warning print

---
 osal/linux/tcpipHelpers.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/osal/linux/tcpipHelpers.cpp b/osal/linux/tcpipHelpers.cpp
index 3e8f6009..d7c644ec 100644
--- a/osal/linux/tcpipHelpers.cpp
+++ b/osal/linux/tcpipHelpers.cpp
@@ -99,8 +99,8 @@ void tcpip::handleError(Protocol protocol, ErrorSources errorSrc, dur_millis_t s
     sif::warning << "tcpip::handleError: " << protocolString << " | " << errorSrcString <<
             " | " << infoString << std::endl;
 #else
-    sif::printWarning("tcpip::handleError: %s | %s | %s\n", protocolString,
-            errorSrcString, infoString);
+    sif::printWarning("tcpip::handleError: %s | %s | %s\n", protocolString.c_str(),
+            errorSrcString.c_str(), infoString.c_str());
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 
     if(sleepDuration > 0) {

From a66fc5339606495fd5222a2bbcf6160647289bf8 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 20 Apr 2021 15:31:03 +0200
Subject: [PATCH 058/125] config define moved, better warning

---
 defaultcfg/fsfwconfig/FSFWConfig.h | 13 +++++++------
 osal/linux/PosixThread.cpp         | 10 +++++++++-
 2 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index e1e0ec64..fe18a2f4 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -40,6 +40,13 @@
 //! Specify whether a special mode store is used for Subsystem components.
 #define FSFW_USE_MODESTORE          0
 
+//! Defines if the real time scheduler for linux should be used.
+//! If set to 0, this will also disable priority settings for linux
+//! as most systems will not allow to set nice values without privileges
+//! For embedded linux system set this to 1.
+//! If set to 1 the binary needs "cap_sys_nice=eip" privileges to run
+#define FSFW_USE_REALTIME_FOR_LINUX 1
+
 namespace fsfwconfig {
 //! Default timestamp size. The default timestamp will be an eight byte CDC
 //! short timestamp.
@@ -58,12 +65,6 @@ static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
 
 static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
 
-//! Defines if the real time scheduler for linux should be used.
-//! If set to 0, this will also disable priority settings for linux
-//! as most systems will not allow to set nice values without privileges
-//! For embedded linux system set this to 1.
-//! If set to 1 the binary needs "cap_sys_nice=eip" privileges to run
-#define FSFW_USE_REALTIME_FOR_LINUX 1
 }
 
 #endif /* CONFIG_FSFWCONFIG_H_ */
diff --git a/osal/linux/PosixThread.cpp b/osal/linux/PosixThread.cpp
index f1cff992..72adfb14 100644
--- a/osal/linux/PosixThread.cpp
+++ b/osal/linux/PosixThread.cpp
@@ -223,8 +223,16 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
 	status = pthread_create(&thread,&attributes,fnc_,arg_);
 	if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread create failed with: " <<
+		sif::error << "PosixThread::createTask: Failed with: " <<
 				strerror(status) << std::endl;
+		sif::error << "For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call " <<
+		        "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set "
+		        "/etc/security/limit.conf" << std::endl;
+#else
+		sif::printError("PosixThread::createTask: Create failed with: %s\n", strerror(status));
+		sif::printError("For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call "
+		        "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set "
+                "/etc/security/limit.conf\n");
 #endif
 	}
 

From cc8c3aef3d3047c4a6692f24513459043a3c9cfd Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 20 Apr 2021 16:18:39 +0200
Subject: [PATCH 059/125] removed unused interface

---
 tmtcpacket/pus/TmPacketIF.h | 8 --------
 1 file changed, 8 deletions(-)
 delete mode 100644 tmtcpacket/pus/TmPacketIF.h

diff --git a/tmtcpacket/pus/TmPacketIF.h b/tmtcpacket/pus/TmPacketIF.h
deleted file mode 100644
index bd66523b..00000000
--- a/tmtcpacket/pus/TmPacketIF.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef FSFW_TMTCPACKET_PUS_TMPACKETIF_H_
-#define FSFW_TMTCPACKET_PUS_TMPACKETIF_H_
-
-
-
-
-
-#endif /* FSFW_TMTCPACKET_PUS_TMPACKETIF_H_ */

From 3f91803422a97a448a23da550a05cae1564f900b Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 20 Apr 2021 18:35:11 +0200
Subject: [PATCH 060/125] fixes from code review

---
 defaultcfg/fsfwconfig/FSFWConfig.h  | 4 ++--
 tmtcpacket/pus/TmPacketStoredPusC.h | 5 ++++-
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index 2e5a7aae..1abdab4c 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -50,8 +50,8 @@
 #define FSFW_USE_REALTIME_FOR_LINUX 1
 
 namespace fsfwconfig {
-//! Default timestamp size. The default timestamp will be an eight byte CDC
-//! short timestamp.
+
+//! Default timestamp size. The default timestamp will be an seven byte CDC short timestamp.
 static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 7;
 
 //! Configure the allocated pool sizes for the event manager.
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.h b/tmtcpacket/pus/TmPacketStoredPusC.h
index 83478a94..883dc0ff 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.h
+++ b/tmtcpacket/pus/TmPacketStoredPusC.h
@@ -5,7 +5,7 @@
 #include <fsfw/tmtcpacket/pus/TmPacketStoredBase.h>
 
 /**
- *  This class generates a ECSS PUS A Telemetry packet within a given
+ *  This class generates a ECSS PUS C Telemetry packet within a given
  *  intermediate storage.
  *  As most packets are passed between tasks with the help of a storage
  *  anyway, it seems logical to create a Packet-In-Storage access class
@@ -42,6 +42,9 @@ public:
      * @param headerData    The header Data of the Application field,
      *                       will be copied in front of data
      * @param headerSize    The size of the headerDataF
+     * @param destinationId Destination ID containing the application process ID as specified
+     *                      by PUS C
+     * @param timeRefField  4 bit time reference field as specified by PUS C
      */
     TmPacketStoredPusC( uint16_t apid, uint8_t service, uint8_t subservice,
             uint16_t packetCounter = 0, const uint8_t* data = nullptr,

From 3356ccc9d4bbbe336249b2fb4763adcaea0acd52 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 22 Apr 2021 17:45:46 +0200
Subject: [PATCH 061/125] tested new comment export string  for retvals

---
 ipc/MessageQueueIF.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/ipc/MessageQueueIF.h b/ipc/MessageQueueIF.h
index 74ccb29a..217174f6 100644
--- a/ipc/MessageQueueIF.h
+++ b/ipc/MessageQueueIF.h
@@ -22,11 +22,11 @@ public:
 	static const uint8_t INTERFACE_ID = CLASS_ID::MESSAGE_QUEUE_IF;
 	//! No new messages on the queue
 	static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(1);
-	//! No space left for more messages
+	//! [EXPORT] : [COMMENT] No space left for more messages
 	static const ReturnValue_t FULL = MAKE_RETURN_CODE(2);
-	//! Returned if a reply method was called without partner
+	//! [EXPORT] : [COMMENT] Returned if a reply method was called without partner
 	static const ReturnValue_t NO_REPLY_PARTNER = MAKE_RETURN_CODE(3);
-	//! Returned if the target destination is invalid.
+	//! [EXPORT] : [COMMENT] Returned if the target destination is invalid.
 	static constexpr ReturnValue_t DESTINATION_INVALID = MAKE_RETURN_CODE(4);
 
 	virtual ~MessageQueueIF() {}

From 7f1cbaef23d2c508e0bf88b190cb57eefd361e3e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 22 Apr 2021 18:09:23 +0200
Subject: [PATCH 062/125] updated default cmakelists

---
 defaultcfg/fsfwconfig/CMakeLists.txt | 28 ++++++++++++++++++----------
 1 file changed, 18 insertions(+), 10 deletions(-)

diff --git a/defaultcfg/fsfwconfig/CMakeLists.txt b/defaultcfg/fsfwconfig/CMakeLists.txt
index cbd4ecde..178fc273 100644
--- a/defaultcfg/fsfwconfig/CMakeLists.txt
+++ b/defaultcfg/fsfwconfig/CMakeLists.txt
@@ -1,15 +1,23 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE
-	ipc/missionMessageTypes.cpp
-	objects/FsfwFactory.cpp
-	pollingsequence/PollingSequenceFactory.cpp
-)
-
-# Should be added to include path
 target_include_directories(${TARGET_NAME} PRIVATE
-	${CMAKE_CURRENT_SOURCE_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}
 )
 
-if(NOT FSFW_CONFIG_PATH)
-	set(FSFW_CONFIG_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+target_sources(${TARGET_NAME} PRIVATE
+    ipc/missionMessageTypes.cpp
+    pollingsequence/PollingSequenceFactory.cpp
+    objects/FsfwFactory.cpp
+)
+
+# If a special translation file for object IDs exists, compile it.
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/objects/translateObjects.cpp")
+    target_sources(${TARGET_NAME} PRIVATE
+        objects/translateObjects.cpp
+    )
 endif()
 
+# If a special translation file for events exists, compile it.
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/objects/translateObjects.cpp")
+    target_sources(${TARGET_NAME} PRIVATE
+        events/translateEvents.cpp
+    )
+endif()

From 18a9729c75875f8bee674f03dd93a5eeeb657a66 Mon Sep 17 00:00:00 2001
From: "Jakob.Meier" <meierj@irs.uni-stuttgart.de>
Date: Sat, 24 Apr 2021 13:52:06 +0200
Subject: [PATCH 063/125] added multiple reply support

---
 devicehandlers/DeviceHandlerBase.cpp | 19 ++++++++++++-------
 devicehandlers/DeviceHandlerBase.h   | 18 ++++++++++++++++--
 2 files changed, 28 insertions(+), 9 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 0649aaa0..472b81f0 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -456,6 +456,15 @@ ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceComm
     }
 }
 
+size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId){
+    DeviceReplyIter iter = deviceReplyMap.find(commandId);
+    if(iter != deviceReplyMap.end()) {
+        return iter->second.replyLen;
+    }else{
+        return 0;
+   }
+}
+
 ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceReply,
         uint16_t delayCycles, uint16_t maxDelayCycles, bool periodic) {
     auto replyIter = deviceReplyMap.find(deviceReply);
@@ -646,16 +655,12 @@ void DeviceHandlerBase::doGetWrite() {
 void DeviceHandlerBase::doSendRead() {
     ReturnValue_t result;
 
-    size_t requestLen = 0;
+    size_t replyLen = 0;
     if(cookieInfo.pendingCommand != deviceCommandMap.end()) {
-        DeviceReplyIter iter = deviceReplyMap.find(
-                cookieInfo.pendingCommand->first);
-        if(iter != deviceReplyMap.end()) {
-            requestLen = iter->second.replyLen;
-        }
+        replyLen = getNextReplyLength(cookieInfo.pendingCommand->first);
     }
 
-    result = communicationInterface->requestReceiveMessage(comCookie, requestLen);
+    result = communicationInterface->requestReceiveMessage(comCookie, replyLen);
 
     if (result == RETURN_OK) {
         cookieInfo.state = COOKIE_READ_SENT;
diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index d61b0407..496c08ff 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -471,13 +471,27 @@ protected:
     ReturnValue_t insertInReplyMap(DeviceCommandId_t deviceCommand,
             uint16_t maxDelayCycles, LocalPoolDataSetBase* dataSet = nullptr,
             size_t replyLen = 0, bool periodic = false);
+
     /**
-     * @brief 	A simple command to add a command to the commandList.
+     * @brief   A simple command to add a command to the commandList.
      * @param deviceCommand The command to add
      * @return - @c RETURN_OK when the command was successfully inserted,
      *         - @c RETURN_FAILED else.
      */
     ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand);
+
+    /**
+     * @brief   This function returns the reply length of the next reply to read.
+     *
+     * @param deviceCommand The command which triggered the device reply.
+     *
+     * @details The default implementation assumes only one reply is triggered by the command. In
+     *          case the command triggers multiple replies (e.g. one acknowledgment, one data,
+     *          and one execution status reply), this function can be overwritten to get the
+     *          reply length of the next reply to read.
+     */
+    virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand);
+
     /**
      * @brief 	This is a helper method to facilitate updating entries
      *        	in the reply map.
@@ -953,7 +967,7 @@ protected:
      * 			- NO_REPLY_EXPECTED if there was no reply found. This is not an
      * 			  error case as many commands do not expect a reply.
      */
-    virtual ReturnValue_t enableReplyInReplyMap(DeviceCommandMap::iterator cmd,
+    virtual ReturnValue_t enableReplyInReplyMap(DeviceCommandMap::iterator command,
             uint8_t expectedReplies = 1, bool useAlternateId = false,
             DeviceCommandId_t alternateReplyID = 0);
 

From 56d43f5b49c0a737e4604238dfe8ce0988ed4b04 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 24 Apr 2021 22:46:12 +0200
Subject: [PATCH 064/125] better output

---
 objectmanager/ObjectManager.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/objectmanager/ObjectManager.cpp b/objectmanager/ObjectManager.cpp
index 3c2be532..674fedc1 100644
--- a/objectmanager/ObjectManager.cpp
+++ b/objectmanager/ObjectManager.cpp
@@ -108,9 +108,9 @@ void ObjectManager::initialize() {
 		result = it.second->checkObjectConnections();
 		if ( result != RETURN_OK ) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "ObjectManager::ObjectManager: Object " << std::hex <<
-					(int) it.first  << " connection check failed with code 0x"
-					<< result << std::dec << std::endl;
+			sif::error << "ObjectManager::ObjectManager: Object 0x" << std::hex <<
+					(int) it.first  << " connection check failed with code 0x" << result <<
+					std::dec << std::endl;
 #endif
 			errorCount++;
 		}

From ff33a1274daf1af83832501036f2adbb807ba690 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 24 Apr 2021 23:54:13 +0200
Subject: [PATCH 065/125] deleted old bridge

---
 osal/linux/TmTcUnixUdpBridge.cpp | 157 -------------------------------
 osal/linux/TmTcUnixUdpBridge.h   |  49 ----------
 2 files changed, 206 deletions(-)
 delete mode 100644 osal/linux/TmTcUnixUdpBridge.cpp
 delete mode 100644 osal/linux/TmTcUnixUdpBridge.h

diff --git a/osal/linux/TmTcUnixUdpBridge.cpp b/osal/linux/TmTcUnixUdpBridge.cpp
deleted file mode 100644
index 62ac41c0..00000000
--- a/osal/linux/TmTcUnixUdpBridge.cpp
+++ /dev/null
@@ -1,157 +0,0 @@
-#include "TmTcUnixUdpBridge.h"
-#include "../common/tcpipHelpers.h"
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../ipc/MutexGuard.h"
-
-#include <arpa/inet.h>
-#include <unistd.h>
-#include <netdb.h>
-
-#include <cstring>
-
-//! Debugging preprocessor define.
-#define FSFW_UDP_SEND_WIRETAPPING_ENABLED   0
-
-const std::string TmTcUnixUdpBridge::DEFAULT_UDP_SERVER_PORT =  tcpip::DEFAULT_SERVER_PORT;
-
-TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId, object_id_t tcDestination,
-        object_id_t tmStoreId, object_id_t tcStoreId, std::string udpServerPort):
-		TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
-    if(udpServerPort == "") {
-        this->udpServerPort = DEFAULT_UDP_SERVER_PORT;
-    }
-    else {
-        this->udpServerPort = udpServerPort;
-    }
-
-    mutex = MutexFactory::instance()->createMutex();
-    communicationLinkUp = false;
-}
-
-ReturnValue_t TmTcUnixUdpBridge::initialize() {
-    using namespace tcpip;
-
-    ReturnValue_t result = TmTcBridge::initialize();
-    if(result != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcUnixUdpBridge::initialize: TmTcBridge initialization failed!"
-                << std::endl;
-#endif
-        return result;
-    }
-
-    struct addrinfo *addrResult = nullptr;
-    struct addrinfo hints;
-
-    std::memset(&hints, 0, sizeof(hints));
-    hints.ai_family = AF_INET;
-    hints.ai_socktype = SOCK_DGRAM;
-    hints.ai_protocol = IPPROTO_UDP;
-    hints.ai_flags = AI_PASSIVE;
-
-    /* Set up UDP socket:
-    https://man7.org/linux/man-pages/man3/getaddrinfo.3.html
-    Passing nullptr as the first parameter and specifying AI_PASSIVE in hints will cause
-    getaddrinfo to assign the address 0.0.0.0 (any address) */
-    int retval = getaddrinfo(nullptr, udpServerPort.c_str(), &hints, &addrResult);
-    if (retval != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TmTcWinUdpBridge::TmTcWinUdpBridge: Retrieving address info failed!" <<
-                std::endl;
-#endif
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-
-    /* Set up UDP socket: https://man7.org/linux/man-pages/man7/ip.7.html */
-    serverSocket = socket(addrResult->ai_family, addrResult->ai_socktype, addrResult->ai_protocol);
-    if(serverSocket < 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not open UDP socket!" <<
-                std::endl;
-#else
-        sif::printError("TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not open UDP socket!\n");
-#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-        freeaddrinfo(addrResult);
-        handleError(Protocol::UDP, ErrorSources::SOCKET_CALL);
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-
-    retval = bind(serverSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
-    if(retval != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TmTcWinUdpBridge::TmTcWinUdpBridge: Could not bind "
-                "local port (" << udpServerPort << ") to server socket!" << std::endl;
-#endif
-        freeaddrinfo(addrResult);
-        handleError(Protocol::UDP, ErrorSources::BIND_CALL);
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-TmTcUnixUdpBridge::~TmTcUnixUdpBridge() {
-    if(mutex != nullptr) {
-        MutexFactory::instance()->deleteMutex(mutex);
-    }
-    close(serverSocket);
-}
-
-ReturnValue_t TmTcUnixUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
-    int flags = 0;
-
-    /* The target address can be set by different threads so this lock ensures thread-safety */
-    MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
-
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
-    char ipAddress [15];
-    sif::debug << "IP Address Sender: "<<
-            inet_ntop(AF_INET,&clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
-#endif
-
-    ssize_t bytesSent = sendto(
-            serverSocket,
-            data,
-            dataLen,
-            flags,
-            reinterpret_cast<sockaddr*>(&clientAddress),
-            clientAddressLen
-    );
-    if(bytesSent < 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TmTcUnixUdpBridge::sendTm: Send operation failed." << std::endl;
-#endif
-        tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::SENDTO_CALL);
-    }
-
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
-    sif::debug << "TmTcUnixUdpBridge::sendTm: " << bytesSent << " bytes were"
-            " sent." << std::endl;
-#endif
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-void TmTcUnixUdpBridge::checkAndSetClientAddress(sockaddr_in& newAddress) {
-    /* The target address can be set by different threads so this lock ensures thread-safety */
-    MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
-
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1
-    char ipAddress [15];
-    sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
-            &newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
-    sif::debug << "IP Address Old: " <<  inet_ntop(AF_INET,
-            &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
-#endif
-    registerCommConnect();
-
-    /* Set new IP address to reply to. */
-    clientAddress = newAddress;
-    clientAddressLen = sizeof(clientAddress);
-}
-
-void TmTcUnixUdpBridge::setMutexProperties(MutexIF::TimeoutType timeoutType,
-        dur_millis_t timeoutMs) {
-    this->timeoutType = timeoutType;
-    this->mutexTimeoutMs = timeoutMs;
-}
diff --git a/osal/linux/TmTcUnixUdpBridge.h b/osal/linux/TmTcUnixUdpBridge.h
deleted file mode 100644
index fd932714..00000000
--- a/osal/linux/TmTcUnixUdpBridge.h
+++ /dev/null
@@ -1,49 +0,0 @@
-#ifndef FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_
-#define FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_
-
-#include "../../tmtcservices/AcceptsTelecommandsIF.h"
-#include "../../tmtcservices/TmTcBridge.h"
-#include <sys/socket.h>
-#include <netinet/in.h>
-#include <netinet/udp.h>
-
-class TmTcUnixUdpBridge:
-        public TmTcBridge {
-    friend class UdpTcPollingTask;
-public:
-
-    /* The ports chosen here should not be used by any other process.
-	List of used ports on Linux: /etc/services */
-    static const std::string DEFAULT_UDP_SERVER_PORT;
-
-    TmTcUnixUdpBridge(object_id_t objectId, object_id_t tcDestination,
-            object_id_t tmStoreId, object_id_t tcStoreId,
-            std::string serverPort = "");
-    virtual~ TmTcUnixUdpBridge();
-
-    /**
-     * Set properties of internal mutex.
-     */
-    void setMutexProperties(MutexIF::TimeoutType timeoutType, dur_millis_t timeoutMs);
-
-    ReturnValue_t initialize() override;
-
-    void checkAndSetClientAddress(sockaddr_in& clientAddress);
-
-protected:
-    virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override;
-
-private:
-    int serverSocket = 0;
-    std::string udpServerPort;
-
-    struct sockaddr_in clientAddress;
-    socklen_t clientAddressLen = 0;
-
-    //! Access to the client address is mutex protected as it is set by another task.
-    MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
-    dur_millis_t mutexTimeoutMs = 20;
-    MutexIF* mutex;
-};
-
-#endif /* FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_ */

From 413ff0d1b9f0a0ed172ff1b427aab8f3f8d40a16 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 26 Apr 2021 14:17:57 +0200
Subject: [PATCH 066/125] parameter wrapper bugfix

---
 parameters/ParameterHelper.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/parameters/ParameterHelper.cpp b/parameters/ParameterHelper.cpp
index 24d0b2b1..e80c2c47 100644
--- a/parameters/ParameterHelper.cpp
+++ b/parameters/ParameterHelper.cpp
@@ -65,6 +65,9 @@ ReturnValue_t ParameterHelper::handleParameterMessage(CommandMessage *message) {
         ParameterWrapper ownerWrapper;
         result = owner->getParameter(domain, uniqueIdentifier, &ownerWrapper,
                 &streamWrapper, linearIndex);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
 
         result = ownerWrapper.copyFrom(&streamWrapper, linearIndex);
         if (result != HasReturnvaluesIF::RETURN_OK) {

From 097244bf8b429d1b107343a3dbd77fa537a9c86d Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 29 Apr 2021 19:51:38 +0200
Subject: [PATCH 067/125] use timestamp size from FSFWConfig.h

---
 timemanager/TimeStamperIF.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/timemanager/TimeStamperIF.h b/timemanager/TimeStamperIF.h
index 57b7f014..534cc734 100644
--- a/timemanager/TimeStamperIF.h
+++ b/timemanager/TimeStamperIF.h
@@ -2,6 +2,7 @@
 #define FSFW_TIMEMANAGER_TIMESTAMPERIF_H_
 
 #include "../returnvalues/HasReturnvaluesIF.h"
+#include <FSFWConfig.h>
 
 /**
  * A class implementing this IF provides facilities to add a time stamp to the
@@ -16,8 +17,7 @@ public:
 
 	//! This is a mission-specific constant and determines the total
 	//! size reserved for timestamps.
-	//! TODO: Default define in FSFWConfig ?
-	static const uint8_t MISSION_TIMESTAMP_SIZE = 8;
+	static const uint8_t MISSION_TIMESTAMP_SIZE = fsfwconfig::FSFW_MISSION_TIMESTAMP_SIZE;
 	virtual ReturnValue_t addTimeStamp(uint8_t* buffer,
 	        const uint8_t maxSize) = 0;
 	virtual ~TimeStamperIF() {}

From f3530d3c7ede036e02a65963fcf36da4e5dc024a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 30 Apr 2021 23:59:45 +0200
Subject: [PATCH 068/125] small format change for event to test

---
 datalinklayer/DataLinkLayer.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/datalinklayer/DataLinkLayer.h b/datalinklayer/DataLinkLayer.h
index 17a57d61..27e69006 100644
--- a/datalinklayer/DataLinkLayer.h
+++ b/datalinklayer/DataLinkLayer.h
@@ -19,7 +19,8 @@ class VirtualChannelReception;
 class DataLinkLayer : public CCSDSReturnValuesIF {
 public:
 	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_1;
-	static const Event RF_AVAILABLE = MAKE_EVENT(0, severity::INFO); //!< A RF available signal was detected. P1: raw RFA state, P2: 0
+	//! [EXPORT] : [COMMENT] A RF available signal was detected. P1: raw RFA state, P2: 0
+	static const Event RF_AVAILABLE = MAKE_EVENT(0, severity::INFO);
 	static const Event RF_LOST = MAKE_EVENT(1, severity::INFO); //!< A previously found RF available signal was lost. P1: raw RFA state, P2: 0
 	static const Event BIT_LOCK = MAKE_EVENT(2, severity::INFO); //!< A Bit Lock signal. Was detected. P1: raw BLO state, P2: 0
 	static const Event BIT_LOCK_LOST = MAKE_EVENT(3, severity::INFO); //!< A previously found Bit Lock signal was lost. P1: raw BLO state, P2: 0

From 126def219b30b1ca97c8fbc03825ed139e2a56ec Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 5 May 2021 12:59:42 +0200
Subject: [PATCH 069/125] continued tmtc server and bridge

---
 osal/common/CMakeLists.txt       |   1 +
 osal/common/TcpTmTcBridge.cpp    |  57 ++++++++++++
 osal/common/TcpTmTcBridge.h      |  54 +++++++++++
 osal/common/TcpTmTcServer.cpp    | 152 +++++++++++++++++++++++--------
 osal/common/TcpTmTcServer.h      |  26 +++++-
 osal/common/UdpTcPollingTask.cpp |   4 +-
 osal/common/UdpTcPollingTask.h   |   8 +-
 osal/common/UdpTmTcBridge.cpp    |  21 ++---
 osal/common/UdpTmTcBridge.h      |   6 +-
 osal/common/tcpipCommon.cpp      |   3 +
 osal/common/tcpipCommon.h        |   3 +-
 11 files changed, 270 insertions(+), 65 deletions(-)
 create mode 100644 osal/common/TcpTmTcBridge.cpp
 create mode 100644 osal/common/TcpTmTcBridge.h

diff --git a/osal/common/CMakeLists.txt b/osal/common/CMakeLists.txt
index af76484d..b7c8c033 100644
--- a/osal/common/CMakeLists.txt
+++ b/osal/common/CMakeLists.txt
@@ -5,6 +5,7 @@ if(DEFINED WIN32 OR DEFINED UNIX)
         UdpTcPollingTask.cpp
         UdpTmTcBridge.cpp
         TcpTmTcServer.cpp
+        TcpTmTcBridge.cpp
     )
 endif()
 
diff --git a/osal/common/TcpTmTcBridge.cpp b/osal/common/TcpTmTcBridge.cpp
new file mode 100644
index 00000000..a88b68e9
--- /dev/null
+++ b/osal/common/TcpTmTcBridge.cpp
@@ -0,0 +1,57 @@
+#include "TcpTmTcBridge.h"
+#include "tcpipHelpers.h"
+
+#include <fsfw/serviceinterface/ServiceInterface.h>
+#include <fsfw/ipc/MutexGuard.h>
+#include <fsfw/osal/common/TcpTmTcBridge.h>
+
+#ifdef _WIN32
+
+#include <ws2tcpip.h>
+
+#elif defined(__unix__)
+
+#include <netdb.h>
+#include <arpa/inet.h>
+
+#endif
+
+const std::string TcpTmTcBridge::DEFAULT_UDP_SERVER_PORT =  tcpip::DEFAULT_SERVER_PORT;
+
+TcpTmTcBridge::TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
+        object_id_t tmStoreId, object_id_t tcStoreId):
+        TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
+    mutex = MutexFactory::instance()->createMutex();
+    communicationLinkUp = false;
+}
+
+ReturnValue_t TcpTmTcBridge::initialize() {
+    ReturnValue_t result = TmTcBridge::initialize();
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "TmTcUdpBridge::initialize: TmTcBridge initialization failed!"
+                << std::endl;
+#endif
+        return result;
+    }
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+TcpTmTcBridge::~TcpTmTcBridge() {
+    if(mutex != nullptr) {
+        MutexFactory::instance()->deleteMutex(mutex);
+    }
+}
+
+ReturnValue_t TcpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) {
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+void TcpTmTcBridge::setMutexProperties(MutexIF::TimeoutType timeoutType,
+        dur_millis_t timeoutMs) {
+    this->timeoutType = timeoutType;
+    this->mutexTimeoutMs = timeoutMs;
+}
diff --git a/osal/common/TcpTmTcBridge.h b/osal/common/TcpTmTcBridge.h
new file mode 100644
index 00000000..db45fca8
--- /dev/null
+++ b/osal/common/TcpTmTcBridge.h
@@ -0,0 +1,54 @@
+#ifndef FSFW_OSAL_COMMON_TCPTMTCBRIDGE_H_
+#define FSFW_OSAL_COMMON_TCPTMTCBRIDGE_H_
+
+#include "TcpIpBase.h"
+#include "../../tmtcservices/TmTcBridge.h"
+
+#ifdef _WIN32
+
+#include <ws2tcpip.h>
+
+#elif defined(__unix__)
+
+#include <sys/socket.h>
+
+#endif
+
+#include <string>
+
+/**
+ * @brief   This class should be used with the UdpTcPollingTask to implement a UDP server
+ *          for receiving and sending PUS TMTC.
+ */
+class TcpTmTcBridge:
+        public TmTcBridge {
+    //friend class UdpTcPollingTask;
+public:
+    /* The ports chosen here should not be used by any other process. */
+    static const std::string DEFAULT_UDP_SERVER_PORT;
+
+    TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
+            object_id_t tmStoreId = objects::TM_STORE,
+            object_id_t tcStoreId = objects::TC_STORE);
+    virtual~ TcpTmTcBridge();
+
+    /**
+     * Set properties of internal mutex.
+     */
+    void setMutexProperties(MutexIF::TimeoutType timeoutType, dur_millis_t timeoutMs);
+
+    ReturnValue_t initialize() override;
+
+protected:
+    virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override;
+
+private:
+
+    //! Access to the client address is mutex protected as it is set by another task.
+    MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+    dur_millis_t mutexTimeoutMs = 20;
+    MutexIF* mutex;
+};
+
+#endif /* FSFW_OSAL_COMMON_TCPTMTCBRIDGE_H_ */
+
diff --git a/osal/common/TcpTmTcServer.cpp b/osal/common/TcpTmTcServer.cpp
index 08a62ffb..8717783e 100644
--- a/osal/common/TcpTmTcServer.cpp
+++ b/osal/common/TcpTmTcServer.cpp
@@ -1,6 +1,12 @@
 #include "TcpTmTcServer.h"
+#include "TcpTmTcBridge.h"
 #include "tcpipHelpers.h"
+
+#include "../../container/SharedRingBuffer.h"
+#include "../../ipc/MessageQueueSenderIF.h"
+#include "../../objectmanager/ObjectManagerIF.h"
 #include "../../serviceinterface/ServiceInterface.h"
+#include "../../tmtcservices/TmTcMessage.h"
 
 #ifdef _WIN32
 #include <winsock2.h>
@@ -12,12 +18,17 @@
 
 #endif
 
-const std::string TcpTmTcServer::DEFAULT_TCP_SERVER_PORT =  "7301";
-const std::string TcpTmTcServer::DEFAULT_TCP_CLIENT_PORT =  "7302";
+#ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED
+#define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0
+#endif
 
-TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
+const std::string TcpTmTcServer::DEFAULT_TCP_SERVER_PORT =  "7301";
+
+TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
+        /*SharedRingBuffer* tcpRingBuffer, */ size_t receptionBufferSize,
         std::string customTcpServerPort):
-        SystemObject(objectId), tcpPort(customTcpServerPort) {
+                        SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), tcpPort(customTcpServerPort),
+                        receptionBuffer(receptionBufferSize) /*, tcpRingBuffer(tcpRingBuffer) */ {
     if(tcpPort == "") {
         tcpPort = DEFAULT_TCP_SERVER_PORT;
     }
@@ -26,11 +37,34 @@ TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcUnixUdpBridge
 ReturnValue_t TcpTmTcServer::initialize() {
     using namespace tcpip;
 
+    /*
+    if(tcpRingBuffer == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "TcpTmTcServer::initialize: Invalid ring buffer!" << std::endl;
+#else
+        sif::printError("TcpTmTcServer::initialize: Invalid ring buffer!\n");
+#endif
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+    */
+
     ReturnValue_t result = TcpIpBase::initialize();
     if(result != HasReturnvaluesIF::RETURN_OK) {
         return result;
     }
 
+    tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
+    if (tcStore == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "TcpTmTcServer::initialize: TC store uninitialized!" << std::endl;
+#else
+        sif::printError("TcpTmTcServer::initialize: TC store uninitialized!\n");
+#endif
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+
+    tmtcBridge = objectManager->get<TcpTmTcBridge>(tmtcBridgeId);
+
     int retval = 0;
     struct addrinfo *addrResult = nullptr;
     struct addrinfo hints = {};
@@ -42,10 +76,6 @@ ReturnValue_t TcpTmTcServer::initialize() {
 
     retval = getaddrinfo(nullptr, tcpPort.c_str(), &hints, &addrResult);
     if (retval != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TcWinTcpServer::TcpTmTcServer: Retrieving address info failed!" <<
-                std::endl;
-#endif
         handleError(Protocol::TCP, ErrorSources::GETADDRINFO_CALL);
         return HasReturnvaluesIF::RETURN_FAILED;
     }
@@ -54,9 +84,6 @@ ReturnValue_t TcpTmTcServer::initialize() {
     listenerTcpSocket = socket(addrResult->ai_family, addrResult->ai_socktype,
             addrResult->ai_protocol);
     if(listenerTcpSocket == INVALID_SOCKET) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TcWinTcpServer::TcWinTcpServer: Socket creation failed!" << std::endl;
-#endif
         freeaddrinfo(addrResult);
         handleError(Protocol::TCP, ErrorSources::SOCKET_CALL);
         return HasReturnvaluesIF::RETURN_FAILED;
@@ -64,10 +91,6 @@ ReturnValue_t TcpTmTcServer::initialize() {
 
     retval = bind(listenerTcpSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
     if(retval == SOCKET_ERROR) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TcWinTcpServer::TcpTmTcServer: Binding socket failed!" <<
-                std::endl;
-#endif
         freeaddrinfo(addrResult);
         handleError(Protocol::TCP, ErrorSources::BIND_CALL);
         return HasReturnvaluesIF::RETURN_FAILED;
@@ -85,7 +108,7 @@ TcpTmTcServer::~TcpTmTcServer() {
 ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
     using namespace tcpip;
     /* If a connection is accepted, the corresponding socket will be assigned to the new socket */
-    socket_t clientSocket = 0;
+    socket_t connSocket = 0;
     sockaddr clientSockAddr = {};
     socklen_t connectorSockAddrLen = 0;
     int retval = 0;
@@ -98,35 +121,92 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
             continue;
         }
 
-        clientSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen);
+        connSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen);
 
-        if(clientSocket == INVALID_SOCKET) {
+        if(connSocket == INVALID_SOCKET) {
             handleError(Protocol::TCP, ErrorSources::ACCEPT_CALL, 500);
-            closeSocket(clientSocket);
+            closeSocket(connSocket);
             continue;
         };
 
-        retval = recv(clientSocket, reinterpret_cast<char*>(receptionBuffer.data()),
-                receptionBuffer.size(), 0);
-        if(retval > 0) {
-#if FSFW_TCP_RCV_WIRETAPPING_ENABLED == 1
-            sif::info << "TcpTmTcServer::performOperation: Received " << retval << " bytes."
-                    std::endl;
-#endif
-            handleError(Protocol::TCP, ErrorSources::RECV_CALL, 500);
-        }
-        else if(retval == 0) {
-
-        }
-        else {
-
-        }
+        handleServerOperation(connSocket);
 
         /* Done, shut down connection */
-        retval = shutdown(clientSocket, SHUT_SEND);
-        closeSocket(clientSocket);
+        retval = shutdown(connSocket, SHUT_SEND);
+        if(retval != 0) {
+            handleError(Protocol::TCP, ErrorSources::SHUTDOWN_CALL);
+        }
+        closeSocket(connSocket);
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
 
+ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() {
+    /* Initialize the destination after task creation. This ensures
+    that the destination has already been set in the TMTC bridge. */
+    targetTcDestination = tmtcBridge->getRequestQueue();
 
+//
+//    if(tcpRingBuffer != nullptr) {
+//        auto fifoCheck = tcpRingBuffer->getReceiveSizesFIFO();
+//        if (fifoCheck == nullptr) {
+//#if FSFW_CPP_OSTREAM_ENABLED == 1
+//            sif::error << "TcpTmTcServer::initializeAfterTaskCreation: "
+//                    "TCP ring buffer does not have a FIFO!" << std::endl;
+//#else
+//            sif::printError("TcpTmTcServer::initialize: TCP ring buffer does not have a FIFO!\n");
+//#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
+//        }
+//    }
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void TcpTmTcServer::handleServerOperation(socket_t connSocket) {
+    int retval = 0;
+    do {
+        retval = recv(connSocket,
+                reinterpret_cast<char*>(receptionBuffer.data()),
+                receptionBuffer.capacity(),
+                tcpFlags);
+        if (retval > 0) {
+            handleTcReception(retval);
+        }
+        else if(retval == 0) {
+            /* Client has finished sending telecommands, send telemetry now */
+        }
+        else {
+            /* Should not happen */
+        }
+    } while(retval > 0);
+}
+
+ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
+#if FSFW_TCP_RECV_WIRETAPPING_ENABLED == 1
+    arrayprinter::print(receptionBuffer.data(), bytesRead);
+#endif
+    store_address_t storeId;
+    ReturnValue_t result = tcStore->addData(&storeId, receptionBuffer.data(), bytesRecvd);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning<< "TcpTmTcServer::handleServerOperation: Data storage failed." << std::endl;
+        sif::warning << "Packet size: " << bytesRecvd << std::endl;
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+    }
+
+    TmTcMessage message(storeId);
+
+    result  = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "UdpTcPollingTask::handleSuccessfullTcRead: "
+                " Sending message to queue failed" << std::endl;
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        tcStore->deleteData(storeId);
+    }
+    return result;
+}
diff --git a/osal/common/TcpTmTcServer.h b/osal/common/TcpTmTcServer.h
index 4dcc77a2..6db6621d 100644
--- a/osal/common/TcpTmTcServer.h
+++ b/osal/common/TcpTmTcServer.h
@@ -2,7 +2,11 @@
 #define FSFW_OSAL_WINDOWS_TCWINTCPSERVER_H_
 
 #include "TcpIpBase.h"
+#include "../../ipc/messageQueueDefinitions.h"
+#include "../../ipc/MessageQueueIF.h"
+#include "../../objectmanager/frameworkObjects.h"
 #include "../../objectmanager/SystemObject.h"
+#include "../../storagemanager/StorageManagerIF.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
 #ifdef __unix__
@@ -12,8 +16,9 @@
 #include <string>
 #include <vector>
 
-//! Debugging preprocessor define.
-#define FSFW_TCP_RCV_WIRETAPPING_ENABLED    0
+class TcpTmTcBridge;
+//class SharedRingBuffer;
+
 
 /**
  * @brief   Windows TCP server used to receive telecommands on a Windows Host
@@ -28,26 +33,39 @@ public:
     /* The ports chosen here should not be used by any other process. */
     static const std::string DEFAULT_TCP_SERVER_PORT;
     static const std::string DEFAULT_TCP_CLIENT_PORT;
+    static constexpr  size_t ETHERNET_MTU_SIZE = 1500;
 
-    TcpTmTcServer(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
+    TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge /*, SharedRingBuffer* tcpRingBuffer*/,
+            size_t receptionBufferSize = ETHERNET_MTU_SIZE,
             std::string customTcpServerPort = "");
     virtual~ TcpTmTcServer();
 
     ReturnValue_t initialize() override;
     ReturnValue_t performOperation(uint8_t opCode) override;
+    ReturnValue_t initializeAfterTaskCreation() override;
+
+protected:
+    StorageManagerIF* tcStore = nullptr;
 
 private:
+    //! TMTC bridge is cached.
+    object_id_t tmtcBridgeId = objects::NO_OBJECT;
+    TcpTmTcBridge* tmtcBridge = nullptr;
 
     std::string tcpPort;
+    int tcpFlags = 0;
     socket_t listenerTcpSocket = 0;
     struct sockaddr tcpAddress;
+    MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
     int tcpAddrLen = sizeof(tcpAddress);
     int currentBacklog = 3;
 
     std::vector<uint8_t> receptionBuffer;
+    //SharedRingBuffer* tcpRingBuffer;
     int tcpSockOpt = 0;
 
-
+    void handleServerOperation(socket_t connSocket);
+    ReturnValue_t handleTcReception(size_t bytesRecvd);
 };
 
 #endif /* FSFW_OSAL_WINDOWS_TCWINTCPSERVER_H_ */
diff --git a/osal/common/UdpTcPollingTask.cpp b/osal/common/UdpTcPollingTask.cpp
index 47f67b29..d79e2447 100644
--- a/osal/common/UdpTcPollingTask.cpp
+++ b/osal/common/UdpTcPollingTask.cpp
@@ -18,9 +18,9 @@
 #define FSFW_UDP_RECV_WIRETAPPING_ENABLED    0
 
 UdpTcPollingTask::UdpTcPollingTask(object_id_t objectId,
-        object_id_t tmtcUnixUdpBridge, size_t maxRecvSize,
+        object_id_t tmtcUdpBridge, size_t maxRecvSize,
         double timeoutSeconds): SystemObject(objectId),
-        tmtcBridgeId(tmtcUnixUdpBridge) {
+        tmtcBridgeId(tmtcUdpBridge) {
     if(frameSize > 0) {
         this->frameSize = frameSize;
     }
diff --git a/osal/common/UdpTcPollingTask.h b/osal/common/UdpTcPollingTask.h
index 052eced5..cfe74b34 100644
--- a/osal/common/UdpTcPollingTask.h
+++ b/osal/common/UdpTcPollingTask.h
@@ -1,5 +1,5 @@
-#ifndef FSFW_OSAL_WINDOWS_TCSOCKETPOLLINGTASK_H_
-#define FSFW_OSAL_WINDOWS_TCSOCKETPOLLINGTASK_H_
+#ifndef FSFW_OSAL_COMMON_UDPTCPOLLINGTASK_H_
+#define FSFW_OSAL_COMMON_UDPTCPOLLINGTASK_H_
 
 #include "UdpTmTcBridge.h"
 #include "../../objectmanager/SystemObject.h"
@@ -22,7 +22,7 @@ public:
 	//! 0.5  default milliseconds timeout for now.
 	static constexpr timeval DEFAULT_TIMEOUT = {0, 500};
 
-	UdpTcPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
+	UdpTcPollingTask(object_id_t objectId, object_id_t tmtcUdpBridge,
 			size_t maxRecvSize = 0, double timeoutSeconds = -1);
 	virtual~ UdpTcPollingTask();
 
@@ -57,4 +57,4 @@ private:
 	ReturnValue_t handleSuccessfullTcRead(size_t bytesRead);
 };
 
-#endif /* FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_ */
+#endif /* FSFW_OSAL_COMMON_UDPTCPOLLINGTASK_H_ */
diff --git a/osal/common/UdpTmTcBridge.cpp b/osal/common/UdpTmTcBridge.cpp
index ba23f521..ccbb194e 100644
--- a/osal/common/UdpTmTcBridge.cpp
+++ b/osal/common/UdpTmTcBridge.cpp
@@ -16,7 +16,9 @@
 #endif
 
 //! Debugging preprocessor define.
+#ifndef FSFW_UDP_SEND_WIRETAPPING_ENABLED
 #define FSFW_UDP_SEND_WIRETAPPING_ENABLED    0
+#endif
 
 const std::string UdpTmTcBridge::DEFAULT_UDP_SERVER_PORT =  tcpip::DEFAULT_SERVER_PORT;
 
@@ -38,7 +40,7 @@ ReturnValue_t UdpTmTcBridge::initialize() {
     ReturnValue_t result = TmTcBridge::initialize();
     if(result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcUdpBridge::initialize: TmTcBridge initialization failed!"
+        sif::error << "UdpTmTcBridge::initialize: TmTcBridge initialization failed!"
                 << std::endl;
 #endif
         return result;
@@ -54,10 +56,10 @@ ReturnValue_t UdpTmTcBridge::initialize() {
         /* Tell the user that we could not find a usable */
         /* Winsock DLL.                                  */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcUdpBridge::TmTcUdpBridge: WSAStartup failed with error: " <<
+        sif::error << "UdpTmTcBridge::UdpTmTcBridge: WSAStartup failed with error: " <<
                 err << std::endl;
 #else
-        sif::printError("TmTcUdpBridge::TmTcUdpBridge: WSAStartup failed with error: %d\n",
+        sif::printError("UdpTmTcBridge::UdpTmTcBridge: WSAStartup failed with error: %d\n",
                 err);
 #endif
         return HasReturnvaluesIF::RETURN_FAILED;
@@ -78,19 +80,12 @@ ReturnValue_t UdpTmTcBridge::initialize() {
     getaddrinfo to assign the address 0.0.0.0 (any address) */
     int retval = getaddrinfo(nullptr, udpServerPort.c_str(), &hints, &addrResult);
     if (retval != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TmTcUdpBridge::TmTcUdpBridge: Retrieving address info failed!" <<
-                std::endl;
-#endif
+        tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::GETADDRINFO_CALL);
         return HasReturnvaluesIF::RETURN_FAILED;
     }
 
     serverSocket = socket(addrResult->ai_family, addrResult->ai_socktype, addrResult->ai_protocol);
     if(serverSocket == INVALID_SOCKET) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TmTcUdpBridge::TmTcUdpBridge: Could not open UDP socket!" <<
-                std::endl;
-#endif
         freeaddrinfo(addrResult);
         tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::SOCKET_CALL);
         return HasReturnvaluesIF::RETURN_FAILED;
@@ -102,10 +97,6 @@ ReturnValue_t UdpTmTcBridge::initialize() {
 
     retval = bind(serverSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
     if(retval != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcUdpBridge::TmTcUdpBridge: Could not bind "
-                "local port (" << udpServerPort << ") to server socket!" << std::endl;
-#endif
         freeaddrinfo(addrResult);
         tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::BIND_CALL);
         return HasReturnvaluesIF::RETURN_FAILED;
diff --git a/osal/common/UdpTmTcBridge.h b/osal/common/UdpTmTcBridge.h
index 8b8d1949..290f5eb0 100644
--- a/osal/common/UdpTmTcBridge.h
+++ b/osal/common/UdpTmTcBridge.h
@@ -1,5 +1,5 @@
-#ifndef FSFW_OSAL_WINDOWS_TMTCWINUDPBRIDGE_H_
-#define FSFW_OSAL_WINDOWS_TMTCWINUDPBRIDGE_H_
+#ifndef FSFW_OSAL_COMMON_TMTCUDPBRIDGE_H_
+#define FSFW_OSAL_COMMON_TMTCUDPBRIDGE_H_
 
 #include "TcpIpBase.h"
 #include "../../tmtcservices/TmTcBridge.h"
@@ -56,5 +56,5 @@ private:
     MutexIF* mutex;
 };
 
-#endif /* FSFW_OSAL_HOST_TMTCWINUDPBRIDGE_H_ */
+#endif /* FSFW_OSAL_COMMON_TMTCUDPBRIDGE_H_ */
 
diff --git a/osal/common/tcpipCommon.cpp b/osal/common/tcpipCommon.cpp
index 551e2a42..d4a3e8e6 100644
--- a/osal/common/tcpipCommon.cpp
+++ b/osal/common/tcpipCommon.cpp
@@ -35,6 +35,9 @@ void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std:
     else if(errorSrc == ErrorSources::GETADDRINFO_CALL) {
         srcString = "getaddrinfo call";
     }
+    else if(errorSrc == ErrorSources::SHUTDOWN_CALL) {
+        srcString = "shutdown call";
+    }
     else {
         srcString = "unknown call";
     }
diff --git a/osal/common/tcpipCommon.h b/osal/common/tcpipCommon.h
index 22b914dc..eb1bd910 100644
--- a/osal/common/tcpipCommon.h
+++ b/osal/common/tcpipCommon.h
@@ -29,7 +29,8 @@ enum class ErrorSources {
     RECVFROM_CALL,
     LISTEN_CALL,
     ACCEPT_CALL,
-    SENDTO_CALL
+    SENDTO_CALL,
+    SHUTDOWN_CALL
 };
 
 void determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string& protStr,

From c1d30aad13753591b57db9e549333a596b606947 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 5 May 2021 15:59:41 +0200
Subject: [PATCH 070/125] TCP server implementation finished

A lot of smaller tweaks and smaller refactoring done
in UDP TMTC bridge as well
---
 osal/common/TcpTmTcBridge.cpp  | 26 ++++++++--
 osal/common/TcpTmTcBridge.h    | 25 ++++++++--
 osal/common/TcpTmTcServer.cpp  | 86 ++++++++++++++++++++--------------
 osal/common/TcpTmTcServer.h    | 44 ++++++++++++-----
 osal/common/UdpTcPollingTask.h |  7 +--
 osal/common/UdpTmTcBridge.cpp  |  2 +-
 osal/common/UdpTmTcBridge.h    | 10 +++-
 osal/common/tcpipCommon.cpp    |  6 +++
 osal/common/tcpipCommon.h      |  1 +
 tmtcservices/TmTcBridge.cpp    |  7 ++-
 tmtcservices/TmTcBridge.h      |  3 +-
 11 files changed, 152 insertions(+), 65 deletions(-)

diff --git a/osal/common/TcpTmTcBridge.cpp b/osal/common/TcpTmTcBridge.cpp
index a88b68e9..24fab9a9 100644
--- a/osal/common/TcpTmTcBridge.cpp
+++ b/osal/common/TcpTmTcBridge.cpp
@@ -22,15 +22,18 @@ TcpTmTcBridge::TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
         object_id_t tmStoreId, object_id_t tcStoreId):
         TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
     mutex = MutexFactory::instance()->createMutex();
-    communicationLinkUp = false;
+    // Connection is always up, TM is requested by connecting to server and receiving packets
+    registerCommConnect();
 }
 
 ReturnValue_t TcpTmTcBridge::initialize() {
     ReturnValue_t result = TmTcBridge::initialize();
     if(result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcUdpBridge::initialize: TmTcBridge initialization failed!"
+        sif::error << "TcpTmTcBridge::initialize: TmTcBridge initialization failed!"
                 << std::endl;
+#else
+        sif::printError("TcpTmTcBridge::initialize: TmTcBridge initialization failed!\n");
 #endif
         return result;
     }
@@ -44,8 +47,25 @@ TcpTmTcBridge::~TcpTmTcBridge() {
     }
 }
 
-ReturnValue_t TcpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) {
+ReturnValue_t TcpTmTcBridge::handleTm() {
+    // Simply store the telemetry in the FIFO, the server will use it to access the TM
+    MutexGuard guard(mutex, timeoutType, mutexTimeoutMs);
+    TmTcMessage message;
+    ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
+    for (ReturnValue_t result = tmTcReceptionQueue->receiveMessage(&message);
+         result == HasReturnvaluesIF::RETURN_OK;
+         result = tmTcReceptionQueue->receiveMessage(&message))
+    {
+        status = storeDownlinkData(&message);
+        if(status != HasReturnvaluesIF::RETURN_OK) {
+            break;
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
 
+ReturnValue_t TcpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) {
+    // Not used. The Server uses the FIFO to access and send the telemetry.
     return HasReturnvaluesIF::RETURN_OK;
 }
 
diff --git a/osal/common/TcpTmTcBridge.h b/osal/common/TcpTmTcBridge.h
index db45fca8..6cfacb9f 100644
--- a/osal/common/TcpTmTcBridge.h
+++ b/osal/common/TcpTmTcBridge.h
@@ -17,16 +17,30 @@
 #include <string>
 
 /**
- * @brief   This class should be used with the UdpTcPollingTask to implement a UDP server
- *          for receiving and sending PUS TMTC.
+ * @brief   This class should be used with the TcpTmTcServer to implement a TCP server
+ *          for receiving and sending PUS telemetry and telecommands (TMTC)
+ * @details
+ * This bridge tasks takes care of filling a FIFO which generated telemetry. The TcpTmTcServer
+ * will take care of sending the telemetry stored in the FIFO if a client connects to the
+ * server. This bridge will also be the default destination for telecommands, but the telecommands
+ * will be relayed to a specified tcDestination directly.
  */
 class TcpTmTcBridge:
         public TmTcBridge {
-    //friend class UdpTcPollingTask;
+    friend class TcpTmTcServer;
 public:
     /* The ports chosen here should not be used by any other process. */
     static const std::string DEFAULT_UDP_SERVER_PORT;
 
+    /**
+     * Constructor
+     * @param objectId  Object ID of the TcpTmTcBridge.
+     * @param tcDestination Destination for received TC packets. Any received telecommands will
+     *                      be sent there directly. The destination object needs to implement
+     *                      AcceptsTelecommandsIF.
+     * @param tmStoreId TM store object ID. It is recommended to the default object ID
+     * @param tcStoreId TC store object ID. It is recommended to the default object ID
+     */
     TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
             object_id_t tmStoreId = objects::TM_STORE,
             object_id_t tcStoreId = objects::TC_STORE);
@@ -39,12 +53,15 @@ public:
 
     ReturnValue_t initialize() override;
 
+
 protected:
+    ReturnValue_t handleTm() override;
     virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override;
 
 private:
 
-    //! Access to the client address is mutex protected as it is set by another task.
+    //! Access to the FIFO needs to be mutex protected because it is used by the bridge and
+    //! the server.
     MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
     dur_millis_t mutexTimeoutMs = 20;
     MutexIF* mutex;
diff --git a/osal/common/TcpTmTcServer.cpp b/osal/common/TcpTmTcServer.cpp
index 8717783e..25403e8b 100644
--- a/osal/common/TcpTmTcServer.cpp
+++ b/osal/common/TcpTmTcServer.cpp
@@ -4,6 +4,7 @@
 
 #include "../../container/SharedRingBuffer.h"
 #include "../../ipc/MessageQueueSenderIF.h"
+#include "../../ipc/MutexGuard.h"
 #include "../../objectmanager/ObjectManagerIF.h"
 #include "../../serviceinterface/ServiceInterface.h"
 #include "../../tmtcservices/TmTcMessage.h"
@@ -25,10 +26,9 @@
 const std::string TcpTmTcServer::DEFAULT_TCP_SERVER_PORT =  "7301";
 
 TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
-        /*SharedRingBuffer* tcpRingBuffer, */ size_t receptionBufferSize,
-        std::string customTcpServerPort):
-                        SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), tcpPort(customTcpServerPort),
-                        receptionBuffer(receptionBufferSize) /*, tcpRingBuffer(tcpRingBuffer) */ {
+        size_t receptionBufferSize, std::string customTcpServerPort):
+        SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge),
+        tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) {
     if(tcpPort == "") {
         tcpPort = DEFAULT_TCP_SERVER_PORT;
     }
@@ -37,17 +37,6 @@ TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
 ReturnValue_t TcpTmTcServer::initialize() {
     using namespace tcpip;
 
-    /*
-    if(tcpRingBuffer == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TcpTmTcServer::initialize: Invalid ring buffer!" << std::endl;
-#else
-        sif::printError("TcpTmTcServer::initialize: Invalid ring buffer!\n");
-#endif
-        return ObjectManagerIF::CHILD_INIT_FAILED;
-    }
-    */
-
     ReturnValue_t result = TcpIpBase::initialize();
     if(result != HasReturnvaluesIF::RETURN_OK) {
         return result;
@@ -74,13 +63,14 @@ ReturnValue_t TcpTmTcServer::initialize() {
     hints.ai_protocol = IPPROTO_TCP;
     hints.ai_flags = AI_PASSIVE;
 
+    // Listen to all addresses (0.0.0.0) by using AI_PASSIVE in the hint flags
     retval = getaddrinfo(nullptr, tcpPort.c_str(), &hints, &addrResult);
     if (retval != 0) {
         handleError(Protocol::TCP, ErrorSources::GETADDRINFO_CALL);
         return HasReturnvaluesIF::RETURN_FAILED;
     }
 
-    /* Open TCP (stream) socket */
+    // Open TCP (stream) socket
     listenerTcpSocket = socket(addrResult->ai_family, addrResult->ai_socktype,
             addrResult->ai_protocol);
     if(listenerTcpSocket == INVALID_SOCKET) {
@@ -89,6 +79,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
         return HasReturnvaluesIF::RETURN_FAILED;
     }
 
+    // Bind to the address found by getaddrinfo
     retval = bind(listenerTcpSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
     if(retval == SOCKET_ERROR) {
         freeaddrinfo(addrResult);
@@ -107,15 +98,15 @@ TcpTmTcServer::~TcpTmTcServer() {
 
 ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
     using namespace tcpip;
-    /* If a connection is accepted, the corresponding socket will be assigned to the new socket */
+    // If a connection is accepted, the corresponding socket will be assigned to the new socket
     socket_t connSocket = 0;
     sockaddr clientSockAddr = {};
     socklen_t connectorSockAddrLen = 0;
     int retval = 0;
 
-    /* Listen for connection requests permanently for lifetime of program */
+    // Listen for connection requests permanently for lifetime of program
     while(true) {
-        retval = listen(listenerTcpSocket, currentBacklog);
+        retval = listen(listenerTcpSocket, tcpBacklog);
         if(retval == SOCKET_ERROR) {
             handleError(Protocol::TCP, ErrorSources::LISTEN_CALL, 500);
             continue;
@@ -131,7 +122,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
 
         handleServerOperation(connSocket);
 
-        /* Done, shut down connection */
+        // Done, shut down connection and go back to listening for client requests
         retval = shutdown(connSocket, SHUT_SEND);
         if(retval != 0) {
             handleError(Protocol::TCP, ErrorSources::SHUTDOWN_CALL);
@@ -142,29 +133,21 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
 }
 
 ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() {
+    if(tmtcBridge == nullptr) {
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
     /* Initialize the destination after task creation. This ensures
     that the destination has already been set in the TMTC bridge. */
     targetTcDestination = tmtcBridge->getRequestQueue();
-
-//
-//    if(tcpRingBuffer != nullptr) {
-//        auto fifoCheck = tcpRingBuffer->getReceiveSizesFIFO();
-//        if (fifoCheck == nullptr) {
-//#if FSFW_CPP_OSTREAM_ENABLED == 1
-//            sif::error << "TcpTmTcServer::initializeAfterTaskCreation: "
-//                    "TCP ring buffer does not have a FIFO!" << std::endl;
-//#else
-//            sif::printError("TcpTmTcServer::initialize: TCP ring buffer does not have a FIFO!\n");
-//#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
-//        }
-//    }
-
+    tcStore = tmtcBridge->tcStore;
+    tmStore = tmtcBridge->tmStore;
     return HasReturnvaluesIF::RETURN_OK;
 }
 
 void TcpTmTcServer::handleServerOperation(socket_t connSocket) {
     int retval = 0;
     do {
+        // Read all telecommands sent by the client
         retval = recv(connSocket,
                 reinterpret_cast<char*>(receptionBuffer.data()),
                 receptionBuffer.capacity(),
@@ -173,10 +156,12 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) {
             handleTcReception(retval);
         }
         else if(retval == 0) {
-            /* Client has finished sending telecommands, send telemetry now */
+            // Client has finished sending telecommands, send telemetry now
+            handleTmSending(connSocket);
         }
         else {
-            /* Should not happen */
+            // Should not happen
+            tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL);
         }
     } while(retval > 0);
 }
@@ -210,3 +195,32 @@ ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
     }
     return result;
 }
+
+void TcpTmTcServer::setTcpBacklog(uint8_t tcpBacklog) {
+    this->tcpBacklog = tcpBacklog;
+}
+
+ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) {
+    // Access to the FIFO is mutex protected because it is filled by the bridge
+    MutexGuard(tmtcBridge->mutex, tmtcBridge->timeoutType, tmtcBridge->mutexTimeoutMs);
+    store_address_t storeId;
+    while((not tmtcBridge->tmFifo->empty()) and
+            (tmtcBridge->packetSentCounter < tmtcBridge->sentPacketsPerCycle)) {
+        tmtcBridge->tmFifo->retrieve(&storeId);
+
+        // Using the store accessor will take care of deleting TM from the store automatically
+        ConstStorageAccessor storeAccessor(storeId);
+        ReturnValue_t result = tmStore->getData(storeId, storeAccessor);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+        int retval = send(connSocket,
+                reinterpret_cast<const char*>(storeAccessor.data()),
+                storeAccessor.size(),
+                tcpTmFlags);
+        if(retval != static_cast<int>(storeAccessor.size())) {
+            tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL);
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
diff --git a/osal/common/TcpTmTcServer.h b/osal/common/TcpTmTcServer.h
index 6db6621d..e4328370 100644
--- a/osal/common/TcpTmTcServer.h
+++ b/osal/common/TcpTmTcServer.h
@@ -1,5 +1,5 @@
-#ifndef FSFW_OSAL_WINDOWS_TCWINTCPSERVER_H_
-#define FSFW_OSAL_WINDOWS_TCWINTCPSERVER_H_
+#ifndef FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_
+#define FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_
 
 #include "TcpIpBase.h"
 #include "../../ipc/messageQueueDefinitions.h"
@@ -17,13 +17,22 @@
 #include <vector>
 
 class TcpTmTcBridge;
-//class SharedRingBuffer;
-
 
 /**
- * @brief   Windows TCP server used to receive telecommands on a Windows Host
+ * @brief   TCP server implementation
  * @details
- * Based on: https://docs.microsoft.com/en-us/windows/win32/winsock/complete-server-code
+ * This server will run for the whole program lifetime and will take care of serving client
+ * requests on a specified TCP server port. This server waas written in a generic way and
+ * can be used on Unix and on Windows systems.
+ *
+ * If a connection is accepted, the server will read all telecommands sent by a client and then
+ * send all telemetry currently found in the TMTC bridge FIFO.
+ *
+ * Reading telemetry without sending telecommands is possible by connecting, shutting down the
+ * send operation immediately and then reading the telemetry. It is therefore recommended to
+ * connect to the server regularly, even if no telecommands need to be sent.
+ *
+ * The server will listen to a specific port on all addresses (0.0.0.0).
  */
 class TcpTmTcServer:
         public SystemObject,
@@ -35,18 +44,28 @@ public:
     static const std::string DEFAULT_TCP_CLIENT_PORT;
     static constexpr  size_t ETHERNET_MTU_SIZE = 1500;
 
-    TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge /*, SharedRingBuffer* tcpRingBuffer*/,
-            size_t receptionBufferSize = ETHERNET_MTU_SIZE,
+    /**
+     * TCP Server Constructor
+     * @param objectId              Object ID of the TCP Server
+     * @param tmtcTcpBridge         Object ID of the TCP TMTC Bridge object
+     * @param receptionBufferSize   This will be the size of the reception buffer. Default buffer
+     *                              size will be the Ethernet MTU size
+     * @param customTcpServerPort   The user can specify another port than the default (7301) here.
+     */
+    TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
+            size_t receptionBufferSize = ETHERNET_MTU_SIZE + 1,
             std::string customTcpServerPort = "");
     virtual~ TcpTmTcServer();
 
+    void setTcpBacklog(uint8_t tcpBacklog);
+
     ReturnValue_t initialize() override;
     ReturnValue_t performOperation(uint8_t opCode) override;
     ReturnValue_t initializeAfterTaskCreation() override;
 
 protected:
     StorageManagerIF* tcStore = nullptr;
-
+    StorageManagerIF* tmStore = nullptr;
 private:
     //! TMTC bridge is cached.
     object_id_t tmtcBridgeId = objects::NO_OBJECT;
@@ -58,14 +77,15 @@ private:
     struct sockaddr tcpAddress;
     MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
     int tcpAddrLen = sizeof(tcpAddress);
-    int currentBacklog = 3;
+    int tcpBacklog = 3;
 
     std::vector<uint8_t> receptionBuffer;
-    //SharedRingBuffer* tcpRingBuffer;
     int tcpSockOpt = 0;
+    int tcpTmFlags = 0;
 
     void handleServerOperation(socket_t connSocket);
     ReturnValue_t handleTcReception(size_t bytesRecvd);
+    ReturnValue_t handleTmSending(socket_t connSocket);
 };
 
-#endif /* FSFW_OSAL_WINDOWS_TCWINTCPSERVER_H_ */
+#endif /* FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ */
diff --git a/osal/common/UdpTcPollingTask.h b/osal/common/UdpTcPollingTask.h
index cfe74b34..9a680fb8 100644
--- a/osal/common/UdpTcPollingTask.h
+++ b/osal/common/UdpTcPollingTask.h
@@ -9,8 +9,11 @@
 #include <vector>
 
 /**
- * @brief   This class should be used with the UdpTmTcBridge to implement a UDP server
+ * @brief   This class can be used with the UdpTmTcBridge to implement a UDP server
  *          for receiving and sending PUS TMTC.
+ * @details
+ * This task is exclusively used to poll telecommands from a given socket and transfer them
+ * to the FSFW software bus. It used the blocking recvfrom call to do this.
  */
 class UdpTcPollingTask:
         public TcpIpBase,
@@ -45,8 +48,6 @@ private:
 	object_id_t tmtcBridgeId = objects::NO_OBJECT;
 	UdpTmTcBridge* tmtcBridge = nullptr;
 	MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
-
-	//! See: https://docs.microsoft.com/en-us/windows/win32/api/winsock/nf-winsock-recvfrom
 	int receptionFlags = 0;
 
 	std::vector<uint8_t> receptionBuffer;
diff --git a/osal/common/UdpTmTcBridge.cpp b/osal/common/UdpTmTcBridge.cpp
index ccbb194e..b07ca7a5 100644
--- a/osal/common/UdpTmTcBridge.cpp
+++ b/osal/common/UdpTmTcBridge.cpp
@@ -23,7 +23,7 @@
 const std::string UdpTmTcBridge::DEFAULT_UDP_SERVER_PORT =  tcpip::DEFAULT_SERVER_PORT;
 
 UdpTmTcBridge::UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
-        object_id_t tmStoreId, object_id_t tcStoreId, std::string udpServerPort):
+        std::string udpServerPort, object_id_t tmStoreId, object_id_t tcStoreId):
         TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
     if(udpServerPort == "") {
         this->udpServerPort = DEFAULT_UDP_SERVER_PORT;
diff --git a/osal/common/UdpTmTcBridge.h b/osal/common/UdpTmTcBridge.h
index 290f5eb0..74084a9d 100644
--- a/osal/common/UdpTmTcBridge.h
+++ b/osal/common/UdpTmTcBridge.h
@@ -17,8 +17,13 @@
 #include <string>
 
 /**
- * @brief   This class should be used with the UdpTcPollingTask to implement a UDP server
+ * @brief   This class can be used with the UdpTcPollingTask to implement a UDP server
  *          for receiving and sending PUS TMTC.
+ * @details
+ * This bridge task will take care of sending telemetry back to a UDP client if a connection
+ * was established and store them in a FIFO if this was not done yet. It is also be the default
+ * destination for telecommands, but the telecommands will be relayed to a specified tcDestination
+ * directly.
  */
 class UdpTmTcBridge:
         public TmTcBridge,
@@ -29,7 +34,8 @@ public:
     static const std::string DEFAULT_UDP_SERVER_PORT;
 
     UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
-            object_id_t tmStoreId, object_id_t tcStoreId, std::string udpServerPort = "");
+            std::string udpServerPort = "", object_id_t tmStoreId = objects::TM_STORE,
+            object_id_t tcStoreId = objects::TC_STORE);
     virtual~ UdpTmTcBridge();
 
     /**
diff --git a/osal/common/tcpipCommon.cpp b/osal/common/tcpipCommon.cpp
index d4a3e8e6..ca4dbf18 100644
--- a/osal/common/tcpipCommon.cpp
+++ b/osal/common/tcpipCommon.cpp
@@ -32,6 +32,12 @@ void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std:
     else if(errorSrc == ErrorSources::RECVFROM_CALL) {
         srcString = "recvfrom call";
     }
+    else if(errorSrc == ErrorSources::SEND_CALL) {
+        srcString = "send call";
+    }
+    else if(errorSrc == ErrorSources::SENDTO_CALL) {
+        srcString = "sendto call";
+    }
     else if(errorSrc == ErrorSources::GETADDRINFO_CALL) {
         srcString = "getaddrinfo call";
     }
diff --git a/osal/common/tcpipCommon.h b/osal/common/tcpipCommon.h
index eb1bd910..ce7a90cd 100644
--- a/osal/common/tcpipCommon.h
+++ b/osal/common/tcpipCommon.h
@@ -29,6 +29,7 @@ enum class ErrorSources {
     RECVFROM_CALL,
     LISTEN_CALL,
     ACCEPT_CALL,
+    SEND_CALL,
     SENDTO_CALL,
     SHUTDOWN_CALL
 };
diff --git a/tmtcservices/TmTcBridge.cpp b/tmtcservices/TmTcBridge.cpp
index dcffac41..1257ef89 100644
--- a/tmtcservices/TmTcBridge.cpp
+++ b/tmtcservices/TmTcBridge.cpp
@@ -183,8 +183,11 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage *message) {
 
 	if(tmFifo->full()) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	    sif::debug << "TmTcBridge::storeDownlinkData: TM downlink max. number "
-	                    << "of stored packet IDs reached! " << std::endl;
+	    sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number "
+	            "of stored packet IDs reached!" << std::endl;
+#else
+	    sif::printWarning("TmTcBridge::storeDownlinkData: TM downlink max. number "
+                "of stored packet IDs reached!\n");
 #endif
 	    if(overwriteOld) {
 	        tmFifo->retrieve(&storeId);
diff --git a/tmtcservices/TmTcBridge.h b/tmtcservices/TmTcBridge.h
index 0177648c..d3e1c547 100644
--- a/tmtcservices/TmTcBridge.h
+++ b/tmtcservices/TmTcBridge.h
@@ -150,8 +150,7 @@ protected:
 	void printData(uint8_t * data, size_t dataLen);
 
 	/**
-	 * This fifo can be used to store downlink data
-	 * which can not be sent at the moment.
+	 * This FIFO can be used to store downlink data which can not be sent at the moment.
 	 */
 	DynamicFIFO<store_address_t>* tmFifo = nullptr;
     uint8_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE;

From 8b17c40aa61537a70ce947b1db8bf338dea2ae16 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 11 May 2021 15:02:04 +0200
Subject: [PATCH 071/125] Added Network Byte Order

---
 serialize/SerializeIF.h | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/serialize/SerializeIF.h b/serialize/SerializeIF.h
index d72218f0..dfd854e3 100644
--- a/serialize/SerializeIF.h
+++ b/serialize/SerializeIF.h
@@ -19,7 +19,10 @@
 class SerializeIF {
 public:
 	enum class Endianness : uint8_t {
-		BIG, LITTLE, MACHINE
+		BIG,
+		LITTLE,
+		MACHINE,
+		NETWORK = BIG // Added for convenience like htons on sockets
 	};
 
 	static const uint8_t INTERFACE_ID = CLASS_ID::SERIALIZE_IF;

From d807998f4de768be499aa3a48f9bf2d577057667 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 11 May 2021 15:25:38 +0200
Subject: [PATCH 072/125] Added Missing includes in Host Osal

Updated correct defaults for Host MessageQueues
---
 osal/host/MessageQueue.cpp | 2 ++
 osal/host/MessageQueue.h   | 6 +++---
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index 41c55a3d..4e286271 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -5,6 +5,8 @@
 #include "../../ipc/MutexFactory.h"
 #include "../../ipc/MutexGuard.h"
 
+#include <cstring>
+
 MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
 		messageSize(maxMessageSize), messageDepth(messageDepth) {
 	queueLock = MutexFactory::instance()->createMutex();
diff --git a/osal/host/MessageQueue.h b/osal/host/MessageQueue.h
index e965123d..1c9b5e33 100644
--- a/osal/host/MessageQueue.h
+++ b/osal/host/MessageQueue.h
@@ -217,15 +217,15 @@ private:
 	 * @brief	The class stores the queue id it got assigned.
 	 * 			If initialization fails, the queue id is set to zero.
 	 */
-	MessageQueueId_t mqId = 0;
+	MessageQueueId_t mqId = MessageQueueIF::NO_QUEUE;
 	size_t messageSize = 0;
 	size_t messageDepth = 0;
 
 	MutexIF* queueLock;
 
 	bool defaultDestinationSet = false;
-	MessageQueueId_t defaultDestination = 0;
-	MessageQueueId_t lastPartner = 0;
+	MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE;
+	MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE;
 };
 
 #endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */

From 9d0155d9ae58425bf982cd4712be3b7d83efc87b Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 11 May 2021 15:30:49 +0200
Subject: [PATCH 073/125] Added a returnvalue

---
 osal/host/MessageQueue.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index 4e286271..a779bdcb 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -128,8 +128,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 				internalErrorReporter->queueMessageNotSent();
 			}
 		}
-		// TODO: Better returnvalue
-		return HasReturnvaluesIF::RETURN_FAILED;
+		return MessageQueueIF::DESTINATION_INVALID;
 	}
 	if(targetQueue->messageQueue.size() < targetQueue->messageDepth) {
 		MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);

From e1c91f82b763b0d57710a16ab40697df87e6ce74 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 10:54:10 +0200
Subject: [PATCH 074/125] some fixes and tweaks

---
 events/EventManager.cpp             | 48 +++++++++++++++++++----------
 objectmanager/ObjectManagerIF.h     |  2 +-
 serviceinterface/ServiceInterface.h |  4 +--
 3 files changed, 35 insertions(+), 19 deletions(-)

diff --git a/events/EventManager.cpp b/events/EventManager.cpp
index 5b2b31b5..2d4b6a49 100644
--- a/events/EventManager.cpp
+++ b/events/EventManager.cpp
@@ -2,7 +2,7 @@
 #include "EventMessage.h"
 
 #include <FSFWConfig.h>
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../ipc/QueueFactory.h"
 #include "../ipc/MutexFactory.h"
 
@@ -120,23 +120,39 @@ ReturnValue_t EventManager::unsubscribeFromEventRange(MessageQueueId_t listener,
 void EventManager::printEvent(EventMessage* message) {
 	const char *string = 0;
 	switch (message->getSeverity()) {
-	case severity::INFO:
-#if DEBUG_INFO_EVENT == 1
-		string = translateObject(message->getReporter());
+	case severity::INFO: {
+#if FSFW_DEBUG_INFO == 1
+        string = translateObject(message->getReporter());
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::info << "EVENT: ";
-		if (string != 0) {
-			sif::info << string;
-		} else {
-			sif::info << "0x" << std::hex << message->getReporter() << std::dec;
-		}
-		sif::info << " reported " << translateEvents(message->getEvent()) << " ("
-				<< std::dec << message->getEventId() << std::hex << ") P1: 0x"
-				<< message->getParameter1() << " P2: 0x"
-				<< message->getParameter2() << std::dec << std::endl;
-#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+        sif::info << "EVENT: ";
+        if (string != 0) {
+            sif::info << string;
+        }
+        else {
+            sif::info << "0x" << std::hex << std::setfill('0') << std::setw(8) <<
+                    message->getReporter() << std::setfill(' ') << std::dec;
+        }
+        sif::info << " reported " << translateEvents(message->getEvent()) << " ("
+                << std::dec << message->getEventId() << std::hex << ") P1: 0x"
+                << message->getParameter1() << " P2: 0x"
+                << message->getParameter2() << std::dec << std::endl;
+#else
+        const char totalString[140] = {};
+        if (string != 0) {
+            snprintf((char*) totalString, sizeof(totalString),"Event: %s", string);
+        }
+        else {
+            snprintf((char*) totalString, sizeof(totalString),"Event: 0x%08x",
+                    message->getReporter());
+        }
+        snprintf((char*) totalString, sizeof(totalString),
+                " reported %s | ID %d | P1: 0x%x | P2: 0x%x\n", translateEvents(message->getEvent()),
+                message->getEventId(), message->getParameter1(), message->getParameter2());
+        sif::printInfo("%s", totalString);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
 #endif /* DEBUG_INFO_EVENT == 1 */
-		break;
+        break;
+	}
 	default:
 		string = translateObject(message->getReporter());
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/objectmanager/ObjectManagerIF.h b/objectmanager/ObjectManagerIF.h
index 8bebb609..61e6f423 100644
--- a/objectmanager/ObjectManagerIF.h
+++ b/objectmanager/ObjectManagerIF.h
@@ -4,7 +4,7 @@
 #include "frameworkObjects.h"
 #include "SystemObjectIF.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 
 /**
  * @brief	This class provides an interface to the global object manager.
diff --git a/serviceinterface/ServiceInterface.h b/serviceinterface/ServiceInterface.h
index 1f7e5e84..e95dd9a4 100644
--- a/serviceinterface/ServiceInterface.h
+++ b/serviceinterface/ServiceInterface.h
@@ -5,9 +5,9 @@
 #include "serviceInterfaceDefintions.h"
 
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#include "ServiceInterfaceStream.h"
 #else
-#include <fsfw/serviceinterface/ServiceInterfacePrinter.h>
+#include "ServiceInterfacePrinter.h"
 #endif
 
 #endif /* FSFW_SERVICEINTERFACE_SERVICEINTERFACE_H_ */

From cd016c8281598140708952e44c7823f71e1e63cf Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 13:40:26 +0200
Subject: [PATCH 075/125] event manager printout refactoring

---
 events/EventManager.cpp | 150 +++++++++++++++++++++++-----------------
 events/EventManager.h   |   2 +
 2 files changed, 87 insertions(+), 65 deletions(-)

diff --git a/events/EventManager.cpp b/events/EventManager.cpp
index 2d4b6a49..8e8f797f 100644
--- a/events/EventManager.cpp
+++ b/events/EventManager.cpp
@@ -1,8 +1,6 @@
 #include "EventManager.h"
 #include "EventMessage.h"
 
-#include <FSFWConfig.h>
-#include "../serviceinterface/ServiceInterface.h"
 #include "../ipc/QueueFactory.h"
 #include "../ipc/MutexFactory.h"
 
@@ -115,69 +113,6 @@ ReturnValue_t EventManager::unsubscribeFromEventRange(MessageQueueId_t listener,
 	return result;
 }
 
-#if FSFW_OBJ_EVENT_TRANSLATION == 1
-
-void EventManager::printEvent(EventMessage* message) {
-	const char *string = 0;
-	switch (message->getSeverity()) {
-	case severity::INFO: {
-#if FSFW_DEBUG_INFO == 1
-        string = translateObject(message->getReporter());
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::info << "EVENT: ";
-        if (string != 0) {
-            sif::info << string;
-        }
-        else {
-            sif::info << "0x" << std::hex << std::setfill('0') << std::setw(8) <<
-                    message->getReporter() << std::setfill(' ') << std::dec;
-        }
-        sif::info << " reported " << translateEvents(message->getEvent()) << " ("
-                << std::dec << message->getEventId() << std::hex << ") P1: 0x"
-                << message->getParameter1() << " P2: 0x"
-                << message->getParameter2() << std::dec << std::endl;
-#else
-        const char totalString[140] = {};
-        if (string != 0) {
-            snprintf((char*) totalString, sizeof(totalString),"Event: %s", string);
-        }
-        else {
-            snprintf((char*) totalString, sizeof(totalString),"Event: 0x%08x",
-                    message->getReporter());
-        }
-        snprintf((char*) totalString, sizeof(totalString),
-                " reported %s | ID %d | P1: 0x%x | P2: 0x%x\n", translateEvents(message->getEvent()),
-                message->getEventId(), message->getParameter1(), message->getParameter2());
-        sif::printInfo("%s", totalString);
-#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
-#endif /* DEBUG_INFO_EVENT == 1 */
-        break;
-	}
-	default:
-		string = translateObject(message->getReporter());
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "EventManager: ";
-		if (string != 0) {
-			sif::debug << string;
-		}
-		else {
-			sif::debug << "0x" << std::hex << message->getReporter() << std::dec;
-		}
-		sif::debug << " reported " << translateEvents(message->getEvent())
-				<< " (" << std::dec << message->getEventId() << ") "
-				<< std::endl;
-		sif::debug << std::hex << "P1 Hex: 0x" << message->getParameter1()
-				<< ", P1 Dec: " << std::dec << message->getParameter1()
-				<< std::endl;
-		sif::debug << std::hex << "P2 Hex: 0x" << message->getParameter2()
-				<< ", P2 Dec: " <<  std::dec << message->getParameter2()
-				<< std::endl;
-#endif
-		break;
-	}
-}
-#endif
-
 void EventManager::lockMutex() {
 	mutex->lockMutex(timeoutType, timeoutMs);
 }
@@ -191,3 +126,88 @@ void EventManager::setMutexTimeout(MutexIF::TimeoutType timeoutType,
 	this->timeoutType = timeoutType;
 	this->timeoutMs = timeoutMs;
 }
+
+#if FSFW_OBJ_EVENT_TRANSLATION == 1
+
+void EventManager::printEvent(EventMessage* message) {
+    switch (message->getSeverity()) {
+    case severity::INFO: {
+#if FSFW_DEBUG_INFO == 1
+        printUtility(sif::OutputTypes::OUT_INFO, message);
+#endif /* DEBUG_INFO_EVENT == 1 */
+        break;
+    }
+    default:
+        printUtility(sif::OutputTypes::OUT_DEBUG, message);
+        break;
+    }
+}
+
+void EventManager::printUtility(sif::OutputTypes printType, EventMessage *message) {
+    const char *string = 0;
+    if(printType == sif::OutputTypes::OUT_INFO) {
+        string = translateObject(message->getReporter());
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::info << "EventManager: ";
+        if (string != 0) {
+            sif::info << string;
+        }
+        else {
+            sif::info << "0x" << std::hex << std::setw(8) << std::setfill('0') <<
+                    message->getReporter() << std::setfill(' ') << std::dec;
+        }
+        sif::info << " reported " << translateEvents(message->getEvent())
+                        << " with event ID " << std::dec << message->getEventId() << std::endl;
+        sif::info << std::hex << "P1 Hex: 0x" << message->getParameter1() <<
+                " | P1 Dec: " << std::dec << message->getParameter1() <<  std::hex <<
+                " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<  std::dec <<
+                message->getParameter2() << std::endl;
+#else
+        if (string != 0) {
+            sif::printInfo("Event Manager: %s reported %s with event ID %d\n",
+                    message->getReporter(), translateEvents(message->getEvent()),
+                    message->getEventId());
+        }
+        else {
+            sif::printInfo("Event Manager: Reporter ID 0x%08x reported %s with event ID %d\n",
+                    string, translateEvents(message->getEvent()), message->getEventId());
+        }
+        sif::printInfo("P1 Hex: 0x%x | P1 Dec: %d | P2 Hex: 0x%x | P2 Dec: %d\n",
+                message->getParameter1(), message->getParameter1(),
+                message->getParameter2(), message->getParameter2());
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
+    }
+    else {
+        string = translateObject(message->getReporter());
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::debug << "EventManager: ";
+        if (string != 0) {
+            sif::debug << string;
+        }
+        else {
+            sif::debug << "0x" << std::hex << message->getReporter() << std::dec;
+        }
+        sif::debug << " reported " << translateEvents(message->getEvent())
+                        << " with event ID " << std::dec << message->getEventId() << std::endl;
+        sif::debug << std::hex << "P1 Hex: 0x" << message->getParameter1() <<
+                " | P1 Dec: " << std::dec << message->getParameter1() <<  std::hex <<
+                " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<  std::dec <<
+                message->getParameter2() << std::endl;
+#else
+        if (string != 0) {
+            sif::printDebug("Event Manager: %s reported %s with event ID %d\n",
+                    message->getReporter(), translateEvents(message->getEvent()),
+                    message->getEventId());
+        }
+        else {
+            sif::printDebug("Event Manager: Reporter ID 0x%08x reported %s with event ID %d\n",
+                    string, translateEvents(message->getEvent()), message->getEventId());
+        }
+        sif::printDebug("P1 Hex: 0x%x | P1 Dec: %d | P2 Hex: 0x%x | P2 Dec: %d\n",
+                message->getParameter1(), message->getParameter1(),
+                message->getParameter2(), message->getParameter2());
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
+    }
+}
+
+#endif /* FSFW_OBJ_EVENT_TRANSLATION == 1 */
diff --git a/events/EventManager.h b/events/EventManager.h
index abce9b8b..9189d9e7 100644
--- a/events/EventManager.h
+++ b/events/EventManager.h
@@ -6,6 +6,7 @@
 
 #include <FSFWConfig.h>
 
+#include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/SystemObject.h"
 #include "../storagemanager/LocalPool.h"
 #include "../tasks/ExecutableObjectIF.h"
@@ -67,6 +68,7 @@ protected:
 
 #if FSFW_OBJ_EVENT_TRANSLATION == 1
 	void printEvent(EventMessage *message);
+	void printUtility(sif::OutputTypes printType, EventMessage* message);
 #endif
 
 	void lockMutex();

From 5b23b928cf9b187eb19b18f366a9fecfaff21c31 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 14:28:37 +0200
Subject: [PATCH 076/125] added missing include

---
 osal/host/MessageQueue.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index 41c55a3d..4e286271 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -5,6 +5,8 @@
 #include "../../ipc/MutexFactory.h"
 #include "../../ipc/MutexGuard.h"
 
+#include <cstring>
+
 MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
 		messageSize(maxMessageSize), messageDepth(messageDepth) {
 	queueLock = MutexFactory::instance()->createMutex();

From d81c6f40fbc86ac9fd0123d08b5fdba09bad9d00 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 14:43:08 +0200
Subject: [PATCH 077/125] define fixes

---
 osal/host/Clock.cpp | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp
index c097f619..bcf486ec 100644
--- a/osal/host/Clock.cpp
+++ b/osal/host/Clock.cpp
@@ -2,9 +2,9 @@
 #include "../../timemanager/Clock.h"
 
 #include <chrono>
-#if defined(WIN32)
+#if defined(_WIN32)
 #include <sysinfoapi.h>
-#elif defined(LINUX)
+#elif defined(__unix__)
 #include <fstream>
 #endif
 
@@ -92,7 +92,7 @@ timeval Clock::getUptime() {
 	auto fraction = uptime - secondsChrono;
 	timeval.tv_usec = std::chrono::duration_cast<std::chrono::microseconds>(
 	            fraction).count();
-#elif defined(LINUX)
+#elif defined(__unix__)
 	double uptimeSeconds;
 	if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds)
 	{
@@ -120,7 +120,6 @@ ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) {
 }
 
 
-ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 	/* Do some magic with chrono (C++20!) */
 	/* Right now, the library doesn't have the new features to get the required values yet.
 	so we work around that for now. */

From 8293e6b0c742e18ddd78d4db05ac64fe58f09c60 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 14:48:39 +0200
Subject: [PATCH 078/125] more include fixes

---
 osal/host/Clock.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp
index bcf486ec..cf9349c1 100644
--- a/osal/host/Clock.cpp
+++ b/osal/host/Clock.cpp
@@ -46,7 +46,7 @@ ReturnValue_t Clock::setClock(const timeval* time) {
 }
 
 ReturnValue_t Clock::getClock_timeval(timeval* time) {
-#if defined(WIN32)
+#if defined(_WIN32)
 	auto now  = std::chrono::system_clock::now();
 	auto secondsChrono = std::chrono::time_point_cast<std::chrono::seconds>(now);
 	auto epoch = now.time_since_epoch();
@@ -54,7 +54,7 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) {
 	auto fraction = now - secondsChrono;
 	time->tv_usec = std::chrono::duration_cast<std::chrono::microseconds>(fraction).count();
 	return HasReturnvaluesIF::RETURN_OK;
-#elif defined(LINUX)
+#elif defined(__unix__)
 	timespec timeUnix;
 	int status = clock_gettime(CLOCK_REALTIME,&timeUnix);
 	if(status!=0){
@@ -85,7 +85,7 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
 
 timeval Clock::getUptime() {
 	timeval timeval;
-#if defined(WIN32)
+#if defined(_WIN32)
 	auto uptime  = std::chrono::milliseconds(GetTickCount64());
 	auto secondsChrono = std::chrono::duration_cast<std::chrono::seconds>(uptime);
 	timeval.tv_sec = secondsChrono.count();

From a6dd2d5dcb21058c4c487ac1ef027ea6ece1b98f Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 16:37:12 +0200
Subject: [PATCH 079/125] event manager update

---
 events/EventManager.cpp | 23 ++++++++++-------------
 1 file changed, 10 insertions(+), 13 deletions(-)

diff --git a/events/EventManager.cpp b/events/EventManager.cpp
index 8e8f797f..2337ba83 100644
--- a/events/EventManager.cpp
+++ b/events/EventManager.cpp
@@ -156,21 +156,19 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage *messag
             sif::info << "0x" << std::hex << std::setw(8) << std::setfill('0') <<
                     message->getReporter() << std::setfill(' ') << std::dec;
         }
-        sif::info << " reported " << translateEvents(message->getEvent())
-                        << " with event ID " << std::dec << message->getEventId() << std::endl;
+        sif::info << " report event with ID " << message->getEventId() << std::endl;
         sif::info << std::hex << "P1 Hex: 0x" << message->getParameter1() <<
                 " | P1 Dec: " << std::dec << message->getParameter1() <<  std::hex <<
                 " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<  std::dec <<
                 message->getParameter2() << std::endl;
 #else
         if (string != 0) {
-            sif::printInfo("Event Manager: %s reported %s with event ID %d\n",
-                    message->getReporter(), translateEvents(message->getEvent()),
+            sif::printInfo("Event Manager: %s reported event with ID %d\n", string,
                     message->getEventId());
         }
         else {
-            sif::printInfo("Event Manager: Reporter ID 0x%08x reported %s with event ID %d\n",
-                    string, translateEvents(message->getEvent()), message->getEventId());
+            sif::printInfo("Event Manager: Reporter ID 0x%08x reported event with ID %d\n",
+                    message->getReporter(), message->getEventId());
         }
         sif::printInfo("P1 Hex: 0x%x | P1 Dec: %d | P2 Hex: 0x%x | P2 Dec: %d\n",
                 message->getParameter1(), message->getParameter1(),
@@ -185,23 +183,22 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage *messag
             sif::debug << string;
         }
         else {
-            sif::debug << "0x" << std::hex << message->getReporter() << std::dec;
+            sif::debug << "0x" << std::hex << std::setw(8) << std::setfill('0') <<
+                    message->getReporter() << std::setfill(' ') << std::dec;
         }
-        sif::debug << " reported " << translateEvents(message->getEvent())
-                        << " with event ID " << std::dec << message->getEventId() << std::endl;
+        sif::debug << " report event with ID " << message->getEventId() << std::endl;
         sif::debug << std::hex << "P1 Hex: 0x" << message->getParameter1() <<
                 " | P1 Dec: " << std::dec << message->getParameter1() <<  std::hex <<
                 " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<  std::dec <<
                 message->getParameter2() << std::endl;
 #else
         if (string != 0) {
-            sif::printDebug("Event Manager: %s reported %s with event ID %d\n",
-                    message->getReporter(), translateEvents(message->getEvent()),
+            sif::printDebug("Event Manager: %s reported event with ID %d\n", string,
                     message->getEventId());
         }
         else {
-            sif::printDebug("Event Manager: Reporter ID 0x%08x reported %s with event ID %d\n",
-                    string, translateEvents(message->getEvent()), message->getEventId());
+            sif::printDebug("Event Manager: Reporter ID 0x%08x reported event with ID %d\n",
+                    message->getReporter(), message->getEventId());
         }
         sif::printDebug("P1 Hex: 0x%x | P1 Dec: %d | P2 Hex: 0x%x | P2 Dec: %d\n",
                 message->getParameter1(), message->getParameter1(),

From d27f49c9680190169e004dead07ae0ebe0cc4fc4 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 16:38:02 +0200
Subject: [PATCH 080/125] added platform header file

---
 platform.h | 15 +++++++++++++++
 1 file changed, 15 insertions(+)
 create mode 100644 platform.h

diff --git a/platform.h b/platform.h
new file mode 100644
index 00000000..08260094
--- /dev/null
+++ b/platform.h
@@ -0,0 +1,15 @@
+#ifndef FSFW_PLATFORM_H_
+#define FSFW_PLATFORM_H_
+
+#if defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))
+
+#define PLATFORM_UNIX
+
+#elif defined(_WIN32)
+
+#define PLATFORM_WIN
+
+#endif
+
+
+#endif /* FSFW_PLATFORM_H_ */

From 1626b266d703f10444b6ea3f9fb1c1be228ec400 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 16:47:53 +0200
Subject: [PATCH 081/125] platform header file

---
 osal/common/TcpIpBase.cpp        | 13 ++++++-------
 osal/common/TcpIpBase.h          |  9 +++++----
 osal/common/TcpTmTcServer.cpp    |  8 +++-----
 osal/common/TcpTmTcServer.h      |  3 ++-
 osal/common/UdpTcPollingTask.cpp | 13 +++++--------
 osal/common/UdpTmTcBridge.cpp    | 15 ++++++---------
 osal/common/UdpTmTcBridge.h      |  9 +++------
 osal/host/Clock.cpp              |  6 ++++--
 osal/host/FixedTimeslotTask.cpp  |  5 +++--
 osal/host/PeriodicTask.cpp       |  9 +++++----
 10 files changed, 42 insertions(+), 48 deletions(-)

diff --git a/osal/common/TcpIpBase.cpp b/osal/common/TcpIpBase.cpp
index 27384ecc..0b37e38c 100644
--- a/osal/common/TcpIpBase.cpp
+++ b/osal/common/TcpIpBase.cpp
@@ -1,10 +1,9 @@
 #include "TcpIpBase.h"
+#include "../../platform.h"
 
-#ifdef __unix__
-
+#ifdef PLATFORM_UNIX
 #include <errno.h>
 #include <unistd.h>
-
 #endif
 
 TcpIpBase::TcpIpBase() {
@@ -37,17 +36,17 @@ TcpIpBase::~TcpIpBase() {
 }
 
 int TcpIpBase::closeSocket(socket_t socket) {
-#ifdef _WIN32
+#ifdef PLATFORM_WIN
     return closesocket(socket);
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
     return close(socket);
 #endif
 }
 
 int TcpIpBase::getLastSocketError() {
-#ifdef _WIN32
+#ifdef PLATFORM_WIN
     return WSAGetLastError();
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
     return errno;
 #endif
 }
diff --git a/osal/common/TcpIpBase.h b/osal/common/TcpIpBase.h
index 652d791a..79eefad2 100644
--- a/osal/common/TcpIpBase.h
+++ b/osal/common/TcpIpBase.h
@@ -1,13 +1,14 @@
 #ifndef FSFW_OSAL_COMMON_TCPIPIF_H_
 #define FSFW_OSAL_COMMON_TCPIPIF_H_
 
-#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include "../../returnvalues/HasReturnvaluesIF.h"
+#include "../../platform.h"
 
-#ifdef _WIN32
+#ifdef PLATFORM_WIN
 
 #include <winsock2.h>
 
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
 
 #include <sys/socket.h>
 
@@ -16,7 +17,7 @@
 class TcpIpBase {
 protected:
 
-#ifdef _WIN32
+#ifdef PLATFORM_WIN
     static constexpr int SHUT_RECV = SD_RECEIVE;
     static constexpr int SHUT_SEND = SD_SEND;
     static constexpr int SHUT_BOTH = SD_BOTH;
diff --git a/osal/common/TcpTmTcServer.cpp b/osal/common/TcpTmTcServer.cpp
index 08a62ffb..e1a26fe7 100644
--- a/osal/common/TcpTmTcServer.cpp
+++ b/osal/common/TcpTmTcServer.cpp
@@ -1,15 +1,13 @@
 #include "TcpTmTcServer.h"
 #include "tcpipHelpers.h"
+#include "../../platform.h"
 #include "../../serviceinterface/ServiceInterface.h"
 
-#ifdef _WIN32
+#ifdef PLATFORM_WIN
 #include <winsock2.h>
 #include <ws2tcpip.h>
-
-#elif defined(__unix__)
-
+#elif defined(PLATFORM_UNIX)
 #include <netdb.h>
-
 #endif
 
 const std::string TcpTmTcServer::DEFAULT_TCP_SERVER_PORT =  "7301";
diff --git a/osal/common/TcpTmTcServer.h b/osal/common/TcpTmTcServer.h
index 4dcc77a2..91c579e5 100644
--- a/osal/common/TcpTmTcServer.h
+++ b/osal/common/TcpTmTcServer.h
@@ -2,10 +2,11 @@
 #define FSFW_OSAL_WINDOWS_TCWINTCPSERVER_H_
 
 #include "TcpIpBase.h"
+#include "../../platform.h"
 #include "../../objectmanager/SystemObject.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
-#ifdef __unix__
+#ifdef PLATFORM_UNIX
 #include <sys/socket.h>
 #endif
 
diff --git a/osal/common/UdpTcPollingTask.cpp b/osal/common/UdpTcPollingTask.cpp
index 47f67b29..68108323 100644
--- a/osal/common/UdpTcPollingTask.cpp
+++ b/osal/common/UdpTcPollingTask.cpp
@@ -1,17 +1,14 @@
 #include "UdpTcPollingTask.h"
 #include "tcpipHelpers.h"
+#include "../../platform.h"
 #include "../../globalfunctions/arrayprinter.h"
 #include "../../serviceinterface/ServiceInterfaceStream.h"
 
-#ifdef _WIN32
-
+#ifdef PLATFORM_WIN
 #include <winsock2.h>
-
-#else
-
+#elif defined(PLATFORM_UNIX)
 #include <sys/types.h>
 #include <sys/socket.h>
-
 #endif
 
 //! Debugging preprocessor define.
@@ -155,7 +152,7 @@ ReturnValue_t UdpTcPollingTask::initializeAfterTaskCreation() {
 }
 
 void UdpTcPollingTask::setTimeout(double timeoutSeconds) {
-#ifdef _WIN32
+#ifdef PLATFORM_WIN
     DWORD timeoutMs = timeoutSeconds * 1000.0;
     int result = setsockopt(serverSocket, SOL_SOCKET, SO_RCVTIMEO,
             reinterpret_cast<const char*>(&timeoutMs), sizeof(DWORD));
@@ -165,7 +162,7 @@ void UdpTcPollingTask::setTimeout(double timeoutSeconds) {
                 "receive timeout failed with " << strerror(errno) << std::endl;
 #endif
     }
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
     timeval tval;
     tval = timevalOperations::toTimeval(timeoutSeconds);
     int result = setsockopt(serverSocket, SOL_SOCKET, SO_RCVTIMEO,
diff --git a/osal/common/UdpTmTcBridge.cpp b/osal/common/UdpTmTcBridge.cpp
index ba23f521..2ecc7b7c 100644
--- a/osal/common/UdpTmTcBridge.cpp
+++ b/osal/common/UdpTmTcBridge.cpp
@@ -1,18 +1,15 @@
+#include "UdpTmTcBridge.h"
 #include "tcpipHelpers.h"
 
-#include <fsfw/serviceinterface/ServiceInterface.h>
-#include <fsfw/ipc/MutexGuard.h>
-#include <fsfw/osal/common/UdpTmTcBridge.h>
-
-#ifdef _WIN32
+#include "../../platform.h"
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../ipc/MutexGuard.h"
 
+#ifdef PLATFORM_WIN
 #include <ws2tcpip.h>
-
-#elif defined(__unix__)
-
+#elif defined(PLATFORM_UNIX)
 #include <netdb.h>
 #include <arpa/inet.h>
-
 #endif
 
 //! Debugging preprocessor define.
diff --git a/osal/common/UdpTmTcBridge.h b/osal/common/UdpTmTcBridge.h
index 8b8d1949..360643bb 100644
--- a/osal/common/UdpTmTcBridge.h
+++ b/osal/common/UdpTmTcBridge.h
@@ -2,16 +2,13 @@
 #define FSFW_OSAL_WINDOWS_TMTCWINUDPBRIDGE_H_
 
 #include "TcpIpBase.h"
+#include "../../platform.h"
 #include "../../tmtcservices/TmTcBridge.h"
 
-#ifdef _WIN32
-
+#ifdef PLATFORM_WIN
 #include <ws2tcpip.h>
-
-#elif defined(__unix__)
-
+#elif defined(PLATFORM_UNIX)
 #include <sys/socket.h>
-
 #endif
 
 #include <string>
diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp
index cf9349c1..b2e4c171 100644
--- a/osal/host/Clock.cpp
+++ b/osal/host/Clock.cpp
@@ -1,10 +1,12 @@
 #include "../../serviceinterface/ServiceInterface.h"
 #include "../../timemanager/Clock.h"
+#include "../../platform.h"
 
 #include <chrono>
-#if defined(_WIN32)
+
+#if defined(PLATFORM_WIN)
 #include <sysinfoapi.h>
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
 #include <fstream>
 #endif
 
diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp
index 89daa278..55f37499 100644
--- a/osal/host/FixedTimeslotTask.cpp
+++ b/osal/host/FixedTimeslotTask.cpp
@@ -1,4 +1,5 @@
 #include "taskHelpers.h"
+#include "../../platform.h"
 #include "../../osal/host/FixedTimeslotTask.h"
 #include "../../ipc/MutexFactory.h"
 #include "../../osal/host/Mutex.h"
@@ -9,10 +10,10 @@
 #include <thread>
 #include <chrono>
 
-#if defined(WIN32)
+#if defined(PLATFORM_WIN)
 #include <windows.h>
 #include "../windows/winTaskHelpers.h"
-#elif defined(LINUX)
+#elif defined(PLATFORM_UNIX)
 #include <pthread.h>
 #endif
 
diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp
index 09df410f..4b3fa626 100644
--- a/osal/host/PeriodicTask.cpp
+++ b/osal/host/PeriodicTask.cpp
@@ -2,6 +2,7 @@
 #include "PeriodicTask.h"
 #include "taskHelpers.h"
 
+#include "../../platform.h"
 #include "../../ipc/MutexFactory.h"
 #include "../../serviceinterface/ServiceInterfaceStream.h"
 #include "../../tasks/ExecutableObjectIF.h"
@@ -9,10 +10,10 @@
 #include <thread>
 #include <chrono>
 
-#if defined(WIN32)
+#if defined(PLATFORM_WIN)
 #include <processthreadsapi.h>
 #include <fsfw/osal/windows/winTaskHelpers.h>
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
 #include <pthread.h>
 #endif
 
@@ -24,9 +25,9 @@ PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority,
     // It is probably possible to set task priorities by using the native
     // task handles for Windows / Linux
 	mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this);
-#if defined(_WIN32)
+#if defined(PLATFORM_WIN)
 	tasks::setTaskPriority(reinterpret_cast<HANDLE>(mainThread.native_handle()), setPriority);
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
     // TODO: We could reuse existing code here.
 #endif
     tasks::insertTaskName(mainThread.get_id(), taskName);

From 4095be449ae4b6ebaf89fdcbfc5bc31b854594d5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 17:32:40 +0200
Subject: [PATCH 082/125] some more preprocessor replacements

---
 osal/common/TcpIpBase.h |  6 +-----
 osal/host/Clock.cpp     | 10 +++++-----
 platform.h              |  5 -----
 3 files changed, 6 insertions(+), 15 deletions(-)

diff --git a/osal/common/TcpIpBase.h b/osal/common/TcpIpBase.h
index 79eefad2..fe6a763c 100644
--- a/osal/common/TcpIpBase.h
+++ b/osal/common/TcpIpBase.h
@@ -5,13 +5,9 @@
 #include "../../platform.h"
 
 #ifdef PLATFORM_WIN
-
 #include <winsock2.h>
-
 #elif defined(PLATFORM_UNIX)
-
 #include <sys/socket.h>
-
 #endif
 
 class TcpIpBase {
@@ -23,7 +19,7 @@ protected:
     static constexpr int SHUT_BOTH = SD_BOTH;
 
     using socket_t = SOCKET;
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
     using socket_t = int;
 
     static constexpr int INVALID_SOCKET = -1;
diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp
index b2e4c171..ed5aab62 100644
--- a/osal/host/Clock.cpp
+++ b/osal/host/Clock.cpp
@@ -48,7 +48,7 @@ ReturnValue_t Clock::setClock(const timeval* time) {
 }
 
 ReturnValue_t Clock::getClock_timeval(timeval* time) {
-#if defined(_WIN32)
+#if defined(PLATFORM_WIN)
 	auto now  = std::chrono::system_clock::now();
 	auto secondsChrono = std::chrono::time_point_cast<std::chrono::seconds>(now);
 	auto epoch = now.time_since_epoch();
@@ -56,7 +56,7 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) {
 	auto fraction = now - secondsChrono;
 	time->tv_usec = std::chrono::duration_cast<std::chrono::microseconds>(fraction).count();
 	return HasReturnvaluesIF::RETURN_OK;
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
 	timespec timeUnix;
 	int status = clock_gettime(CLOCK_REALTIME,&timeUnix);
 	if(status!=0){
@@ -87,14 +87,14 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
 
 timeval Clock::getUptime() {
 	timeval timeval;
-#if defined(_WIN32)
+#if defined(PLATFORM_WIN)
 	auto uptime  = std::chrono::milliseconds(GetTickCount64());
 	auto secondsChrono = std::chrono::duration_cast<std::chrono::seconds>(uptime);
 	timeval.tv_sec = secondsChrono.count();
 	auto fraction = uptime - secondsChrono;
 	timeval.tv_usec = std::chrono::duration_cast<std::chrono::microseconds>(
 	            fraction).count();
-#elif defined(__unix__)
+#elif defined(PLATFORM_UNIX)
 	double uptimeSeconds;
 	if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds)
 	{
@@ -121,7 +121,7 @@ ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) {
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
-
+ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 	/* Do some magic with chrono (C++20!) */
 	/* Right now, the library doesn't have the new features to get the required values yet.
 	so we work around that for now. */
diff --git a/platform.h b/platform.h
index 08260094..4bca3398 100644
--- a/platform.h
+++ b/platform.h
@@ -2,14 +2,9 @@
 #define FSFW_PLATFORM_H_
 
 #if defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))
-
 #define PLATFORM_UNIX
-
 #elif defined(_WIN32)
-
 #define PLATFORM_WIN
-
 #endif
 
-
 #endif /* FSFW_PLATFORM_H_ */

From 4fa56a2f1d3424c2afa30d34013ce2e11533ae84 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 12 May 2021 18:37:52 +0200
Subject: [PATCH 083/125] moved string port argument

---
 osal/common/UdpTmTcBridge.cpp | 2 +-
 osal/common/UdpTmTcBridge.h   | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/osal/common/UdpTmTcBridge.cpp b/osal/common/UdpTmTcBridge.cpp
index ba23f521..a3b4efa7 100644
--- a/osal/common/UdpTmTcBridge.cpp
+++ b/osal/common/UdpTmTcBridge.cpp
@@ -21,7 +21,7 @@
 const std::string UdpTmTcBridge::DEFAULT_UDP_SERVER_PORT =  tcpip::DEFAULT_SERVER_PORT;
 
 UdpTmTcBridge::UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
-        object_id_t tmStoreId, object_id_t tcStoreId, std::string udpServerPort):
+        std::string udpServerPort, object_id_t tmStoreId, object_id_t tcStoreId):
         TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
     if(udpServerPort == "") {
         this->udpServerPort = DEFAULT_UDP_SERVER_PORT;
diff --git a/osal/common/UdpTmTcBridge.h b/osal/common/UdpTmTcBridge.h
index 8b8d1949..49d846a1 100644
--- a/osal/common/UdpTmTcBridge.h
+++ b/osal/common/UdpTmTcBridge.h
@@ -28,8 +28,8 @@ public:
     /* The ports chosen here should not be used by any other process. */
     static const std::string DEFAULT_UDP_SERVER_PORT;
 
-    UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
-            object_id_t tmStoreId, object_id_t tcStoreId, std::string udpServerPort = "");
+    UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, std::string udpServerPort = "",
+            object_id_t tmStoreId = objects::TM_STORE, object_id_t tcStoreId = objects::TC_STORE);
     virtual~ UdpTmTcBridge();
 
     /**

From 0bc124fd2187bfad49589562bba812ac79a1d387 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 13 May 2021 21:46:46 +0200
Subject: [PATCH 084/125] renamed tcpip tasks

---
 unittest/user/testcfg/objects/systemObjectList.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/unittest/user/testcfg/objects/systemObjectList.h b/unittest/user/testcfg/objects/systemObjectList.h
index 88b92131..76f1ff90 100644
--- a/unittest/user/testcfg/objects/systemObjectList.h
+++ b/unittest/user/testcfg/objects/systemObjectList.h
@@ -15,8 +15,8 @@ namespace objects {
 		PUS_DISTRIBUTOR = 11,
 		TM_FUNNEL = 12,
 
-		UDP_BRIDGE = 15,
-		UDP_POLLING_TASK = 16,
+		TCPIP_BRIDGE = 15,
+		TCPIP_HELPER = 16,
 
 		TEST_ECHO_COM_IF = 20,
 		TEST_DEVICE = 21,

From 5847081a246aea8d56b184e122052bd792b02661 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 13 May 2021 22:17:21 +0200
Subject: [PATCH 085/125] tweaks and fixes for TCP

---
 osal/common/TcpTmTcServer.cpp | 7 ++++---
 osal/common/TcpTmTcServer.h   | 2 +-
 osal/windows/tcpipHelpers.cpp | 4 ++--
 3 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/osal/common/TcpTmTcServer.cpp b/osal/common/TcpTmTcServer.cpp
index 0663fed1..28fab422 100644
--- a/osal/common/TcpTmTcServer.cpp
+++ b/osal/common/TcpTmTcServer.cpp
@@ -99,8 +99,8 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
     using namespace tcpip;
     // If a connection is accepted, the corresponding socket will be assigned to the new socket
     socket_t connSocket = 0;
-    sockaddr clientSockAddr = {};
-    socklen_t connectorSockAddrLen = 0;
+    // sockaddr clientSockAddr = {};
+    // socklen_t connectorSockAddrLen = 0;
     int retval = 0;
 
     // Listen for connection requests permanently for lifetime of program
@@ -111,7 +111,8 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
             continue;
         }
 
-        connSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen);
+        //connSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen);
+        connSocket = accept(listenerTcpSocket, nullptr, nullptr);
 
         if(connSocket == INVALID_SOCKET) {
             handleError(Protocol::TCP, ErrorSources::ACCEPT_CALL, 500);
diff --git a/osal/common/TcpTmTcServer.h b/osal/common/TcpTmTcServer.h
index 191fff7b..f7c36d69 100644
--- a/osal/common/TcpTmTcServer.h
+++ b/osal/common/TcpTmTcServer.h
@@ -43,7 +43,7 @@ class TcpTmTcServer:
 public:
     /* The ports chosen here should not be used by any other process. */
     static const std::string DEFAULT_TCP_SERVER_PORT;
-    static const std::string DEFAULT_TCP_CLIENT_PORT;
+
     static constexpr  size_t ETHERNET_MTU_SIZE = 1500;
 
     /**
diff --git a/osal/windows/tcpipHelpers.cpp b/osal/windows/tcpipHelpers.cpp
index 03278a92..3dab9406 100644
--- a/osal/windows/tcpipHelpers.cpp
+++ b/osal/windows/tcpipHelpers.cpp
@@ -50,8 +50,8 @@ void tcpip::handleError(Protocol protocol, ErrorSources errorSrc, dur_millis_t s
     sif::warning << "tcpip::handleError: " << protocolString << " | " << errorSrcString <<
             " | " << infoString << std::endl;
 #else
-    sif::printWarning("tcpip::handleError: %s | %s | %s\n", protocolString,
-            errorSrcString, infoString);
+    sif::printWarning("tcpip::handleError: %s | %s | %s\n", protocolString.c_str(),
+            errorSrcString.c_str(), infoString.c_str());
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */

From 58a532fc4d54139ed0f39517e03f2b7788870c9a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 11:13:28 +0200
Subject: [PATCH 086/125] added health service to fsfw objects

---
 objectmanager/frameworkObjects.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/objectmanager/frameworkObjects.h b/objectmanager/frameworkObjects.h
index 2174f829..47ba6df2 100644
--- a/objectmanager/frameworkObjects.h
+++ b/objectmanager/frameworkObjects.h
@@ -16,6 +16,8 @@ enum framework_objects: object_id_t {
     PUS_SERVICE_17_TEST = 0x53000017,
     PUS_SERVICE_20_PARAMETERS = 0x53000020,
     PUS_SERVICE_200_MODE_MGMT = 0x53000200,
+    PUS_SERVICE_201_HEALTH = 0x53000201,
+
 
     //Generic IDs for IPC, modes, health, events
     HEALTH_TABLE = 0x53010000,

From d3423b30b0f0e2259f9fbde12b1757d54cdd1259 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 11:14:26 +0200
Subject: [PATCH 087/125] minot changes

---
 objectmanager/frameworkObjects.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/objectmanager/frameworkObjects.h b/objectmanager/frameworkObjects.h
index 47ba6df2..fdb72e30 100644
--- a/objectmanager/frameworkObjects.h
+++ b/objectmanager/frameworkObjects.h
@@ -1,7 +1,7 @@
 #ifndef FSFW_OBJECTMANAGER_FRAMEWORKOBJECTS_H_
 #define FSFW_OBJECTMANAGER_FRAMEWORKOBJECTS_H_
 
-#include <fsfw/objectmanager/SystemObjectIF.h>
+#include "SystemObjectIF.h"
 
 namespace objects {
 enum framework_objects: object_id_t {
@@ -18,7 +18,6 @@ enum framework_objects: object_id_t {
     PUS_SERVICE_200_MODE_MGMT = 0x53000200,
     PUS_SERVICE_201_HEALTH = 0x53000201,
 
-
     //Generic IDs for IPC, modes, health, events
     HEALTH_TABLE = 0x53010000,
     //	MODE_STORE = 0x53010100,

From 9e0146f5791b1c7f34c467e7118f4efb683d7205 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 11:16:54 +0200
Subject: [PATCH 088/125] comment added

---
 objectmanager/frameworkObjects.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/objectmanager/frameworkObjects.h b/objectmanager/frameworkObjects.h
index fdb72e30..36010807 100644
--- a/objectmanager/frameworkObjects.h
+++ b/objectmanager/frameworkObjects.h
@@ -3,6 +3,7 @@
 
 #include "SystemObjectIF.h"
 
+// The objects will be instantiated in the ID order
 namespace objects {
 enum framework_objects: object_id_t {
     FSFW_OBJECTS_START = 0x53000000,

From 94062c67d398966ef814a206811fde07b926866a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 11:28:37 +0200
Subject: [PATCH 089/125] added more pus services

---
 events/fwSubsystemIdRanges.h | 51 ++++++++++++++++++++----------------
 1 file changed, 29 insertions(+), 22 deletions(-)

diff --git a/events/fwSubsystemIdRanges.h b/events/fwSubsystemIdRanges.h
index 1b0b238e..88dee9b4 100644
--- a/events/fwSubsystemIdRanges.h
+++ b/events/fwSubsystemIdRanges.h
@@ -1,30 +1,37 @@
 #ifndef FSFW_EVENTS_FWSUBSYSTEMIDRANGES_H_
 #define FSFW_EVENTS_FWSUBSYSTEMIDRANGES_H_
 
+#include <cstdint>
+
 namespace SUBSYSTEM_ID {
-enum {
-	MEMORY = 22,
-	OBSW = 26,
-	CDH = 28,
-	TCS_1 = 59,
-	PCDU_1 = 42,
-	PCDU_2 = 43,
-	HEATER = 50,
-	T_SENSORS = 52,
-	FDIR = 70,
-	FDIR_1 = 71,
-	FDIR_2 = 72,
-	HK = 73,
-	SYSTEM_MANAGER = 74,
-	SYSTEM_MANAGER_1 = 75,
-	SYSTEM_1 = 79,
-	PUS_SERVICE_1 = 80,
-	PUS_SERVICE_9 = 89,
-	PUS_SERVICE_17 = 97,
-	FW_SUBSYSTEM_ID_RANGE
+enum: uint8_t {
+    MEMORY = 22,
+    OBSW = 26,
+    CDH = 28,
+    TCS_1 = 59,
+    PCDU_1 = 42,
+    PCDU_2 = 43,
+    HEATER = 50,
+    T_SENSORS = 52,
+    FDIR = 70,
+    FDIR_1 = 71,
+    FDIR_2 = 72,
+    HK = 73,
+    SYSTEM_MANAGER = 74,
+    SYSTEM_MANAGER_1 = 75,
+    SYSTEM_1 = 79,
+    PUS_SERVICE_1 = 80,
+    PUS_SERVICE_2 = 82,
+    PUS_SERVICE_3 = 83,
+    PUS_SERVICE_5 = 85,
+    PUS_SERVICE_6 = 86,
+    PUS_SERVICE_8 = 88,
+    PUS_SERVICE_9 = 89,
+    PUS_SERVICE_17 = 97,
+    PUS_SERVICE_23 = 103,
+
+    FW_SUBSYSTEM_ID_RANGE
 };
 }
 
-
-
 #endif /* FSFW_EVENTS_FWSUBSYSTEMIDRANGES_H_ */

From 6db2efc20dc04de5e433ad2a66f23b89a70bb875 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 11:36:36 +0200
Subject: [PATCH 090/125] copmment block for fw class ids

---
 returnvalues/FwClassIds.h | 135 ++++++++++++++++++++------------------
 1 file changed, 70 insertions(+), 65 deletions(-)

diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h
index 4c9f022b..2bb83f73 100644
--- a/returnvalues/FwClassIds.h
+++ b/returnvalues/FwClassIds.h
@@ -1,72 +1,77 @@
 #ifndef FSFW_RETURNVALUES_FWCLASSIDS_H_
 #define FSFW_RETURNVALUES_FWCLASSIDS_H_
 
+#include <cstdint>
+
+// The comment block at the end is used by the returnvalue exporter.
+// It is recommended to add it as well for mission returnvalues
 namespace CLASS_ID {
-enum {
-	OPERATING_SYSTEM_ABSTRACTION = 1,  //OS
-	OBJECT_MANAGER_IF, //OM
-	DEVICE_HANDLER_BASE, //DHB
-	RMAP_CHANNEL, //RMP
-	POWER_SWITCH_IF, //PS
-	HAS_MEMORY_IF, //PP
-	DEVICE_STATE_MACHINE_BASE, //DSMB
-	DATA_SET_CLASS, //DPS
-	POOL_RAW_ACCESS_CLASS, //DPR
-	CONTROLLER_BASE, //CTR
-	SUBSYSTEM_BASE, //SB
-	MODE_STORE_IF, //MS
-	SUBSYSTEM, //SS
-	HAS_MODES_IF, //HM
-	COMMAND_MESSAGE, //CM
-	CCSDS_TIME_HELPER_CLASS, //TIM
-	ARRAY_LIST, //AL
-	ASSEMBLY_BASE, //AB
-	MEMORY_HELPER, //MH
-	SERIALIZE_IF, //SE
-	FIXED_MAP, //FM
-	FIXED_MULTIMAP, //FMM
-	HAS_HEALTH_IF, //HHI
-	FIFO_CLASS, //FF
-	MESSAGE_PROXY, //MQP
-	TRIPLE_REDUNDACY_CHECK, //TRC
-	TC_PACKET_CHECK, //TCC
-	PACKET_DISTRIBUTION, //TCD
-	ACCEPTS_TELECOMMANDS_IF, //PUS
-	DEVICE_SERVICE_BASE, //DSB
-	COMMAND_SERVICE_BASE, //CSB
-	TM_STORE_BACKEND_IF, //TMB
-	TM_STORE_FRONTEND_IF, //TMF
-	STORAGE_AND_RETRIEVAL_SERVICE, //SR
-	MATCH_TREE_CLASS, //MT
-	EVENT_MANAGER_IF, //EV
-	HANDLES_FAILURES_IF, //FDI
-	DEVICE_HANDLER_IF, //DHI
-	STORAGE_MANAGER_IF, //SM
-	THERMAL_COMPONENT_IF, //TC
-	INTERNAL_ERROR_CODES, //IEC
-	TRAP, //TRP
-	CCSDS_HANDLER_IF, //CCS
-	PARAMETER_WRAPPER, //PAW
-	HAS_PARAMETERS_IF, //HPA
-	ASCII_CONVERTER, //ASC
-	POWER_SWITCHER, //POS
-	LIMITS_IF, //LIM
-	COMMANDS_ACTIONS_IF, //CF
-	HAS_ACTIONS_IF, //HF
-	DEVICE_COMMUNICATION_IF, //DC
-	BSP, //BSP
-	TIME_STAMPER_IF, //TSI 53
-	SGP4PROPAGATOR_CLASS, //SGP4 54
-	MUTEX_IF, //MUX 55
-	MESSAGE_QUEUE_IF,//MQI 56
-	SEMAPHORE_IF, //SPH 57
-	LOCAL_POOL_OWNER_IF, //LPIF 58
-	POOL_VARIABLE_IF, //PVA 59
-	HOUSEKEEPING_MANAGER, //HKM 60
-	DLE_ENCODER, //DLEE 61
-	PUS_SERVICE_9, //PUS9 62
-	FILE_SYSTEM, //FILS 63
-	FW_CLASS_ID_COUNT //is actually count + 1 !
+enum: uint8_t {
+    OPERATING_SYSTEM_ABSTRACTION = 1,  //OS
+    OBJECT_MANAGER_IF, //OM
+    DEVICE_HANDLER_BASE, //DHB
+    RMAP_CHANNEL, //RMP
+    POWER_SWITCH_IF, //PS
+    HAS_MEMORY_IF, //PP
+    DEVICE_STATE_MACHINE_BASE, //DSMB
+    DATA_SET_CLASS, //DPS
+    POOL_RAW_ACCESS_CLASS, //DPR
+    CONTROLLER_BASE, //CTR
+    SUBSYSTEM_BASE, //SB
+    MODE_STORE_IF, //MS
+    SUBSYSTEM, //SS
+    HAS_MODES_IF, //HM
+    COMMAND_MESSAGE, //CM
+    CCSDS_TIME_HELPER_CLASS, //TIM
+    ARRAY_LIST, //AL
+    ASSEMBLY_BASE, //AB
+    MEMORY_HELPER, //MH
+    SERIALIZE_IF, //SE
+    FIXED_MAP, //FM
+    FIXED_MULTIMAP, //FMM
+    HAS_HEALTH_IF, //HHI
+    FIFO_CLASS, //FF
+    MESSAGE_PROXY, //MQP
+    TRIPLE_REDUNDACY_CHECK, //TRC
+    TC_PACKET_CHECK, //TCC
+    PACKET_DISTRIBUTION, //TCD
+    ACCEPTS_TELECOMMANDS_IF, //PUS
+    DEVICE_SERVICE_BASE, //DSB
+    COMMAND_SERVICE_BASE, //CSB
+    TM_STORE_BACKEND_IF, //TMB
+    TM_STORE_FRONTEND_IF, //TMF
+    STORAGE_AND_RETRIEVAL_SERVICE, //SR
+    MATCH_TREE_CLASS, //MT
+    EVENT_MANAGER_IF, //EV
+    HANDLES_FAILURES_IF, //FDI
+    DEVICE_HANDLER_IF, //DHI
+    STORAGE_MANAGER_IF, //SM
+    THERMAL_COMPONENT_IF, //TC
+    INTERNAL_ERROR_CODES, //IEC
+    TRAP, //TRP
+    CCSDS_HANDLER_IF, //CCS
+    PARAMETER_WRAPPER, //PAW
+    HAS_PARAMETERS_IF, //HPA
+    ASCII_CONVERTER, //ASC
+    POWER_SWITCHER, //POS
+    LIMITS_IF, //LIM
+    COMMANDS_ACTIONS_IF, //CF
+    HAS_ACTIONS_IF, //HF
+    DEVICE_COMMUNICATION_IF, //DC
+    BSP, //BSP
+    TIME_STAMPER_IF, //TSI
+    SGP4PROPAGATOR_CLASS, //SGP4
+    MUTEX_IF, //MUX
+    MESSAGE_QUEUE_IF,//MQI
+    SEMAPHORE_IF, //SPH
+    LOCAL_POOL_OWNER_IF, //LPIF
+    POOL_VARIABLE_IF, //PVA
+    HOUSEKEEPING_MANAGER, //HKM
+    DLE_ENCODER, //DLEE
+    PUS_SERVICE_3, //PUS3
+    PUS_SERVICE_9, //PUS9
+    FILE_SYSTEM, //FILS
+    FW_CLASS_ID_COUNT //is actually count + 1 !
 
 };
 }

From c07672f9b461531e065213e0ef77e0246f09674f Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 14:57:54 +0200
Subject: [PATCH 091/125] changed class id file for refactored modgen

---
 returnvalues/FwClassIds.h | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h
index 2bb83f73..17c37a79 100644
--- a/returnvalues/FwClassIds.h
+++ b/returnvalues/FwClassIds.h
@@ -7,7 +7,8 @@
 // It is recommended to add it as well for mission returnvalues
 namespace CLASS_ID {
 enum: uint8_t {
-    OPERATING_SYSTEM_ABSTRACTION = 1,  //OS
+    FW_CLASS_ID_START = 0, // [EXPORT] : [START]
+    OPERATING_SYSTEM_ABSTRACTION,  //OS
     OBJECT_MANAGER_IF, //OM
     DEVICE_HANDLER_BASE, //DHB
     RMAP_CHANNEL, //RMP
@@ -71,7 +72,7 @@ enum: uint8_t {
     PUS_SERVICE_3, //PUS3
     PUS_SERVICE_9, //PUS9
     FILE_SYSTEM, //FILS
-    FW_CLASS_ID_COUNT //is actually count + 1 !
+    FW_CLASS_ID_COUNT // [EXPORT] : [END]
 
 };
 }

From e2ae9756f3eda0a54d1ec2c2beecba18d74c3618 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 15:12:48 +0200
Subject: [PATCH 092/125] updated documentation

---
 doc/README-config.md    | 24 ++++++++++++++++++---
 doc/README-core.md      | 20 ++++++++++++------
 doc/README-highlevel.md | 47 ++++++++++++++++++++++++-----------------
 3 files changed, 62 insertions(+), 29 deletions(-)

diff --git a/doc/README-config.md b/doc/README-config.md
index 036a7d14..b314b84e 100644
--- a/doc/README-config.md
+++ b/doc/README-config.md
@@ -1,10 +1,27 @@
-
 ## Configuring the FSFW
 
 The FSFW can be configured via the `fsfwconfig` folder. A template folder has
 been provided to have a starting point for this. The folder should be added
-to the include path.
+to the include path. The primary configuration file is the `FSFWConfig.h` folder. Some
+of the available options will be explained in more detail here.
 
+## Auto-Translation of Events
+
+The FSFW allows the automatic translation of events, which allows developers to track triggered
+events directly via consoke output. Using this feature requires:
+
+1. `FSFW_OBJ_EVENT_TRANSLATION` set to 1 in the configuration file.
+2. Special auto-generated translation files which translate event IDs and object IDs into
+   human readable strings. These files can be generated using the 
+   [modgen Python scripts](https://git.ksat-stuttgart.de/source/modgen.git).
+3. The generated translation files for the object IDs should be named `translatesObjects.cpp`
+   and `translateObjects.h` and should be copied to the `fsfwconfig/objects` folder
+4. The generated translation files for the event IDs should be named `translateEvents.cpp` and
+   `translateEvents.h` and should be copied to the `fsfwconfig/events` folder
+   
+An example implementations of these translation file generators can be found as part
+of the [SOURCE project here](https://git.ksat-stuttgart.de/source/sourceobsw/-/tree/development/generators)
+or the [FSFW example](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example_public/src/branch/master/generators)
 
 ### Configuring the Event Manager
 
@@ -18,4 +35,5 @@ static constexpr size_t FSFW_EVENTMGMR_MATCHTREE_NODES = 240;
 static constexpr size_t FSFW_EVENTMGMT_EVENTIDMATCHERS = 120;
 static constexpr size_t FSFW_EVENTMGMR_RANGEMATCHERS   = 120;
 }
-```
\ No newline at end of file
+```
+
diff --git a/doc/README-core.md b/doc/README-core.md
index 6431b831..e34890ae 100644
--- a/doc/README-core.md
+++ b/doc/README-core.md
@@ -19,8 +19,9 @@ A nullptr check of the returning Pointer must be done. This function is based on
 ```cpp
 template <typename T> T* ObjectManagerIF::get( object_id_t id )
 ```
-* A typical way to create all objects on startup is a handing a static produce function to the ObjectManager on creation.
-By calling objectManager->initialize() the produce function will be called and all SystemObjects will be initialized afterwards.
+* A typical way to create all objects on startup is a handing a static produce function to the
+  ObjectManager on creation. By calling objectManager->initialize() the produce function will be
+  called and all SystemObjects will be initialized afterwards.
 
 ### Event Manager
 
@@ -36,14 +37,19 @@ By calling objectManager->initialize() the produce function will be called and a
 
 ### Stores
 
-* The message based communication can only exchange a few bytes of information inside the message itself. Therefore, additional information can 
-  be exchanged with Stores. With this, only the store address must be exchanged in the message.
-* Internally, the FSFW uses an IPC Store to exchange data between processes. For incoming TCs a TC Store is used. For outgoing TM a TM store is used.
+* The message based communication can only exchange a few bytes of information inside the message
+  itself. Therefore, additional information can  be exchanged with Stores. With this, only the
+  store address must be exchanged in the message.
+* Internally, the FSFW uses an IPC Store to exchange data between processes. For incoming TCs a TC
+  Store is used. For outgoing TM a TM store is used.
 * All of them should use the Thread Safe Class storagemanager/PoolManager
 
 ### Tasks
 
 There are two different types of tasks:
- * The PeriodicTask just executes objects that are of type ExecutableObjectIF in the order of the insertion to the Tasks.
- * FixedTimeslotTask executes a list of calls in the order of the given list. This is intended for DeviceHandlers, where polling should be in a defined order. An example can be found in defaultcfg/fsfwconfig/pollingSequence
+ * The PeriodicTask just executes objects that are of type ExecutableObjectIF in the order of the
+   insertion to the Tasks.
+ * FixedTimeslotTask executes a list of calls in the order of the given list. This is intended for
+   DeviceHandlers, where polling should be in a defined order. An example can be found in 
+   `defaultcfg/fsfwconfig/pollingSequence` folder
 
diff --git a/doc/README-highlevel.md b/doc/README-highlevel.md
index fac89c53..dd94d6da 100644
--- a/doc/README-highlevel.md
+++ b/doc/README-highlevel.md
@@ -3,11 +3,12 @@
 ## Structure
 
 The general structure is driven by the usage of interfaces provided by objects. 
-The FSFW uses C++11 as baseline. The intention behind this is that this C++ Standard should be widely available, even with older compilers.
+The FSFW uses C++11 as baseline. The intention behind this is that this C++ Standard should be
+widely available, even with older compilers.
 The FSFW uses dynamic allocation during the initialization but provides static containers during runtime. 
 This simplifies the instantiation of objects and allows the usage of some standard containers. 
-Dynamic Allocation after initialization is discouraged and different solutions are provided in the FSFW to achieve that.
-The fsfw uses run-time type information but exceptions are not allowed.
+Dynamic Allocation after initialization is discouraged and different solutions are provided in the
+FSFW to achieve that. The fsfw uses run-time type information but exceptions are not allowed.
 
 ### Failure Handling
 
@@ -41,10 +42,11 @@ An example setup of ids can be found in the example config in "defaultcft/fsfwco
 
 ### Events
 
-Events are tied to objects. EventIds can be generated by calling the Macro MAKE_EVENT. This works analog to the returnvalues.
-Every object that needs own EventIds has to get a unique SUBSYSTEM_ID. 
-Every SystemObject can call triggerEvent from the parent class.
-Therefore, event messages contain the specific EventId and the objectId of the object that has triggered.
+Events are tied to objects. EventIds can be generated by calling the Macro MAKE_EVENT. 
+This works analog to the returnvalues. Every object that needs own EventIds has to get a
+unique SUBSYSTEM_ID. Every SystemObject can call triggerEvent from the parent class.
+Therefore, event messages contain the specific EventId and the objectId of the object that
+has triggered.
 
 ### Internal Communication
 
@@ -59,17 +61,19 @@ The services can be seen as a conversion from a TC to a message based communicat
 
 #### CCSDS Frames, CCSDS Space Packets and PUS
 
-If the communication is based on CCSDS Frames and Space Packets, several classes can be used to distributed the packets to the corresponding services. Those can be found in tcdistribution. 
-If Space Packets are used, a timestamper must be created. 
-An example can be found in the timemanager folder, this uses CCSDSTime::CDS_short.
+If the communication is based on CCSDS Frames and Space Packets, several classes can be used to
+distributed the packets to the corresponding services. Those can be found in `tcdistribution`. 
+If Space Packets are used, a timestamper has to be provided by the user. 
+An example can be found in the `timemanager` folder, which uses `CCSDSTime::CDS_short`.
 
 #### Device Handlers
 
 DeviceHandlers are another important component of the FSFW. 
-The idea is, to have a software counterpart of every physical device to provide a simple mode, health and commanding interface.
-By separating the underlying Communication Interface with DeviceCommunicationIF, a device handler (DH) can be tested on different hardware.
-The DH has mechanisms to monitor the communication with the physical device which allow for FDIR reaction. 
-Device Handlers can be created by overriding `DeviceHandlerBase`. 
+The idea is, to have a software counterpart of every physical device to provide a simple mode,
+health and commanding interface. By separating the underlying Communication Interface with 
+`DeviceCommunicationIF`, a device handler (DH) can be tested on different hardware.
+The DH has mechanisms to monitor the communication with the physical device which allow
+for FDIR reaction.  Device Handlers can be created by implementing `DeviceHandlerBase`. 
 A standard FDIR component for the DH will be created automatically but can be overwritten by the user.
 More information on DeviceHandlers can be found in the related [documentation section](doc/README-devicehandlers.md#top).
 
@@ -81,13 +85,17 @@ DeviceHandlers and Controllers are the lowest part of the hierarchy.
 The next layer are Assemblies. Those assemblies act as a component which handle redundancies of handlers. 
 Assemblies share a common core with the next level which are the Subsystems. 
 
-Those Assemblies are intended to act as auto-generated components from a database which describes the subsystem modes. 
-The definitions contain transition and target tables which contain the DH, Assembly and Controller Modes to be commanded.
-Transition tables contain as many steps as needed to reach the mode from any other mode, e.g. a switch into any higher AOCS mode might first turn on the sensors, than the actuators and the controller as last component. 
+Those Assemblies are intended to act as auto-generated components from a database which describes
+the subsystem modes. The definitions contain transition and target tables which contain the DH,
+Assembly and Controller Modes to be commanded.
+Transition tables contain as many steps as needed to reach the mode from any other mode, e.g. a
+switch into any higher AOCS mode might first turn on the sensors, than the actuators and the
+controller as last component. 
 The target table is used to describe the state that is checked continuously by the subsystem. 
 All of this allows System Modes to be generated as Subsystem object as well from the same database. 
 This System contains list of subsystem modes in the transition and target tables. 
-Therefore, it allows a modular system to create system modes and easy commanding of those, because only the highest components must be commanded.
+Therefore, it allows a modular system to create system modes and easy commanding of those, because
+only the highest components must be commanded.
 
 The health state represents if the component is able to perform its tasks. 
 This can be used to signal the system to avoid using this component instead of a redundant one.
@@ -95,5 +103,6 @@ The on-board FDIR uses the health state for isolation and recovery.
 
 ## Unit Tests
 
-Unit Tests are provided in the unittest folder. Those use the catch2 framework but do not include catch2 itself. More information on how to run these tests can be found in the separate
+Unit Tests are provided in the unittest folder. Those use the catch2 framework but do not include
+catch2 itself. More information on how to run these tests can be found in the separate
 [`fsfw_tests` reposoitory](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_tests)

From ed27d388d5d954dfbb580b5972d6dfcb3cf59aef Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 15:31:04 +0200
Subject: [PATCH 093/125] added tmtc chapter in doc

---
 doc/README-config.md    |  7 ++--
 doc/README-highlevel.md | 84 ++++++++++++++++++++++++++---------------
 2 files changed, 58 insertions(+), 33 deletions(-)

diff --git a/doc/README-config.md b/doc/README-config.md
index b314b84e..6afba83c 100644
--- a/doc/README-config.md
+++ b/doc/README-config.md
@@ -1,11 +1,12 @@
-## Configuring the FSFW
+Configuring the FSFW
+======
 
 The FSFW can be configured via the `fsfwconfig` folder. A template folder has
 been provided to have a starting point for this. The folder should be added
 to the include path. The primary configuration file is the `FSFWConfig.h` folder. Some
 of the available options will be explained in more detail here.
 
-## Auto-Translation of Events
+# Auto-Translation of Events
 
 The FSFW allows the automatic translation of events, which allows developers to track triggered
 events directly via consoke output. Using this feature requires:
@@ -23,7 +24,7 @@ An example implementations of these translation file generators can be found as
 of the [SOURCE project here](https://git.ksat-stuttgart.de/source/sourceobsw/-/tree/development/generators)
 or the [FSFW example](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example_public/src/branch/master/generators)
 
-### Configuring the Event Manager
+## Configuring the Event Manager
 
 The number of allowed subscriptions can be modified with the following
 parameters:
diff --git a/doc/README-highlevel.md b/doc/README-highlevel.md
index dd94d6da..7bf96c64 100644
--- a/doc/README-highlevel.md
+++ b/doc/README-highlevel.md
@@ -1,6 +1,7 @@
-# High-level overview
+High-level overview
+======
 
-## Structure
+# Structure
 
 The general structure is driven by the usage of interfaces provided by objects. 
 The FSFW uses C++11 as baseline. The intention behind this is that this C++ Standard should be
@@ -10,37 +11,44 @@ This simplifies the instantiation of objects and allows the usage of some standa
 Dynamic Allocation after initialization is discouraged and different solutions are provided in the
 FSFW to achieve that. The fsfw uses run-time type information but exceptions are not allowed.
 
-### Failure Handling
+# Failure Handling
 
-Functions should return a defined ReturnValue_t to signal to the caller that something has gone wrong. 
-Returnvalues must be unique. For this the function HasReturnvaluesIF::makeReturnCode or the Macro MAKE_RETURN can be used.
-The CLASS_ID is a unique id for that type of object. See returnvalues/FwClassIds.
+Functions should return a defined `ReturnValue_t` to signal to the caller that something has
+gone wrong. Returnvalues must be unique. For this the function `HasReturnvaluesIF::makeReturnCode`
+or the macro `MAKE_RETURN` can be used. The `CLASS_ID` is a unique id for that type of object.
+See `returnvalues/FwClassIds` folder. The user can add custom `CLASS_ID`s via the
+`fsfwconfig` folder.
 
-### OSAL
+# OSAL
 
 The FSFW provides operation system abstraction layers for Linux, FreeRTOS and RTEMS. 
 The OSAL provides periodic tasks, message queues, clocks and semaphores as well as mutexes.
-The [OSAL README](doc/README-osal.md#top) provides more detailed information on provided components and how to use them.
+The [OSAL README](doc/README-osal.md#top) provides more detailed information on provided components
+and how to use them.
 
-### Core Components 
+# Core Components 
 
 The FSFW has following core components. More detailed informations can be found in the
 [core component section](doc/README-core.md#top):
 
-1. Tasks: Abstraction for different (periodic) task types like periodic tasks or tasks with fixed timeslots
-2. ObjectManager: This module stores all `SystemObjects` by mapping a provided unique object ID to the object handles.
-3. Static Stores: Different stores are provided to store data of variable size (like telecommands or small telemetry) in a pool structure without
-   using dynamic memory allocation. These pools are allocated up front.
+1. Tasks: Abstraction for different (periodic) task types like periodic tasks or tasks 
+   with fixed timeslots
+2. ObjectManager: This module stores all `SystemObjects` by mapping a provided unique object ID
+   to the object handles.
+3. Static Stores: Different stores are provided to store data of variable size (like telecommands 
+   or small telemetry) in a pool structure without using dynamic memory allocation.
+   These pools are allocated up front.
 3. Clock: This module provided common time related functions
 4. EventManager: This module allows routing of events generated by `SystemObjects`
 5. HealthTable: A component which stores the health states of objects
 
-### Static IDs in the framework
+# Static IDs in the framework
 
 Some parts of the framework use a static routing address for communication. 
-An example setup of ids can be found in the example config in "defaultcft/fsfwconfig/objects/Factory::setStaticFrameworkObjectIds()".
+An example setup of ids can be found in the example config in `defaultcft/fsfwconfig/objects`
+ inside the function `Factory::setStaticFrameworkObjectIds()`.
 
-### Events
+# Events
 
 Events are tied to objects. EventIds can be generated by calling the Macro MAKE_EVENT. 
 This works analog to the returnvalues. Every object that needs own EventIds has to get a
@@ -48,25 +56,39 @@ unique SUBSYSTEM_ID. Every SystemObject can call triggerEvent from the parent cl
 Therefore, event messages contain the specific EventId and the objectId of the object that
 has triggered.
 
-### Internal Communication
+# Internal Communication
 
-Components communicate mostly over Message through Queues. 
-Those queues are created by calling the singleton QueueFactory::instance()->create(). 
+Components communicate mostly via Messages through Queues. 
+Those queues are created by calling the singleton `QueueFactory::instance()->create()` which
+will create `MessageQueue` instances for the used OSAL.
 
-### External Communication
+# External Communication
 
 The external communication with the mission control system is mostly up to the user implementation.
 The FSFW provides PUS Services which can be used to but don't need to be used. 
 The services can be seen as a conversion from a TC to a message based communication and back.
 
-#### CCSDS Frames, CCSDS Space Packets and PUS
+## TMTC Communication
+
+The FSFW provides some components to facilitate TMTC handling via the PUS commands.
+For example, a UDP or TCP PUS server socket can be opended on a specific port using the
+files located in `osal/common`. The FSFW example uses this functionality to allow sending telecommands
+and receiving telemetry using the [TMTC commander application](https://github.com/spacefisch/tmtccmd).
+Simple commands like the PUS Service 17 ping service can be tested by simply running the
+`tmtc_client_cli.py` or `tmtc_client_gui.py` utility in 
+the [example tmtc folder](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example_public/src/branch/master/tmtc). 
+
+More generally, any class responsible for handling incoming telecommands and sending telemetry
+can implement the generic `TmTcBridge` class located in `tmtcservices`.
+
+## CCSDS Frames, CCSDS Space Packets and PUS
 
 If the communication is based on CCSDS Frames and Space Packets, several classes can be used to
 distributed the packets to the corresponding services. Those can be found in `tcdistribution`. 
 If Space Packets are used, a timestamper has to be provided by the user. 
 An example can be found in the `timemanager` folder, which uses `CCSDSTime::CDS_short`.
 
-#### Device Handlers
+# Device Handlers
 
 DeviceHandlers are another important component of the FSFW. 
 The idea is, to have a software counterpart of every physical device to provide a simple mode,
@@ -74,16 +96,18 @@ health and commanding interface. By separating the underlying Communication Inte
 `DeviceCommunicationIF`, a device handler (DH) can be tested on different hardware.
 The DH has mechanisms to monitor the communication with the physical device which allow
 for FDIR reaction.  Device Handlers can be created by implementing `DeviceHandlerBase`. 
-A standard FDIR component for the DH will be created automatically but can be overwritten by the user.
-More information on DeviceHandlers can be found in the related [documentation section](doc/README-devicehandlers.md#top).
+A standard FDIR component for the DH will be created automatically but can
+be overwritten by the user. More information on DeviceHandlers can be found in the
+related [documentation section](doc/README-devicehandlers.md#top).
 
-#### Modes, Health
+# Modes and Health
 
-The two interfaces HasModesIF and HasHealthIF provide access for commanding and monitoring of components.
-On-board Mode Management is implement in hierarchy system. 
+The two interfaces `HasModesIF` and `HasHealthIF` provide access for commanding and monitoring
+of components. On-board Mode Management is implement in hierarchy system. 
 DeviceHandlers and Controllers are the lowest part of the hierarchy. 
-The next layer are Assemblies. Those assemblies act as a component which handle redundancies of handlers. 
-Assemblies share a common core with the next level which are the Subsystems. 
+The next layer are Assemblies. Those assemblies act as a component which handle
+redundancies of handlers. Assemblies share a common core with the next level which
+are the Subsystems. 
 
 Those Assemblies are intended to act as auto-generated components from a database which describes
 the subsystem modes. The definitions contain transition and target tables which contain the DH,
@@ -101,7 +125,7 @@ The health state represents if the component is able to perform its tasks.
 This can be used to signal the system to avoid using this component instead of a redundant one.
 The on-board FDIR uses the health state for isolation and recovery. 
 
-## Unit Tests
+# Unit Tests
 
 Unit Tests are provided in the unittest folder. Those use the catch2 framework but do not include
 catch2 itself. More information on how to run these tests can be found in the separate

From a8c0d96c3901d7424fdc4cb81e9d970599e3614e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 15:32:52 +0200
Subject: [PATCH 094/125] some fixes

---
 doc/README-highlevel.md | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/doc/README-highlevel.md b/doc/README-highlevel.md
index 7bf96c64..33a73239 100644
--- a/doc/README-highlevel.md
+++ b/doc/README-highlevel.md
@@ -71,12 +71,13 @@ The services can be seen as a conversion from a TC to a message based communicat
 ## TMTC Communication
 
 The FSFW provides some components to facilitate TMTC handling via the PUS commands.
-For example, a UDP or TCP PUS server socket can be opended on a specific port using the
+For example, a UDP or TCP PUS server socket can be opened on a specific port using the
 files located in `osal/common`. The FSFW example uses this functionality to allow sending telecommands
 and receiving telemetry using the [TMTC commander application](https://github.com/spacefisch/tmtccmd).
 Simple commands like the PUS Service 17 ping service can be tested by simply running the
 `tmtc_client_cli.py` or `tmtc_client_gui.py` utility in 
-the [example tmtc folder](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example_public/src/branch/master/tmtc). 
+the [example tmtc folder](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example_public/src/branch/master/tmtc)
+while the `fsfw_example` application is running.
 
 More generally, any class responsible for handling incoming telecommands and sending telemetry
 can implement the generic `TmTcBridge` class located in `tmtcservices`.

From e147d5a4f58650adfb2c1466c437173f573723ee Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 15:34:31 +0200
Subject: [PATCH 095/125] some more information

---
 doc/README-highlevel.md | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/doc/README-highlevel.md b/doc/README-highlevel.md
index 33a73239..262138a7 100644
--- a/doc/README-highlevel.md
+++ b/doc/README-highlevel.md
@@ -80,7 +80,9 @@ the [example tmtc folder](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example_pu
 while the `fsfw_example` application is running.
 
 More generally, any class responsible for handling incoming telecommands and sending telemetry
-can implement the generic `TmTcBridge` class located in `tmtcservices`.
+can implement the generic `TmTcBridge` class located in `tmtcservices`. Many applications
+also use a dedicated polling task for reading telecommands which passes telecommands
+to the `TmTcBridge` implementation.
 
 ## CCSDS Frames, CCSDS Space Packets and PUS
 

From b7060a9c78e57f755f9c04de37e8195ddc83194e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 15:35:47 +0200
Subject: [PATCH 096/125] update README

---
 README.md | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/README.md b/README.md
index fb3be429..484d65c0 100644
--- a/README.md
+++ b/README.md
@@ -38,11 +38,12 @@ a starting point. The [configuration section](doc/README-config.md#top) provides
 
 [1. High-level overview](doc/README-highlevel.md#top) <br>
 [2. Core components](doc/README-core.md#top) <br>
-[3. OSAL overview](doc/README-osal.md#top) <br>
-[4. PUS services](doc/README-pus.md#top) <br>
-[5. Device Handler overview](doc/README-devicehandlers.md#top) <br>
-[6. Controller overview](doc/README-controllers.md#top) <br>
-[7. Local Data Pools](doc/README-localpools.md#top) <br>
+[3. Configuration](doc/README-config.md#top) <br>
+[4. OSAL overview](doc/README-osal.md#top) <br>
+[5. PUS services](doc/README-pus.md#top) <br>
+[6. Device Handler overview](doc/README-devicehandlers.md#top) <br>
+[7. Controller overview](doc/README-controllers.md#top) <br>
+[8. Local Data Pools](doc/README-localpools.md#top) <br>
 
 
 

From 36030ef87c96b0fce7664d8c57e12f540fa43090 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 18 May 2021 15:37:05 +0200
Subject: [PATCH 097/125] typo

---
 doc/README-config.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/doc/README-config.md b/doc/README-config.md
index 6afba83c..d71feb97 100644
--- a/doc/README-config.md
+++ b/doc/README-config.md
@@ -9,7 +9,7 @@ of the available options will be explained in more detail here.
 # Auto-Translation of Events
 
 The FSFW allows the automatic translation of events, which allows developers to track triggered
-events directly via consoke output. Using this feature requires:
+events directly via console output. Using this feature requires:
 
 1. `FSFW_OBJ_EVENT_TRANSLATION` set to 1 in the configuration file.
 2. Special auto-generated translation files which translate event IDs and object IDs into

From 5d2c62e75de447a3aed1fde78e8654f9ce8e1205 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 26 May 2021 14:15:48 +0200
Subject: [PATCH 098/125] abstract function defined again

---
 controller/ExtendedControllerBase.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/controller/ExtendedControllerBase.h b/controller/ExtendedControllerBase.h
index d5d43933..63c350e2 100644
--- a/controller/ExtendedControllerBase.h
+++ b/controller/ExtendedControllerBase.h
@@ -66,6 +66,10 @@ protected:
     virtual ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
             LocalDataPoolManager& poolManager) override = 0;
     virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override = 0;
+
+    // Mode abstract functions
+    virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
+            uint32_t *msToReachTheMode) override = 0;
 };
 
 

From 2bf5a972e177676b30d83178488b3da01f14e8d1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 11:56:52 +0200
Subject: [PATCH 099/125] introduced std check

---
 CMakeLists.txt | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8ba6a187..00c0bc95 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,6 +18,13 @@ add_library(${LIB_FSFW_NAME})
 
 set_property(CACHE OS_FSFW PROPERTY STRINGS host linux rtems freertos)
 
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 11)
+    set(CMAKE_CXX_STANDARD_REQUIRED True)
+elseif(${CMAKE_CXX_STANDARD} LESS 11)
+    message(FATAL_ERROR "Compiling the FSFW requires a minimum of C++11 support")
+endif()
+
 if(NOT OS_FSFW)
 	message(STATUS "No OS for FSFW via OS_FSFW set. Assuming host OS")
 	# Assume host OS and autodetermine from OS_FSFW

From 5eadcaf10d03abfec13ddd57f7134c1588f88532 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 12:46:45 +0200
Subject: [PATCH 100/125] file system message update

---
 memory/CMakeLists.txt               |  8 +--
 memory/GenericFileSystemMessage.cpp | 62 ++++++++++++++++++++++
 memory/GenericFileSystemMessage.h   | 80 +++++++++++++++++++++++++++++
 3 files changed, 146 insertions(+), 4 deletions(-)
 create mode 100644 memory/GenericFileSystemMessage.cpp
 create mode 100644 memory/GenericFileSystemMessage.h

diff --git a/memory/CMakeLists.txt b/memory/CMakeLists.txt
index 9edb9031..c713cd42 100644
--- a/memory/CMakeLists.txt
+++ b/memory/CMakeLists.txt
@@ -1,5 +1,5 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		MemoryHelper.cpp
-		MemoryMessage.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE
+    MemoryHelper.cpp
+    MemoryMessage.cpp
+    GenericFileSystemMessage.cpp
 )
\ No newline at end of file
diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
new file mode 100644
index 00000000..a0ce8d59
--- /dev/null
+++ b/memory/GenericFileSystemMessage.cpp
@@ -0,0 +1,62 @@
+#include "GenericFileSystemMessage.h"
+
+
+void GenericFileSystemMessage::setCreateFileCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_CREATE_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setDeleteFileCommand(
+        CommandMessage* message, store_address_t storeId) {
+    message->setCommand(CMD_DELETE_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setCreateDirectoryCommand(
+        CommandMessage* message, store_address_t storeId) {
+    message->setCommand(CMD_CREATE_DIRECTORY);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setReportFileAttributesCommand(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(CMD_REPORT_FILE_ATTRIBUTES);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setReportFileAttributesReply(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(REPLY_REPORT_FILE_ATTRIBUTES);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setDeleteDirectoryCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_DELETE_DIRECTORY);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setLockFileCommand(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(CMD_LOCK_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setUnlockFileCommand(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(CMD_UNLOCK_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setSuccessReply(CommandMessage *message) {
+    message->setCommand(COMPLETION_SUCCESS);
+}
+
+void GenericFileSystemMessage::setFailureReply(CommandMessage *message,
+        ReturnValue_t errorCode, uint32_t errorParam) {
+    message->setCommand(COMPLETION_FAILED);
+    message->setParameter(errorCode);
+    message->setParameter2(errorParam);
+}
+
diff --git a/memory/GenericFileSystemMessage.h b/memory/GenericFileSystemMessage.h
new file mode 100644
index 00000000..174431e4
--- /dev/null
+++ b/memory/GenericFileSystemMessage.h
@@ -0,0 +1,80 @@
+#ifndef MISSION_MEMORY_GENERICFILESYSTEMMESSAGE_H_
+#define MISSION_MEMORY_GENERICFILESYSTEMMESSAGE_H_
+
+#include <cstdint>
+
+#include <fsfw/ipc/CommandMessageIF.h>
+#include <fsfw/ipc/FwMessageTypes.h>
+#include <fsfw/ipc/CommandMessage.h>
+#include <fsfw/storagemanager/storeAddress.h>
+
+/**
+ * @brief   These messages are sent to an object implementing HasFilesystemIF.
+ * @details
+ * Enables a message-based file management. The user can add custo commands be implementing
+ * this generic class.
+ * @author  Jakob Meier, R. Mueller
+ */
+class GenericFileSystemMessage {
+public:
+    /* Instantiation forbidden */
+    GenericFileSystemMessage() = delete;
+
+    static const uint8_t MESSAGE_ID = messagetypes::FILE_SYSTEM_MESSAGE;
+    /* PUS standard  (ECSS-E-ST-70-41C15 2016 p.654) */
+    static const Command_t CMD_CREATE_FILE = MAKE_COMMAND_ID(1);
+    static const Command_t CMD_DELETE_FILE = MAKE_COMMAND_ID(2);
+    /** Report file attributes */
+    static const Command_t CMD_REPORT_FILE_ATTRIBUTES = MAKE_COMMAND_ID(3);
+    static const Command_t REPLY_REPORT_FILE_ATTRIBUTES = MAKE_COMMAND_ID(4);
+    /** Command to lock a file, setting it read-only */
+    static const Command_t CMD_LOCK_FILE = MAKE_COMMAND_ID(5);
+    /** Command to unlock a file, enabling further operations on it */
+    static const Command_t CMD_UNLOCK_FILE = MAKE_COMMAND_ID(6);
+    /**
+     * Find file in repository, using a search pattern.
+     * Please note that * is the wildcard character.
+     * For example, when looking for all files which start with have the
+     * structure tm<something>.bin, tm*.bin can be used.
+     */
+    static const Command_t CMD_FIND_FILE = MAKE_COMMAND_ID(7);
+    static const Command_t CMD_CREATE_DIRECTORY = MAKE_COMMAND_ID(9);
+    static const Command_t CMD_DELETE_DIRECTORY = MAKE_COMMAND_ID(10);
+    static const Command_t CMD_RENAME_DIRECTORY = MAKE_COMMAND_ID(11);
+
+    /** Dump contents of a repository */
+    static const Command_t CMD_DUMP_REPOSITORY = MAKE_COMMAND_ID(12);
+    /** Repository dump reply */
+    static const Command_t REPLY_DUMY_REPOSITORY = MAKE_COMMAND_ID(13);
+    static constexpr Command_t CMD_COPY_FILE = MAKE_COMMAND_ID(14);
+    static constexpr Command_t CMD_MOVE_FILE = MAKE_COMMAND_ID(15);
+
+    static const Command_t COMPLETION_SUCCESS = MAKE_COMMAND_ID(128);
+    static const Command_t COMPLETION_FAILED = MAKE_COMMAND_ID(129);
+
+    static void setLockFileCommand(CommandMessage* message, store_address_t storeId);
+    static void setUnlockFileCommand(CommandMessage* message, store_address_t storeId);
+
+    static void setCreateFileCommand(CommandMessage* message,
+            store_address_t storeId);
+    static void setDeleteFileCommand(CommandMessage* message,
+            store_address_t storeId);
+
+    static void setReportFileAttributesCommand(CommandMessage* message,
+            store_address_t storeId);
+    static void setReportFileAttributesReply(CommandMessage* message,
+            store_address_t storeId);
+
+    static void setCreateDirectoryCommand(CommandMessage* message,
+            store_address_t storeId);
+    static void setDeleteDirectoryCommand(CommandMessage* message,
+            store_address_t storeId);
+
+    static void setSuccessReply(CommandMessage* message);
+    static void setFailureReply(CommandMessage* message,
+            ReturnValue_t errorCode, uint32_t errorParam = 0);
+};
+
+
+
+#endif /* MISSION_MEMORY_GENERICFILESYSTEMMESSAGE_H_ */

From 59e7f0caae5375913ed93fa14710d3ccdcdebea7 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 12:50:48 +0200
Subject: [PATCH 101/125] added more messages

---
 memory/GenericFileSystemMessage.cpp | 70 +++++++++++++++++++++++++++++
 memory/GenericFileSystemMessage.h   | 37 ++++++++++++++-
 2 files changed, 105 insertions(+), 2 deletions(-)

diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
index a0ce8d59..d9355567 100644
--- a/memory/GenericFileSystemMessage.cpp
+++ b/memory/GenericFileSystemMessage.cpp
@@ -60,3 +60,73 @@ void GenericFileSystemMessage::setFailureReply(CommandMessage *message,
     message->setParameter2(errorParam);
 }
 
+store_address_t GenericFileSystemMessage::getStoreId(const CommandMessage* message) {
+    store_address_t temp;
+    temp.raw = message->getParameter2();
+    return temp;
+}
+
+ReturnValue_t GenericFileSystemMessage::getFailureReply(
+        const CommandMessage *message, uint32_t* errorParam) {
+    if(errorParam != nullptr) {
+        *errorParam = message->getParameter2();
+    }
+    return message->getParameter();
+}
+
+void GenericFileSystemMessage::setFinishStopWriteCommand(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(CMD_FINISH_APPEND_TO_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setFinishStopWriteReply(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(REPLY_FINISH_APPEND);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setCopyCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_COPY_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setWriteCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_APPEND_TO_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setReadCommand(CommandMessage* message,
+        store_address_t storeId) {
+    message->setCommand(CMD_READ_FROM_FILE);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setFinishAppendReply(CommandMessage* message,
+        store_address_t storageID) {
+    message->setCommand(REPLY_FINISH_APPEND);
+    message->setParameter2(storageID.raw);
+}
+
+void GenericFileSystemMessage::setReadReply(CommandMessage* message,
+        bool readFinished, store_address_t storeId) {
+    message->setCommand(REPLY_READ_FROM_FILE);
+    message->setParameter(readFinished);
+    message->setParameter2(storeId.raw);
+}
+
+void GenericFileSystemMessage::setReadFinishedReply(CommandMessage *message,
+        store_address_t storeId) {
+    message->setCommand(REPLY_READ_FINISHED_STOP);
+    message->setParameter2(storeId.raw);
+}
+
+bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
+        store_address_t *storeId) {
+    if(storeId != nullptr) {
+        (*storeId).raw = message->getParameter2();
+    }
+    return message->getParameter();
+}
diff --git a/memory/GenericFileSystemMessage.h b/memory/GenericFileSystemMessage.h
index 174431e4..1a19a69b 100644
--- a/memory/GenericFileSystemMessage.h
+++ b/memory/GenericFileSystemMessage.h
@@ -52,6 +52,17 @@ public:
     static const Command_t COMPLETION_SUCCESS = MAKE_COMMAND_ID(128);
     static const Command_t COMPLETION_FAILED = MAKE_COMMAND_ID(129);
 
+    // These command IDs will remain until CFDP has been introduced and consolidated.
+    /** Append operation commands */
+    static const Command_t CMD_APPEND_TO_FILE = MAKE_COMMAND_ID(130);
+    static const Command_t CMD_FINISH_APPEND_TO_FILE = MAKE_COMMAND_ID(131);
+    static const Command_t REPLY_FINISH_APPEND = MAKE_COMMAND_ID(132);
+
+    static const Command_t CMD_READ_FROM_FILE = MAKE_COMMAND_ID(140);
+    static const Command_t REPLY_READ_FROM_FILE = MAKE_COMMAND_ID(141);
+    static const Command_t CMD_STOP_READ = MAKE_COMMAND_ID(142);
+    static const Command_t REPLY_READ_FINISHED_STOP = MAKE_COMMAND_ID(143);
+
     static void setLockFileCommand(CommandMessage* message, store_address_t storeId);
     static void setUnlockFileCommand(CommandMessage* message, store_address_t storeId);
 
@@ -73,8 +84,30 @@ public:
     static void setSuccessReply(CommandMessage* message);
     static void setFailureReply(CommandMessage* message,
             ReturnValue_t errorCode, uint32_t errorParam = 0);
+    static void setCopyCommand(CommandMessage* message, store_address_t storeId);
+
+    static void setWriteCommand(CommandMessage* message,
+            store_address_t storeId);
+    static void setFinishStopWriteCommand(CommandMessage* message,
+            store_address_t storeId);
+    static void setFinishStopWriteReply(CommandMessage* message,
+            store_address_t storeId);
+    static void setFinishAppendReply(CommandMessage* message,
+            store_address_t storeId);
+
+    static void setReadCommand(CommandMessage* message,
+            store_address_t storeId);
+    static void setReadFinishedReply(CommandMessage* message,
+            store_address_t storeId);
+    static void setReadReply(CommandMessage* message, bool readFinished,
+            store_address_t storeId);
+    static bool getReadReply(const CommandMessage* message,
+            store_address_t* storeId);
+
+    static store_address_t getStoreId(const CommandMessage* message);
+    static ReturnValue_t getFailureReply(const CommandMessage* message,
+            uint32_t* errorParam = nullptr);
+
 };
 
-
-
 #endif /* MISSION_MEMORY_GENERICFILESYSTEMMESSAGE_H_ */

From e15f03fb0abf2484b03fe15ef923d868f8a74bde Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 13:12:34 +0200
Subject: [PATCH 102/125] added FreeRTOS queue map manager

---
 osal/FreeRTOS/CMakeLists.txt      |  1 +
 osal/FreeRTOS/QueueMapManager.cpp | 49 +++++++++++++++++++++++
 osal/FreeRTOS/QueueMapManager.h   | 50 ++++++++++++++++++++++++
 osal/host/QueueMapManager.cpp     | 65 +++++++++++++++----------------
 osal/host/QueueMapManager.h       | 49 ++++++++++++-----------
 5 files changed, 157 insertions(+), 57 deletions(-)
 create mode 100644 osal/FreeRTOS/QueueMapManager.cpp
 create mode 100644 osal/FreeRTOS/QueueMapManager.h

diff --git a/osal/FreeRTOS/CMakeLists.txt b/osal/FreeRTOS/CMakeLists.txt
index 95462010..4da24a71 100644
--- a/osal/FreeRTOS/CMakeLists.txt
+++ b/osal/FreeRTOS/CMakeLists.txt
@@ -15,6 +15,7 @@ target_sources(${LIB_FSFW_NAME}
 		TaskFactory.cpp
 		Timekeeper.cpp
 		TaskManagement.cpp
+		QueueMapManager.cpp
 )
 
 # FreeRTOS is required to link the FSFW now. It is recommended to compile 
diff --git a/osal/FreeRTOS/QueueMapManager.cpp b/osal/FreeRTOS/QueueMapManager.cpp
new file mode 100644
index 00000000..520e54cd
--- /dev/null
+++ b/osal/FreeRTOS/QueueMapManager.cpp
@@ -0,0 +1,49 @@
+#include "QueueMapManager.h"
+#include "../../ipc/MutexFactory.h"
+#include "../../ipc/MutexGuard.h"
+
+QueueMapManager::QueueMapManager() {
+    mapLock = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t QueueMapManager::addMessageQueue(QueueHandle_t queue, MessageQueueId_t* id) {
+    MutexGuard lock(mapLock);
+    uint32_t currentId = queueCounter++;
+    auto returnPair = queueMap.emplace(currentId, queue);
+    if(not returnPair.second) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "QueueMapManager::addMessageQueue This ID is already "
+                "inside the map!" << std::endl;
+#else
+        sif::printError("QueueMapManager::addMessageQueue This ID is already "
+                "inside the map!\n");
+#endif
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    if (id != nullptr) {
+        *id = currentId;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+
+}
+
+QueueHandle_t QueueMapManager::getMessageQueue(MessageQueueId_t messageQueueId) const {
+    auto queueIter = queueMap.find(messageQueueId);
+    if(queueIter != queueMap.end()) {
+        return queueIter->second;
+    }
+    else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "QueueMapManager::getQueueHandle: The ID " << messageQueueId <<
+                " does not exists in the map!" << std::endl;
+#else
+        sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
+                messageQueueId);
+#endif
+    }
+    return nullptr;
+}
+
+QueueMapManager::~QueueMapManager() {
+    MutexFactory::instance()->deleteMutex(mapLock);
+}
diff --git a/osal/FreeRTOS/QueueMapManager.h b/osal/FreeRTOS/QueueMapManager.h
new file mode 100644
index 00000000..91a839f0
--- /dev/null
+++ b/osal/FreeRTOS/QueueMapManager.h
@@ -0,0 +1,50 @@
+#ifndef FSFW_OSAL_FREERTOS_QUEUEMAPMANAGER_H_
+#define FSFW_OSAL_FREERTOS_QUEUEMAPMANAGER_H_
+
+#include "../../ipc/MutexIF.h"
+#include "../../ipc/messageQueueDefinitions.h"
+#include "../../ipc/MessageQueueIF.h"
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+
+#include <map>
+
+using QueueMap = std::map<MessageQueueId_t, QueueHandle_t>;
+
+class QueueMapManager {
+public:
+
+    //! Returns the single instance of QueueMapManager
+    static QueueMapManager* instance();
+
+    /**
+     * Insert a message queue and the corresponding QueueHandle into the map
+     * @param queue The message queue to insert.
+     * @param id The passed value will be set unless a nullptr is passed
+     * @return
+     */
+    ReturnValue_t addMessageQueue(QueueHandle_t queue, MessageQueueId_t* id);
+
+    /**
+     * Get the message queue handle by providing a message queue ID. Returns nullptr
+     * if the queue ID does not exist in the internal map.
+     * @param messageQueueId
+     * @return
+     */
+    QueueHandle_t getMessageQueue(MessageQueueId_t messageQueueId) const;
+
+private:
+    //! External instantiation forbidden. Constructor still required for singleton instantiation.
+    QueueMapManager();
+    ~QueueMapManager();
+
+    uint32_t queueCounter = 0;
+    MutexIF* mapLock;
+    QueueMap queueMap;
+    static QueueMapManager* mqManagerInstance;
+};
+
+
+
+#endif /* FSFW_OSAL_FREERTOS_QUEUEMAPMANAGER_H_ */
diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp
index c9100fe9..879bc36d 100644
--- a/osal/host/QueueMapManager.cpp
+++ b/osal/host/QueueMapManager.cpp
@@ -15,52 +15,49 @@ QueueMapManager::~QueueMapManager() {
 }
 
 QueueMapManager* QueueMapManager::instance() {
-	if (mqManagerInstance == nullptr){
-		mqManagerInstance = new QueueMapManager();
-	}
-	return QueueMapManager::mqManagerInstance;
+    if (mqManagerInstance == nullptr){
+        mqManagerInstance = new QueueMapManager();
+    }
+    return QueueMapManager::mqManagerInstance;
 }
 
 ReturnValue_t QueueMapManager::addMessageQueue(
-		MessageQueueIF* queueToInsert, MessageQueueId_t* id) {
-	/* Not thread-safe, but it is assumed all message queues are created at software initialization
-	now. If this is to be made thread-safe in the future, it propably would be sufficient to lock
-	the increment operation here. */
-	uint32_t currentId = queueCounter++;
-	auto returnPair = queueMap.emplace(currentId, queueToInsert);
-	if(not returnPair.second) {
-		/* This should never happen for the atomic variable. */
+        MessageQueueIF* queueToInsert, MessageQueueId_t* id) {
+    MutexGuard lock(mapLock);
+    uint32_t currentId = queueCounter++;
+    auto returnPair = queueMap.emplace(currentId, queueToInsert);
+    if(not returnPair.second) {
+        /* This should never happen for the atomic variable. */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "QueueMapManager::addMessageQueue This ID is already "
-		        "inside the map!" << std::endl;
+        sif::error << "QueueMapManager::addMessageQueue This ID is already "
+                "inside the map!" << std::endl;
 #else
-		sif::printError("QueueMapManager::addMessageQueue This ID is already "
+        sif::printError("QueueMapManager::addMessageQueue This ID is already "
                 "inside the map!\n");
 #endif
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	if (id != nullptr) {
-		*id = currentId;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    if (id != nullptr) {
+        *id = currentId;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 MessageQueueIF* QueueMapManager::getMessageQueue(
-		MessageQueueId_t messageQueueId) const {
-	MutexGuard(mapLock, MutexIF::TimeoutType::WAITING, 50);
-	auto queueIter = queueMap.find(messageQueueId);
-	if(queueIter != queueMap.end()) {
-		return queueIter->second;
-	}
-	else {
+        MessageQueueId_t messageQueueId) const {
+    auto queueIter = queueMap.find(messageQueueId);
+    if(queueIter != queueMap.end()) {
+        return queueIter->second;
+    }
+    else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "QueueMapManager::getQueueHandle: The ID " << messageQueueId <<
-		        " does not exists in the map!" << std::endl;
+        sif::warning << "QueueMapManager::getQueueHandle: The ID " << messageQueueId <<
+                " does not exists in the map!" << std::endl;
 #else
-		sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
-		        messageQueueId);
+        sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
+                messageQueueId);
 #endif
-		return nullptr;
-	}
+    }
+    return nullptr;
 }
 
diff --git a/osal/host/QueueMapManager.h b/osal/host/QueueMapManager.h
index 90c39c2f..e274bed2 100644
--- a/osal/host/QueueMapManager.h
+++ b/osal/host/QueueMapManager.h
@@ -15,33 +15,36 @@ using QueueMap = std::unordered_map<MessageQueueId_t, MessageQueueIF*>;
  */
 class QueueMapManager {
 public:
-	//! Returns the single instance of SemaphoreFactory.
-	static QueueMapManager* instance();
 
-	/**
-	 * Insert a message queue into the map and returns a message queue ID
-	 * @param queue The message queue to insert.
-	 * @param id The passed value will be set unless a nullptr is passed
-	 * @return
-	 */
-	ReturnValue_t addMessageQueue(MessageQueueIF* queue, MessageQueueId_t*
-			id = nullptr);
-	/**
-	 * Get the message queue handle by providing a message queue ID.
-	 * @param messageQueueId
-	 * @return
-	 */
-	MessageQueueIF* getMessageQueue(MessageQueueId_t messageQueueId) const;
+
+    //! Returns the single instance of QueueMapManager.
+    static QueueMapManager* instance();
+
+    /**
+     * Insert a message queue into the map and returns a message queue ID
+     * @param queue The message queue to insert.
+     * @param id The passed value will be set unless a nullptr is passed
+     * @return
+     */
+    ReturnValue_t addMessageQueue(MessageQueueIF* queue, MessageQueueId_t*
+            id = nullptr);
+    /**
+     * Get the message queue handle by providing a message queue ID. Returns nullptr
+     * if the queue ID is not contained inside the internal map.
+     * @param messageQueueId
+     * @return
+     */
+    MessageQueueIF* getMessageQueue(MessageQueueId_t messageQueueId) const;
 
 private:
-	//! External instantiation is forbidden.
-	QueueMapManager();
-	~QueueMapManager();
+    //! External instantiation is forbidden. Constructor still required for singleton instantiation.
+    QueueMapManager();
+    ~QueueMapManager();
 
-	uint32_t queueCounter = 0;
-	MutexIF* mapLock;
-	QueueMap queueMap;
-	static QueueMapManager* mqManagerInstance;
+    uint32_t queueCounter = 0;
+    MutexIF* mapLock;
+    QueueMap queueMap;
+    static QueueMapManager* mqManagerInstance;
 };
 
 

From e6868464bfeb7fd7150b5373b895474e33c877bc Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 13:38:40 +0200
Subject: [PATCH 103/125] 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 3a0f654e..a74d32ea 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<InternalErrorReporterIF>(
-					objects::INTERNAL_ERROR_REPORTER);
+			        get<InternalErrorReporterIF>(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 or sendTo == 0x00) {
-		return MessageQueueIF::DESTINATION_INVALID;
+	if(sendTo == MessageQueueIF::NO_QUEUE) {
+        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,
-                static_cast<const void*>(message->getBuffer()), 0);
+        result = xQueueSendToBack(destination, static_cast<const void*>(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<QueueHandle_t>(sendTo),
-                static_cast<const void*>(message->getBuffer()),
-                &xHigherPriorityTaskWoken);
+                static_cast<const void*>(message->getBuffer()), &xHigherPriorityTaskWoken);
         if(xHigherPriorityTaskWoken == pdTRUE) {
             TaskManagement::requestContextSwitch(callContext);
         }
diff --git a/osal/FreeRTOS/MessageQueue.h b/osal/FreeRTOS/MessageQueue.h
index 8fa86283..9cee5b22 100644
--- a/osal/FreeRTOS/MessageQueue.h
+++ b/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;
diff --git a/osal/FreeRTOS/QueueMapManager.cpp b/osal/FreeRTOS/QueueMapManager.cpp
index 520e54cd..51cfe11d 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++;

From 46cfe7452a198d49528f2c51e1ed4044cae9d933 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 13:40:01 +0200
Subject: [PATCH 104/125] for for ISR variant

---
 osal/FreeRTOS/MessageQueue.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index a74d32ea..b5c9035d 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -148,8 +148,8 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         /* 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<QueueHandle_t>(sendTo),
-                static_cast<const void*>(message->getBuffer()), &xHigherPriorityTaskWoken);
+        result = xQueueSendFromISR(destination, static_cast<const void*>(message->getBuffer()),
+                &xHigherPriorityTaskWoken);
         if(xHigherPriorityTaskWoken == pdTRUE) {
             TaskManagement::requestContextSwitch(callContext);
         }

From aab3fd828b5aaba0b7b4a81ea2a076d97b369fc1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 13:43:13 +0200
Subject: [PATCH 105/125] removed unrelated changes

---
 memory/CMakeLists.txt               |   8 +-
 memory/GenericFileSystemMessage.cpp | 132 ----------------------------
 memory/GenericFileSystemMessage.h   | 113 ------------------------
 3 files changed, 4 insertions(+), 249 deletions(-)
 delete mode 100644 memory/GenericFileSystemMessage.cpp
 delete mode 100644 memory/GenericFileSystemMessage.h

diff --git a/memory/CMakeLists.txt b/memory/CMakeLists.txt
index c713cd42..9edb9031 100644
--- a/memory/CMakeLists.txt
+++ b/memory/CMakeLists.txt
@@ -1,5 +1,5 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE
-    MemoryHelper.cpp
-    MemoryMessage.cpp
-    GenericFileSystemMessage.cpp
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		MemoryHelper.cpp
+		MemoryMessage.cpp
 )
\ No newline at end of file
diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
deleted file mode 100644
index d9355567..00000000
--- a/memory/GenericFileSystemMessage.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-#include "GenericFileSystemMessage.h"
-
-
-void GenericFileSystemMessage::setCreateFileCommand(CommandMessage* message,
-        store_address_t storeId) {
-    message->setCommand(CMD_CREATE_FILE);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setDeleteFileCommand(
-        CommandMessage* message, store_address_t storeId) {
-    message->setCommand(CMD_DELETE_FILE);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setCreateDirectoryCommand(
-        CommandMessage* message, store_address_t storeId) {
-    message->setCommand(CMD_CREATE_DIRECTORY);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setReportFileAttributesCommand(CommandMessage *message,
-        store_address_t storeId) {
-    message->setCommand(CMD_REPORT_FILE_ATTRIBUTES);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setReportFileAttributesReply(CommandMessage *message,
-        store_address_t storeId) {
-    message->setCommand(REPLY_REPORT_FILE_ATTRIBUTES);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setDeleteDirectoryCommand(CommandMessage* message,
-        store_address_t storeId) {
-    message->setCommand(CMD_DELETE_DIRECTORY);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setLockFileCommand(CommandMessage *message,
-        store_address_t storeId) {
-    message->setCommand(CMD_LOCK_FILE);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setUnlockFileCommand(CommandMessage *message,
-        store_address_t storeId) {
-    message->setCommand(CMD_UNLOCK_FILE);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setSuccessReply(CommandMessage *message) {
-    message->setCommand(COMPLETION_SUCCESS);
-}
-
-void GenericFileSystemMessage::setFailureReply(CommandMessage *message,
-        ReturnValue_t errorCode, uint32_t errorParam) {
-    message->setCommand(COMPLETION_FAILED);
-    message->setParameter(errorCode);
-    message->setParameter2(errorParam);
-}
-
-store_address_t GenericFileSystemMessage::getStoreId(const CommandMessage* message) {
-    store_address_t temp;
-    temp.raw = message->getParameter2();
-    return temp;
-}
-
-ReturnValue_t GenericFileSystemMessage::getFailureReply(
-        const CommandMessage *message, uint32_t* errorParam) {
-    if(errorParam != nullptr) {
-        *errorParam = message->getParameter2();
-    }
-    return message->getParameter();
-}
-
-void GenericFileSystemMessage::setFinishStopWriteCommand(CommandMessage *message,
-        store_address_t storeId) {
-    message->setCommand(CMD_FINISH_APPEND_TO_FILE);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setFinishStopWriteReply(CommandMessage *message,
-        store_address_t storeId) {
-    message->setCommand(REPLY_FINISH_APPEND);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setCopyCommand(CommandMessage* message,
-        store_address_t storeId) {
-    message->setCommand(CMD_COPY_FILE);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setWriteCommand(CommandMessage* message,
-        store_address_t storeId) {
-    message->setCommand(CMD_APPEND_TO_FILE);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setReadCommand(CommandMessage* message,
-        store_address_t storeId) {
-    message->setCommand(CMD_READ_FROM_FILE);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setFinishAppendReply(CommandMessage* message,
-        store_address_t storageID) {
-    message->setCommand(REPLY_FINISH_APPEND);
-    message->setParameter2(storageID.raw);
-}
-
-void GenericFileSystemMessage::setReadReply(CommandMessage* message,
-        bool readFinished, store_address_t storeId) {
-    message->setCommand(REPLY_READ_FROM_FILE);
-    message->setParameter(readFinished);
-    message->setParameter2(storeId.raw);
-}
-
-void GenericFileSystemMessage::setReadFinishedReply(CommandMessage *message,
-        store_address_t storeId) {
-    message->setCommand(REPLY_READ_FINISHED_STOP);
-    message->setParameter2(storeId.raw);
-}
-
-bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
-        store_address_t *storeId) {
-    if(storeId != nullptr) {
-        (*storeId).raw = message->getParameter2();
-    }
-    return message->getParameter();
-}
diff --git a/memory/GenericFileSystemMessage.h b/memory/GenericFileSystemMessage.h
deleted file mode 100644
index 1a19a69b..00000000
--- a/memory/GenericFileSystemMessage.h
+++ /dev/null
@@ -1,113 +0,0 @@
-#ifndef MISSION_MEMORY_GENERICFILESYSTEMMESSAGE_H_
-#define MISSION_MEMORY_GENERICFILESYSTEMMESSAGE_H_
-
-#include <cstdint>
-
-#include <fsfw/ipc/CommandMessageIF.h>
-#include <fsfw/ipc/FwMessageTypes.h>
-#include <fsfw/ipc/CommandMessage.h>
-#include <fsfw/storagemanager/storeAddress.h>
-
-/**
- * @brief   These messages are sent to an object implementing HasFilesystemIF.
- * @details
- * Enables a message-based file management. The user can add custo commands be implementing
- * this generic class.
- * @author  Jakob Meier, R. Mueller
- */
-class GenericFileSystemMessage {
-public:
-    /* Instantiation forbidden */
-    GenericFileSystemMessage() = delete;
-
-    static const uint8_t MESSAGE_ID = messagetypes::FILE_SYSTEM_MESSAGE;
-    /* PUS standard  (ECSS-E-ST-70-41C15 2016 p.654) */
-    static const Command_t CMD_CREATE_FILE = MAKE_COMMAND_ID(1);
-    static const Command_t CMD_DELETE_FILE = MAKE_COMMAND_ID(2);
-    /** Report file attributes */
-    static const Command_t CMD_REPORT_FILE_ATTRIBUTES = MAKE_COMMAND_ID(3);
-    static const Command_t REPLY_REPORT_FILE_ATTRIBUTES = MAKE_COMMAND_ID(4);
-    /** Command to lock a file, setting it read-only */
-    static const Command_t CMD_LOCK_FILE = MAKE_COMMAND_ID(5);
-    /** Command to unlock a file, enabling further operations on it */
-    static const Command_t CMD_UNLOCK_FILE = MAKE_COMMAND_ID(6);
-    /**
-     * Find file in repository, using a search pattern.
-     * Please note that * is the wildcard character.
-     * For example, when looking for all files which start with have the
-     * structure tm<something>.bin, tm*.bin can be used.
-     */
-    static const Command_t CMD_FIND_FILE = MAKE_COMMAND_ID(7);
-    static const Command_t CMD_CREATE_DIRECTORY = MAKE_COMMAND_ID(9);
-    static const Command_t CMD_DELETE_DIRECTORY = MAKE_COMMAND_ID(10);
-    static const Command_t CMD_RENAME_DIRECTORY = MAKE_COMMAND_ID(11);
-
-    /** Dump contents of a repository */
-    static const Command_t CMD_DUMP_REPOSITORY = MAKE_COMMAND_ID(12);
-    /** Repository dump reply */
-    static const Command_t REPLY_DUMY_REPOSITORY = MAKE_COMMAND_ID(13);
-    static constexpr Command_t CMD_COPY_FILE = MAKE_COMMAND_ID(14);
-    static constexpr Command_t CMD_MOVE_FILE = MAKE_COMMAND_ID(15);
-
-    static const Command_t COMPLETION_SUCCESS = MAKE_COMMAND_ID(128);
-    static const Command_t COMPLETION_FAILED = MAKE_COMMAND_ID(129);
-
-    // These command IDs will remain until CFDP has been introduced and consolidated.
-    /** Append operation commands */
-    static const Command_t CMD_APPEND_TO_FILE = MAKE_COMMAND_ID(130);
-    static const Command_t CMD_FINISH_APPEND_TO_FILE = MAKE_COMMAND_ID(131);
-    static const Command_t REPLY_FINISH_APPEND = MAKE_COMMAND_ID(132);
-
-    static const Command_t CMD_READ_FROM_FILE = MAKE_COMMAND_ID(140);
-    static const Command_t REPLY_READ_FROM_FILE = MAKE_COMMAND_ID(141);
-    static const Command_t CMD_STOP_READ = MAKE_COMMAND_ID(142);
-    static const Command_t REPLY_READ_FINISHED_STOP = MAKE_COMMAND_ID(143);
-
-    static void setLockFileCommand(CommandMessage* message, store_address_t storeId);
-    static void setUnlockFileCommand(CommandMessage* message, store_address_t storeId);
-
-    static void setCreateFileCommand(CommandMessage* message,
-            store_address_t storeId);
-    static void setDeleteFileCommand(CommandMessage* message,
-            store_address_t storeId);
-
-    static void setReportFileAttributesCommand(CommandMessage* message,
-            store_address_t storeId);
-    static void setReportFileAttributesReply(CommandMessage* message,
-            store_address_t storeId);
-
-    static void setCreateDirectoryCommand(CommandMessage* message,
-            store_address_t storeId);
-    static void setDeleteDirectoryCommand(CommandMessage* message,
-            store_address_t storeId);
-
-    static void setSuccessReply(CommandMessage* message);
-    static void setFailureReply(CommandMessage* message,
-            ReturnValue_t errorCode, uint32_t errorParam = 0);
-    static void setCopyCommand(CommandMessage* message, store_address_t storeId);
-
-    static void setWriteCommand(CommandMessage* message,
-            store_address_t storeId);
-    static void setFinishStopWriteCommand(CommandMessage* message,
-            store_address_t storeId);
-    static void setFinishStopWriteReply(CommandMessage* message,
-            store_address_t storeId);
-    static void setFinishAppendReply(CommandMessage* message,
-            store_address_t storeId);
-
-    static void setReadCommand(CommandMessage* message,
-            store_address_t storeId);
-    static void setReadFinishedReply(CommandMessage* message,
-            store_address_t storeId);
-    static void setReadReply(CommandMessage* message, bool readFinished,
-            store_address_t storeId);
-    static bool getReadReply(const CommandMessage* message,
-            store_address_t* storeId);
-
-    static store_address_t getStoreId(const CommandMessage* message);
-    static ReturnValue_t getFailureReply(const CommandMessage* message,
-            uint32_t* errorParam = nullptr);
-
-};
-
-#endif /* MISSION_MEMORY_GENERICFILESYSTEMMESSAGE_H_ */

From 237ba8112b83944d65cde1c0680184c8ad502f24 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 13:43:48 +0200
Subject: [PATCH 106/125] more unrelated changes removed

---
 datalinklayer/DataLinkLayer.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/datalinklayer/DataLinkLayer.h b/datalinklayer/DataLinkLayer.h
index 27e69006..17a57d61 100644
--- a/datalinklayer/DataLinkLayer.h
+++ b/datalinklayer/DataLinkLayer.h
@@ -19,8 +19,7 @@ class VirtualChannelReception;
 class DataLinkLayer : public CCSDSReturnValuesIF {
 public:
 	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_1;
-	//! [EXPORT] : [COMMENT] A RF available signal was detected. P1: raw RFA state, P2: 0
-	static const Event RF_AVAILABLE = MAKE_EVENT(0, severity::INFO);
+	static const Event RF_AVAILABLE = MAKE_EVENT(0, severity::INFO); //!< A RF available signal was detected. P1: raw RFA state, P2: 0
 	static const Event RF_LOST = MAKE_EVENT(1, severity::INFO); //!< A previously found RF available signal was lost. P1: raw RFA state, P2: 0
 	static const Event BIT_LOCK = MAKE_EVENT(2, severity::INFO); //!< A Bit Lock signal. Was detected. P1: raw BLO state, P2: 0
 	static const Event BIT_LOCK_LOST = MAKE_EVENT(3, severity::INFO); //!< A previously found Bit Lock signal was lost. P1: raw BLO state, P2: 0

From e7ac2c7009102ff241e636544a55df4aae696772 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 13:46:04 +0200
Subject: [PATCH 107/125] maybe this works

---
 datalinklayer/DataLinkLayer.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/datalinklayer/DataLinkLayer.h b/datalinklayer/DataLinkLayer.h
index 17a57d61..27e69006 100644
--- a/datalinklayer/DataLinkLayer.h
+++ b/datalinklayer/DataLinkLayer.h
@@ -19,7 +19,8 @@ class VirtualChannelReception;
 class DataLinkLayer : public CCSDSReturnValuesIF {
 public:
 	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_1;
-	static const Event RF_AVAILABLE = MAKE_EVENT(0, severity::INFO); //!< A RF available signal was detected. P1: raw RFA state, P2: 0
+	//! [EXPORT] : [COMMENT] A RF available signal was detected. P1: raw RFA state, P2: 0
+	static const Event RF_AVAILABLE = MAKE_EVENT(0, severity::INFO);
 	static const Event RF_LOST = MAKE_EVENT(1, severity::INFO); //!< A previously found RF available signal was lost. P1: raw RFA state, P2: 0
 	static const Event BIT_LOCK = MAKE_EVENT(2, severity::INFO); //!< A Bit Lock signal. Was detected. P1: raw BLO state, P2: 0
 	static const Event BIT_LOCK_LOST = MAKE_EVENT(3, severity::INFO); //!< A previously found Bit Lock signal was lost. P1: raw BLO state, P2: 0

From f0a7b1cad2129392b6bc155bece6151cfab15e70 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 27 May 2021 13:47:22 +0200
Subject: [PATCH 108/125] indentation

---
 osal/FreeRTOS/MessageQueue.cpp | 138 +++++++++++++-------------
 osal/FreeRTOS/MessageQueue.h   | 174 ++++++++++++++++-----------------
 2 files changed, 156 insertions(+), 156 deletions(-)

diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index b5c9035d..43d4fa40 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -4,59 +4,59 @@
 #include "../../serviceinterface/ServiceInterfaceStream.h"
 
 MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
-		maxMessageSize(maxMessageSize) {
-	handle = xQueueCreate(messageDepth, maxMessageSize);
-	if (handle == nullptr) {
+        maxMessageSize(maxMessageSize) {
+    handle = xQueueCreate(messageDepth, maxMessageSize);
+    if (handle == nullptr) {
 #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;
+        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);
+        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() {
-	if (handle != nullptr) {
-		vQueueDelete(handle);
-	}
+    if (handle != nullptr) {
+        vQueueDelete(handle);
+    }
 }
 
 void MessageQueue::switchSystemContext(CallContext callContext) {
-	this->callContext = callContext;
+    this->callContext = callContext;
 }
 
 ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
-		MessageQueueMessageIF* message, bool ignoreFault) {
-	return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
+        MessageQueueMessageIF* message, bool ignoreFault) {
+    return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
 }
 
 ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
-	return sendToDefaultFrom(message, this->getId());
+    return sendToDefaultFrom(message, this->getId());
 }
 
 ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
-		MessageQueueId_t sentFrom, bool ignoreFault) {
-	return sendMessageFrom(defaultDestination,message,sentFrom,ignoreFault);
+        MessageQueueId_t sentFrom, bool ignoreFault) {
+    return sendMessageFrom(defaultDestination,message,sentFrom,ignoreFault);
 }
 
 ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
-	if (this->lastPartner != MessageQueueIF::NO_QUEUE) {
-		return sendMessageFrom(this->lastPartner, message, this->getId());
-	} else {
-		return NO_REPLY_PARTNER;
-	}
+    if (this->lastPartner != MessageQueueIF::NO_QUEUE) {
+        return sendMessageFrom(this->lastPartner, message, this->getId());
+    } else {
+        return NO_REPLY_PARTNER;
+    }
 }
 
 ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
-		MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
-		bool ignoreFault) {
-	return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault,
-	        callContext);
+        MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
+        bool ignoreFault) {
+    return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault,
+            callContext);
 }
 
 QueueHandle_t MessageQueue::getNativeQueueHandle() {
@@ -64,65 +64,65 @@ QueueHandle_t MessageQueue::getNativeQueueHandle() {
 }
 
 ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) {
-	if (result != pdPASS) {
-		if (not ignoreFault) {
-			InternalErrorReporterIF* internalErrorReporter = objectManager->
-			        get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
-			if (internalErrorReporter != nullptr) {
-				internalErrorReporter->queueMessageNotSent();
-			}
-		}
-		return MessageQueueIF::FULL;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+    if (result != pdPASS) {
+        if (not ignoreFault) {
+            InternalErrorReporterIF* internalErrorReporter = objectManager->
+                    get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
+            if (internalErrorReporter != nullptr) {
+                internalErrorReporter->queueMessageNotSent();
+            }
+        }
+        return MessageQueueIF::FULL;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
-		MessageQueueId_t* receivedFrom) {
-	ReturnValue_t status = this->receiveMessage(message);
-	if(status == HasReturnvaluesIF::RETURN_OK) {
-		*receivedFrom = this->lastPartner;
-	}
-	return status;
+        MessageQueueId_t* receivedFrom) {
+    ReturnValue_t status = this->receiveMessage(message);
+    if(status == HasReturnvaluesIF::RETURN_OK) {
+        *receivedFrom = this->lastPartner;
+    }
+    return status;
 }
 
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
-	BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(
-			message->getBuffer()), 0);
-	if (result == pdPASS){
-		this->lastPartner = message->getSender();
-		return HasReturnvaluesIF::RETURN_OK;
-	} else {
-		return MessageQueueIF::EMPTY;
-	}
+    BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(
+            message->getBuffer()), 0);
+    if (result == pdPASS){
+        this->lastPartner = message->getSender();
+        return HasReturnvaluesIF::RETURN_OK;
+    } else {
+        return MessageQueueIF::EMPTY;
+    }
 }
 
 MessageQueueId_t MessageQueue::getLastPartner() const {
-	return lastPartner;
+    return lastPartner;
 }
 
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
-	//TODO FreeRTOS does not support flushing partially
-	//Is always successful
-	xQueueReset(handle);
-	return HasReturnvaluesIF::RETURN_OK;
+    //TODO FreeRTOS does not support flushing partially
+    //Is always successful
+    xQueueReset(handle);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 MessageQueueId_t MessageQueue::getId() const {
-	return queueId;
+    return queueId;
 }
 
 void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
-	defaultDestinationSet = true;
-	this->defaultDestination = defaultDestination;
+    defaultDestinationSet = true;
+    this->defaultDestination = defaultDestination;
 }
 
 MessageQueueId_t MessageQueue::getDefaultDestination() const {
-	return defaultDestination;
+    return defaultDestination;
 }
 
 bool MessageQueue::isDefaultDestinationSet() const {
-	return defaultDestinationSet;
+    return defaultDestinationSet;
 }
 
 
@@ -130,15 +130,15 @@ bool MessageQueue::isDefaultDestinationSet() const {
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
         bool ignoreFault, CallContext callContext) {
-	BaseType_t result = pdFALSE;
-	if(sendTo == MessageQueueIF::NO_QUEUE) {
+    BaseType_t result = pdFALSE;
+    if(sendTo == MessageQueueIF::NO_QUEUE) {
         return MessageQueueIF::DESTINATION_INVALID;
-	}
+    }
 
-	QueueHandle_t destination = QueueMapManager::instance()->getMessageQueue(sendTo);
-	if(destination == nullptr) {
+    QueueHandle_t destination = QueueMapManager::instance()->getMessageQueue(sendTo);
+    if(destination == nullptr) {
         return MessageQueueIF::DESTINATION_INVALID;
-	}
+    }
 
     message->setSender(sentFrom);
     if(callContext == CallContext::TASK) {
diff --git a/osal/FreeRTOS/MessageQueue.h b/osal/FreeRTOS/MessageQueue.h
index 9cee5b22..be74d4fe 100644
--- a/osal/FreeRTOS/MessageQueue.h
+++ b/osal/FreeRTOS/MessageQueue.h
@@ -35,116 +35,116 @@
  * @ingroup message_queue
  */
 class MessageQueue : public MessageQueueIF {
-	friend class MessageQueueSenderIF;
+    friend class MessageQueueSenderIF;
 public:
-	/**
-	 * @brief	The constructor initializes and configures the message queue.
-	 * @details
-	 * By making use of the according operating system call, a message queue
-	 * is created and initialized. The message depth - the maximum number of
-	 * messages to be buffered - may be set with the help of a parameter,
-	 * whereas the message size is automatically set to the maximum message
-	 * queue message size. The operating system sets the message queue id, or
-	 * in case of failure, it is set to zero.
-	 * @param message_depth
-	 * The number of messages to be buffered before passing an error to the
-	 * sender. Default is three.
-	 * @param max_message_size
-	 * With this parameter, the maximum message size can be adjusted.
-	 * This should be left default.
-	 */
-	MessageQueue( size_t messageDepth = 3,
-			size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE );
+    /**
+     * @brief	The constructor initializes and configures the message queue.
+     * @details
+     * By making use of the according operating system call, a message queue
+     * is created and initialized. The message depth - the maximum number of
+     * messages to be buffered - may be set with the help of a parameter,
+     * whereas the message size is automatically set to the maximum message
+     * queue message size. The operating system sets the message queue id, or
+     * in case of failure, it is set to zero.
+     * @param message_depth
+     * The number of messages to be buffered before passing an error to the
+     * sender. Default is three.
+     * @param max_message_size
+     * With this parameter, the maximum message size can be adjusted.
+     * This should be left default.
+     */
+    MessageQueue( size_t messageDepth = 3,
+            size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE );
 
-	/** Copying message queues forbidden */
-	MessageQueue(const MessageQueue&) = delete;
-	MessageQueue& operator=(const MessageQueue&) = delete;
+    /** Copying message queues forbidden */
+    MessageQueue(const MessageQueue&) = delete;
+    MessageQueue& operator=(const MessageQueue&) = delete;
 
-	/**
-	 * @brief	The destructor deletes the formerly created message queue.
-	 * @details	This is accomplished by using the delete call provided
-	 * 			by the operating system.
-	 */
-	virtual ~MessageQueue();
+    /**
+     * @brief	The destructor deletes the formerly created message queue.
+     * @details	This is accomplished by using the delete call provided
+     * 			by the operating system.
+     */
+    virtual ~MessageQueue();
 
-	/**
-	 * This function is used to switch the call context. This has to be called
-	 * if a message is sent or received from an ISR!
-	 * @param callContext
-	 */
-	void switchSystemContext(CallContext callContext);
+    /**
+     * This function is used to switch the call context. This has to be called
+     * if a message is sent or received from an ISR!
+     * @param callContext
+     */
+    void switchSystemContext(CallContext callContext);
 
-	/** MessageQueueIF implementation */
-	ReturnValue_t sendMessage(MessageQueueId_t sendTo,
-			MessageQueueMessageIF* message, bool ignoreFault = false) override;
+    /** MessageQueueIF implementation */
+    ReturnValue_t sendMessage(MessageQueueId_t sendTo,
+            MessageQueueMessageIF* message, bool ignoreFault = false) override;
 
-	ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override;
+    ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override;
 
-	ReturnValue_t reply(MessageQueueMessageIF* message) override;
-	virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo,
-			MessageQueueMessageIF* message,
-			MessageQueueId_t sentFrom = NO_QUEUE,
-			bool ignoreFault = false) override;
+    ReturnValue_t reply(MessageQueueMessageIF* message) override;
+    virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo,
+            MessageQueueMessageIF* message,
+            MessageQueueId_t sentFrom = NO_QUEUE,
+            bool ignoreFault = false) override;
 
-	virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessageIF* message,
-			MessageQueueId_t sentFrom = NO_QUEUE,
-			bool ignoreFault = false) override;
+    virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessageIF* message,
+            MessageQueueId_t sentFrom = NO_QUEUE,
+            bool ignoreFault = false) override;
 
-	ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
-			MessageQueueId_t *receivedFrom) override;
+    ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
+            MessageQueueId_t *receivedFrom) override;
 
-	ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override;
+    ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override;
 
-	ReturnValue_t flush(uint32_t* count) override;
+    ReturnValue_t flush(uint32_t* count) override;
 
-	MessageQueueId_t getLastPartner() const override;
+    MessageQueueId_t getLastPartner() const override;
 
-	MessageQueueId_t getId() const override;
+    MessageQueueId_t getId() const override;
 
-	void setDefaultDestination(MessageQueueId_t defaultDestination) override;
+    void setDefaultDestination(MessageQueueId_t defaultDestination) override;
 
-	MessageQueueId_t getDefaultDestination() const override;
+    MessageQueueId_t getDefaultDestination() const override;
 
-	bool isDefaultDestinationSet() const override;
+    bool isDefaultDestinationSet() const override;
 
-	QueueHandle_t getNativeQueueHandle();
+    QueueHandle_t getNativeQueueHandle();
 
 protected:
-	/**
-	 * @brief 	Implementation to be called from any send Call within
-	 * 			MessageQueue and MessageQueueSenderIF.
-	 * @details
-	 * This method takes the message provided, adds the sentFrom information and
-	 * passes it on to the destination provided with an operating system call.
-	 * The OS's return value is returned.
-	 * @param sendTo
-	 * This parameter specifies the message queue id to send the message to.
-	 * @param message
-	 * This is a pointer to a previously created message, which is sent.
-	 * @param sentFrom
-	 * The sentFrom information can be set to inject the sender's queue id into
-	 * the message. This variable is set to zero by default.
-	 * @param ignoreFault
-	 * If set to true, the internal software fault counter  is not incremented
-	 * if queue is full.
-	 * @param context Specify whether call is made from task or from an ISR.
-	 */
-	static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,
-			MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE,
-			bool ignoreFault=false, CallContext callContext = CallContext::TASK);
+    /**
+     * @brief 	Implementation to be called from any send Call within
+     * 			MessageQueue and MessageQueueSenderIF.
+     * @details
+     * This method takes the message provided, adds the sentFrom information and
+     * passes it on to the destination provided with an operating system call.
+     * The OS's return value is returned.
+     * @param sendTo
+     * This parameter specifies the message queue id to send the message to.
+     * @param message
+     * This is a pointer to a previously created message, which is sent.
+     * @param sentFrom
+     * The sentFrom information can be set to inject the sender's queue id into
+     * the message. This variable is set to zero by default.
+     * @param ignoreFault
+     * If set to true, the internal software fault counter  is not incremented
+     * if queue is full.
+     * @param context Specify whether call is made from task or from an ISR.
+     */
+    static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,
+            MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE,
+            bool ignoreFault=false, CallContext callContext = CallContext::TASK);
 
-	static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
+    static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
 
 private:
-	bool defaultDestinationSet = false;
-	QueueHandle_t handle;
-	MessageQueueId_t queueId = MessageQueueIF::NO_QUEUE;
+    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;
-	//! Stores the current system context
-	CallContext callContext =  CallContext::TASK;
+    MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE;
+    MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE;
+    const size_t maxMessageSize;
+    //! Stores the current system context
+    CallContext callContext =  CallContext::TASK;
 };
 
 #endif /* FSFW_OSAL_FREERTOS_MESSAGEQUEUE_H_ */

From 567699954c0e91f0a5ebd692f3e27ab51d94d1f3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 28 May 2021 18:30:43 +0200
Subject: [PATCH 109/125] added missing event translation

---
 events/EventManager.cpp | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/events/EventManager.cpp b/events/EventManager.cpp
index 2337ba83..8e2a2a82 100644
--- a/events/EventManager.cpp
+++ b/events/EventManager.cpp
@@ -156,11 +156,11 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage *messag
             sif::info << "0x" << std::hex << std::setw(8) << std::setfill('0') <<
                     message->getReporter() << std::setfill(' ') << std::dec;
         }
-        sif::info << " report event with ID " << message->getEventId() << std::endl;
-        sif::info << std::hex << "P1 Hex: 0x" << message->getParameter1() <<
-                " | P1 Dec: " << std::dec << message->getParameter1() <<  std::hex <<
-                " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<  std::dec <<
-                message->getParameter2() << std::endl;
+        sif::info << " reported event with ID " << message->getEventId() << std::endl;
+        sif::debug << translateEvents(message->getEvent()) << " | " <<std::hex << "P1 Hex: 0x" <<
+                message->getParameter1() << " | P1 Dec: " << std::dec << message->getParameter1() <<
+                std::hex << " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<
+                std::dec << message->getParameter2() << std::endl;
 #else
         if (string != 0) {
             sif::printInfo("Event Manager: %s reported event with ID %d\n", string,
@@ -186,11 +186,11 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage *messag
             sif::debug << "0x" << std::hex << std::setw(8) << std::setfill('0') <<
                     message->getReporter() << std::setfill(' ') << std::dec;
         }
-        sif::debug << " report event with ID " << message->getEventId() << std::endl;
-        sif::debug << std::hex << "P1 Hex: 0x" << message->getParameter1() <<
-                " | P1 Dec: " << std::dec << message->getParameter1() <<  std::hex <<
-                " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<  std::dec <<
-                message->getParameter2() << std::endl;
+        sif::debug << " reported event with ID " << message->getEventId() << std::endl;
+        sif::debug << translateEvents(message->getEvent()) << " | " <<std::hex << "P1 Hex: 0x" <<
+                message->getParameter1() << " | P1 Dec: " << std::dec << message->getParameter1() <<
+                std::hex << " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<
+                std::dec << message->getParameter2() << std::endl;
 #else
         if (string != 0) {
             sif::printDebug("Event Manager: %s reported event with ID %d\n", string,

From 404c3821e60f9d690fee6167739df237ff907ced Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 31 May 2021 11:12:52 +0200
Subject: [PATCH 110/125] corrected include

---
 unittest/tests/datapoollocal/LocalPoolOwnerBase.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/unittest/tests/datapoollocal/LocalPoolOwnerBase.h b/unittest/tests/datapoollocal/LocalPoolOwnerBase.h
index 8d2073b0..4c244b16 100644
--- a/unittest/tests/datapoollocal/LocalPoolOwnerBase.h
+++ b/unittest/tests/datapoollocal/LocalPoolOwnerBase.h
@@ -1,7 +1,7 @@
 #ifndef FSFW_UNITTEST_TESTS_DATAPOOLLOCAL_LOCALPOOLOWNERBASE_H_
 #define FSFW_UNITTEST_TESTS_DATAPOOLLOCAL_LOCALPOOLOWNERBASE_H_
 
-#include <testcfg/objects/systemObjectList.h>
+#include "objects/systemObjectList.h"
 
 #include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
 #include <fsfw/datapoollocal/LocalDataSet.h>

From c9836abf03388b6fe17a8901d8d8ef2cccd5ca59 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 31 May 2021 11:36:32 +0200
Subject: [PATCH 111/125] minor tweak

---
 events/EventManager.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/events/EventManager.h b/events/EventManager.h
index 9189d9e7..012247db 100644
--- a/events/EventManager.h
+++ b/events/EventManager.h
@@ -3,8 +3,7 @@
 
 #include "EventManagerIF.h"
 #include "eventmatching/EventMatchTree.h"
-
-#include <FSFWConfig.h>
+#include "FSFWConfig.h"
 
 #include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/SystemObject.h"

From e961f3f038eaac96784846a8623cbb0c55cece1d Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 31 May 2021 12:22:33 +0200
Subject: [PATCH 112/125] better error handling for mq_receive()

---
 osal/linux/MessageQueue.cpp | 583 +++++++++++++++++++-----------------
 1 file changed, 300 insertions(+), 283 deletions(-)

diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index 12774a58..9399183d 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -11,65 +11,64 @@
 
 
 MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize):
-		id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE),
-		defaultDestination(MessageQueueIF::NO_QUEUE),
-		maxMessageSize(maxMessageSize) {
-	//debug << "MessageQueue::MessageQueue: Creating a queue" << std::endl;
-	mq_attr attributes;
-	this->id = 0;
-	//Set attributes
-	attributes.mq_curmsgs = 0;
-	attributes.mq_maxmsg = messageDepth;
-	attributes.mq_msgsize = maxMessageSize;
-	attributes.mq_flags = 0; //Flags are ignored on Linux during mq_open
-	//Set the name of the queue. The slash is mandatory!
-	sprintf(name, "/FSFW_MQ%u\n", queueCounter++);
+        id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE),
+        defaultDestination(MessageQueueIF::NO_QUEUE), maxMessageSize(maxMessageSize) {
+    //debug << "MessageQueue::MessageQueue: Creating a queue" << std::endl;
+    mq_attr attributes;
+    this->id = 0;
+    //Set attributes
+    attributes.mq_curmsgs = 0;
+    attributes.mq_maxmsg = messageDepth;
+    attributes.mq_msgsize = maxMessageSize;
+    attributes.mq_flags = 0; //Flags are ignored on Linux during mq_open
+    //Set the name of the queue. The slash is mandatory!
+    sprintf(name, "/FSFW_MQ%u\n", queueCounter++);
 
-	// Create a nonblocking queue if the name is available (the queue is read
-	// and writable for the owner as well as the group)
-	int oflag = O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL;
-	mode_t mode = S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP | S_IROTH | S_IWOTH;
-	mqd_t tempId = mq_open(name, oflag, mode, &attributes);
-	if (tempId == -1) {
-		handleError(&attributes, messageDepth);
-	}
-	else {
-		//Successful mq_open call
-		this->id = tempId;
-	}
+    // Create a nonblocking queue if the name is available (the queue is read
+    // and writable for the owner as well as the group)
+    int oflag = O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL;
+    mode_t mode = S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP | S_IROTH | S_IWOTH;
+    mqd_t tempId = mq_open(name, oflag, mode, &attributes);
+    if (tempId == -1) {
+        handleError(&attributes, messageDepth);
+    }
+    else {
+        //Successful mq_open call
+        this->id = tempId;
+    }
 }
 
 MessageQueue::~MessageQueue() {
-	int status = mq_close(this->id);
-	if(status != 0){
+    int status = mq_close(this->id);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "MessageQueue::Destructor: mq_close Failed with status: "
-				   << strerror(errno) <<std::endl;
+        sif::error << "MessageQueue::Destructor: mq_close Failed with status: "
+                << strerror(errno) <<std::endl;
 #endif
-	}
-	status = mq_unlink(name);
-	if(status != 0){
+    }
+    status = mq_unlink(name);
+    if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "MessageQueue::Destructor: mq_unlink Failed with status: "
-				   << strerror(errno) << std::endl;
+        sif::error << "MessageQueue::Destructor: mq_unlink Failed with status: "
+                << strerror(errno) << std::endl;
 #endif
-	}
+    }
 }
 
 ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
-		uint32_t messageDepth) {
-	switch(errno) {
-	case(EINVAL): {
+        uint32_t messageDepth) {
+    switch(errno) {
+    case(EINVAL): {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "MessageQueue::MessageQueue: Invalid name or attributes"
-				" for message size" << std::endl;
+        sif::error << "MessageQueue::MessageQueue: Invalid name or attributes"
+                " for message size" << std::endl;
 #endif
-		size_t defaultMqMaxMsg = 0;
-		// Not POSIX conformant, but should work for all UNIX systems.
-		// Just an additional helpful printout :-)
-		if(std::ifstream("/proc/sys/fs/mqueue/msg_max",std::ios::in) >>
-				defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) {
-			/*
+        size_t defaultMqMaxMsg = 0;
+        // Not POSIX conformant, but should work for all UNIX systems.
+        // Just an additional helpful printout :-)
+        if(std::ifstream("/proc/sys/fs/mqueue/msg_max",std::ios::in) >>
+                defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) {
+            /*
 			See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html
 			This happens if the msg_max value is not large enough
 			It is ignored if the executable is run in privileged mode.
@@ -83,326 +82,344 @@ ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
 			sudo nano /etc/sysctl.conf
 			Append at end: fs/mqueue/msg_max = <newMsgMaxLen>
 			Apply changes with: sudo sysctl -p
-			*/
+             */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::MessageQueue: Default MQ size "
-					<< defaultMqMaxMsg << " is too small for requested size "
-					<< messageDepth << std::endl;
-			sif::error << "This error can be fixed by setting the maximum "
-					"allowed message size higher!" << std::endl;
+            sif::error << "MessageQueue::MessageQueue: Default MQ size "
+                    << defaultMqMaxMsg << " is too small for requested size "
+                    << messageDepth << std::endl;
+            sif::error << "This error can be fixed by setting the maximum "
+                    "allowed message size higher!" << std::endl;
 #endif
-
-		}
-		break;
-	}
-	case(EEXIST): {
-		// An error occured during open
-		// We need to distinguish if it is caused by an already created queue
-		//There's another queue with the same name
-		//We unlink the other queue
-		int status = mq_unlink(name);
-		if (status != 0) {
+        }
+        break;
+    }
+    case(EEXIST): {
+        // An error occured during open
+        // We need to distinguish if it is caused by an already created queue
+        // There's another queue with the same name
+        // We unlink the other queue
+        int status = mq_unlink(name);
+        if (status != 0) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "mq_unlink Failed with status: " << strerror(errno)
-													<< std::endl;
+            sif::error << "mq_unlink Failed with status: " << strerror(errno)
+													        << std::endl;
 #endif
-		}
-		else {
-			// Successful unlinking, try to open again
-			mqd_t tempId = mq_open(name,
-					O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL,
-					S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP, attributes);
-			if (tempId != -1) {
-				//Successful mq_open
-				this->id = tempId;
-				return HasReturnvaluesIF::RETURN_OK;
-			}
-		}
-		break;
-	}
+        }
+        else {
+            // Successful unlinking, try to open again
+            mqd_t tempId = mq_open(name,
+                    O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL,
+                    S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP, attributes);
+            if (tempId != -1) {
+                //Successful mq_open
+                this->id = tempId;
+                return HasReturnvaluesIF::RETURN_OK;
+            }
+        }
+        break;
+    }
 
-	default: {
-		// Failed either the first time or the second time
+    default: {
+        // Failed either the first time or the second time
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "MessageQueue::MessageQueue: Creating Queue " << name
-				<< " failed with status: " << strerror(errno) << std::endl;
+        sif::error << "MessageQueue::MessageQueue: Creating Queue " << name
+                << " failed with status: " << strerror(errno) << std::endl;
 #else
-		sif::printError("MessageQueue::MessageQueue: Creating Queue %s"
-				" failed with status: %s\n", name, strerror(errno));
+        sif::printError("MessageQueue::MessageQueue: Creating Queue %s"
+                " failed with status: %s\n", name, strerror(errno));
 #endif
-	}
-	}
-	return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    }
+    return HasReturnvaluesIF::RETURN_FAILED;
 
 
 
 }
 
 ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
-		MessageQueueMessageIF* message, bool ignoreFault) {
-	return sendMessageFrom(sendTo, message, this->getId(), false);
+        MessageQueueMessageIF* message, bool ignoreFault) {
+    return sendMessageFrom(sendTo, message, this->getId(), false);
 }
 
 ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
-	return sendToDefaultFrom(message, this->getId());
+    return sendToDefaultFrom(message, this->getId());
 }
 
 ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
-	if (this->lastPartner != 0) {
-		return sendMessageFrom(this->lastPartner, message, this->getId());
-	} else {
-		return NO_REPLY_PARTNER;
-	}
+    if (this->lastPartner != 0) {
+        return sendMessageFrom(this->lastPartner, message, this->getId());
+    } else {
+        return NO_REPLY_PARTNER;
+    }
 }
 
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
-		MessageQueueId_t* receivedFrom) {
-	ReturnValue_t status = this->receiveMessage(message);
-	*receivedFrom = this->lastPartner;
-	return status;
+        MessageQueueId_t* receivedFrom) {
+    ReturnValue_t status = this->receiveMessage(message);
+    *receivedFrom = this->lastPartner;
+    return status;
 }
 
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
-	if(message == nullptr) {
+    if(message == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "MessageQueue::receiveMessage: Message is "
-				"nullptr!" << std::endl;
+        sif::error << "MessageQueue::receiveMessage: Message is "
+                "nullptr!" << std::endl;
 #endif
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 
-	if(message->getMaximumMessageSize() < maxMessageSize) {
+    if(message->getMaximumMessageSize() < maxMessageSize) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "MessageQueue::receiveMessage: Message size "
-				<< message->getMaximumMessageSize()
-				<< " too small to receive data!" << std::endl;
+        sif::error << "MessageQueue::receiveMessage: Message size "
+                << message->getMaximumMessageSize()
+                << " too small to receive data!" << std::endl;
 #endif
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 
-	unsigned int messagePriority = 0;
-	int status = mq_receive(id,reinterpret_cast<char*>(message->getBuffer()),
-			message->getMaximumMessageSize(),&messagePriority);
-	if (status > 0) {
-		this->lastPartner = message->getSender();
-		//Check size of incoming message.
-		if (message->getMessageSize() < message->getMinimumMessageSize()) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		return HasReturnvaluesIF::RETURN_OK;
-	}
-	else if (status==0) {
-		//Success but no message received
-		return MessageQueueIF::EMPTY;
-	}
-	else {
-		//No message was received. Keep lastPartner anyway, I might send
-		//something later. But still, delete packet content.
-		memset(message->getData(), 0, message->getMaximumDataSize());
-		switch(errno){
-		case EAGAIN:
-			//O_NONBLOCK or MQ_NONBLOCK was set and there are no messages
-			//currently on the specified queue.
-			return MessageQueueIF::EMPTY;
-		case EBADF:
-			//mqdes doesn't represent a valid queue open for reading.
+    unsigned int messagePriority = 0;
+    int status = mq_receive(id,reinterpret_cast<char*>(message->getBuffer()),
+            message->getMaximumMessageSize(),&messagePriority);
+    if (status > 0) {
+        this->lastPartner = message->getSender();
+        //Check size of incoming message.
+        if (message->getMessageSize() < message->getMinimumMessageSize()) {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    else if (status==0) {
+        //Success but no message received
+        return MessageQueueIF::EMPTY;
+    }
+    else {
+        //No message was received. Keep lastPartner anyway, I might send
+        //something later. But still, delete packet content.
+        memset(message->getData(), 0, message->getMaximumDataSize());
+        switch(errno){
+        case EAGAIN:
+            //O_NONBLOCK or MQ_NONBLOCK was set and there are no messages
+            //currently on the specified queue.
+            return MessageQueueIF::EMPTY;
+        case EBADF: {
+            //mqdes doesn't represent a valid queue open for reading.
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::receive: configuration error "
-			           << strerror(errno)  << std::endl;
+            sif::error << "MessageQueue::receive: configuration error "
+                    << strerror(errno)  << std::endl;
 #endif
-			/*NO BREAK*/
-		case EINVAL:
-			/*
-			 * This value indicates one of the following:
-			 * - The pointer to the buffer for storing the received message,
-			 *   msg_ptr, is NULL.
-			 * - The number of bytes requested, msg_len is less than zero.
-			 * - msg_len is anything other than the mq_msgsize of the specified
-			 *   queue, and the QNX extended option MQ_READBUF_DYNAMIC hasn't
-			 *   been set in the queue's mq_flags.
-			 */
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        case EINVAL: {
+            /*
+             * This value indicates one of the following:
+             * - The pointer to the buffer for storing the received message,
+             *   msg_ptr, is NULL.
+             * - The number of bytes requested, msg_len is less than zero.
+             * - msg_len is anything other than the mq_msgsize of the specified
+             *   queue, and the QNX extended option MQ_READBUF_DYNAMIC hasn't
+             *   been set in the queue's mq_flags.
+             */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::receive: configuration error "
-					   << strerror(errno)  << std::endl;
+            sif::error << "MessageQueue::receive: EINVAL error "
+                    << strerror(errno)  << std::endl;
 #endif
-			/*NO BREAK*/
-		case EMSGSIZE:
-			/*
-			 * This value indicates one of the following:
-			 * - the QNX extended option MQ_READBUF_DYNAMIC hasn't been set,
-			 *   and the given msg_len is shorter than the mq_msgsize for
-			 *   the given queue.
-			 * - the extended option MQ_READBUF_DYNAMIC has been set, but the
-			 *   given msg_len is too short for the message that would have
-			 *   been received.
-			 */
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        case EMSGSIZE: {
+            /*
+             * This value indicates one of the following:
+             * - the QNX extended option MQ_READBUF_DYNAMIC hasn't been set,
+             *   and the given msg_len is shorter than the mq_msgsize for
+             *   the given queue.
+             * - the extended option MQ_READBUF_DYNAMIC has been set, but the
+             *   given msg_len is too short for the message that would have
+             *   been received.
+             */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::receive: configuration error "
-			           << strerror(errno)  << std::endl;
+            sif::error << "MessageQueue::receive: EMSGSIZE error "
+                    << strerror(errno)  << std::endl;
 #endif
-			/*NO BREAK*/
-		case EINTR:
-			//The operation was interrupted by a signal.
-		default:
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
 
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
+        case EINTR: {
+            //The operation was interrupted by a signal.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "MessageQueue::receiveMessage: EINTR error " << strerror(errno)  <<
+                    std::endl;
+#endif
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        case ETIMEDOUT: {
+            //The operation was interrupted by a signal.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "MessageQueue::receiveMessage: ETIMEDOUT error " << strerror(errno)  <<
+                    std::endl;
+#endif
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
 
-	}
+        default:
+
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+
+    }
 }
 
 MessageQueueId_t MessageQueue::getLastPartner() const {
-	return this->lastPartner;
+    return this->lastPartner;
 }
 
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
-	mq_attr attrib;
-	int status = mq_getattr(id,&attrib);
-	if(status != 0){
-		switch(errno){
-		case EBADF:
-			//mqdes doesn't represent a valid message queue.
+    mq_attr attrib;
+    int status = mq_getattr(id,&attrib);
+    if(status != 0){
+        switch(errno){
+        case EBADF:
+            //mqdes doesn't represent a valid message queue.
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::flush configuration error, "
-					"called flush with an invalid queue ID" << std::endl;
+            sif::error << "MessageQueue::flush configuration error, "
+            "called flush with an invalid queue ID" << std::endl;
 #endif
-			/*NO BREAK*/
-		case EINVAL:
-			//mq_attr is NULL
-		default:
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-	}
-	*count = attrib.mq_curmsgs;
-	attrib.mq_curmsgs = 0;
-	status = mq_setattr(id,&attrib,NULL);
-	if(status != 0){
-		switch(errno){
-		case EBADF:
-			//mqdes doesn't represent a valid message queue.
+            /*NO BREAK*/
+        case EINVAL:
+            //mq_attr is NULL
+        default:
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+    }
+    *count = attrib.mq_curmsgs;
+    attrib.mq_curmsgs = 0;
+    status = mq_setattr(id,&attrib,NULL);
+    if(status != 0){
+        switch(errno){
+        case EBADF:
+            //mqdes doesn't represent a valid message queue.
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::flush configuration error, "
-					"called flush with an invalid queue ID" << std::endl;
+            sif::error << "MessageQueue::flush configuration error, "
+            "called flush with an invalid queue ID" << std::endl;
 #endif
-			/*NO BREAK*/
-		case EINVAL:
-			/*
-			 * This value indicates one of the following:
-			 *  - mq_attr is NULL.
-			 *  - MQ_MULT_NOTIFY had been set for this queue, and the given
-			 *    mq_flags includes a 0 in the MQ_MULT_NOTIFY bit. Once
-			 *    MQ_MULT_NOTIFY has been turned on, it may never be turned off.
-			 */
-		default:
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+            /*NO BREAK*/
+        case EINVAL:
+            /*
+             * This value indicates one of the following:
+             *  - mq_attr is NULL.
+             *  - MQ_MULT_NOTIFY had been set for this queue, and the given
+             *    mq_flags includes a 0 in the MQ_MULT_NOTIFY bit. Once
+             *    MQ_MULT_NOTIFY has been turned on, it may never be turned off.
+             */
+        default:
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 MessageQueueId_t MessageQueue::getId() const {
-	return this->id;
+    return this->id;
 }
 
 void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
-	this->defaultDestination = defaultDestination;
+    this->defaultDestination = defaultDestination;
 }
 
 ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
-		MessageQueueId_t sentFrom, bool ignoreFault) {
-	return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
+        MessageQueueId_t sentFrom, bool ignoreFault) {
+    return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
 }
 
 
 ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
-		MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
-		bool ignoreFault) {
-	return sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault);
+        MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
+        bool ignoreFault) {
+    return sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault);
 
 }
 
 MessageQueueId_t MessageQueue::getDefaultDestination() const {
-	return this->defaultDestination;
+    return this->defaultDestination;
 }
 
 bool MessageQueue::isDefaultDestinationSet() const {
-	return (defaultDestination != NO_QUEUE);
+    return (defaultDestination != NO_QUEUE);
 }
 
 uint16_t MessageQueue::queueCounter = 0;
 
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
-		MessageQueueMessageIF *message, MessageQueueId_t sentFrom,
-		bool ignoreFault) {
-	if(message == nullptr) {
+        MessageQueueMessageIF *message, MessageQueueId_t sentFrom,
+        bool ignoreFault) {
+    if(message == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is "
-				"nullptr!" << std::endl;
+        sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is "
+                "nullptr!" << std::endl;
 #endif
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 
-	message->setSender(sentFrom);
-	int result = mq_send(sendTo,
-			reinterpret_cast<const char*>(message->getBuffer()),
-			message->getMessageSize(),0);
+    message->setSender(sentFrom);
+    int result = mq_send(sendTo,
+            reinterpret_cast<const char*>(message->getBuffer()),
+            message->getMessageSize(),0);
 
-	//TODO: Check if we're in ISR.
-	if (result != 0) {
-		if(!ignoreFault){
-			InternalErrorReporterIF* internalErrorReporter =
-					objectManager->get<InternalErrorReporterIF>(
-						objects::INTERNAL_ERROR_REPORTER);
-			if (internalErrorReporter != NULL) {
-				internalErrorReporter->queueMessageNotSent();
-			}
-		}
-		switch(errno){
-		case EAGAIN:
-			//The O_NONBLOCK flag was set when opening the queue, or the
-			//MQ_NONBLOCK flag was set in its attributes, and the
-			//specified queue is full.
-			return MessageQueueIF::FULL;
-		case EBADF: {
-			//mq_des doesn't represent a valid message queue descriptor,
-			//or mq_des wasn't opened for writing.
+    //TODO: Check if we're in ISR.
+    if (result != 0) {
+        if(!ignoreFault){
+            InternalErrorReporterIF* internalErrorReporter =
+                    objectManager->get<InternalErrorReporterIF>(
+                            objects::INTERNAL_ERROR_REPORTER);
+            if (internalErrorReporter != NULL) {
+                internalErrorReporter->queueMessageNotSent();
+            }
+        }
+        switch(errno){
+        case EAGAIN:
+            //The O_NONBLOCK flag was set when opening the queue, or the
+            //MQ_NONBLOCK flag was set in its attributes, and the
+            //specified queue is full.
+            return MessageQueueIF::FULL;
+        case EBADF: {
+            //mq_des doesn't represent a valid message queue descriptor,
+            //or mq_des wasn't opened for writing.
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::sendMessage: Configuration error, MQ"
-					<< " destination invalid."  << std::endl;
-			sif::error << strerror(errno) << " in "
-					<<"mq_send to: " << sendTo << " sent from "
-					<< sentFrom << std::endl;
+            sif::error << "MessageQueue::sendMessage: Configuration error, MQ"
+                    << " destination invalid."  << std::endl;
+            sif::error << strerror(errno) << " in "
+                    <<"mq_send to: " << sendTo << " sent from "
+                    << sentFrom << std::endl;
 #endif
-			return DESTINATION_INVALID;
-		}
-		case EINTR:
-			//The call was interrupted by a signal.
-		case EINVAL:
-			/*
-			 * This value indicates one of the following:
-			 * - msg_ptr is NULL.
-			 * - msg_len is negative.
-			 * - msg_prio is greater than MQ_PRIO_MAX.
-			 * - msg_prio is less than 0.
-			 * - MQ_PRIO_RESTRICT is set in the mq_attr of mq_des, and
-			 *   msg_prio is greater than the priority of the calling process.
-			 */
+            return DESTINATION_INVALID;
+        }
+        case EINTR:
+            //The call was interrupted by a signal.
+        case EINVAL:
+            /*
+             * This value indicates one of the following:
+             * - msg_ptr is NULL.
+             * - msg_len is negative.
+             * - msg_prio is greater than MQ_PRIO_MAX.
+             * - msg_prio is less than 0.
+             * - MQ_PRIO_RESTRICT is set in the mq_attr of mq_des, and
+             *   msg_prio is greater than the priority of the calling process.
+             */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::sendMessage: Configuration error "
-			           << strerror(errno) << " in mq_send" << std::endl;
+            sif::error << "MessageQueue::sendMessage: Configuration error "
+            << strerror(errno) << " in mq_send" << std::endl;
 #endif
-			/*NO BREAK*/
-		case EMSGSIZE:
-			// The msg_len is greater than the msgsize associated with
-			//the specified queue.
+            /*NO BREAK*/
+        case EMSGSIZE:
+            // The msg_len is greater than the msgsize associated with
+            //the specified queue.
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::sendMessage: Size error [" <<
-					strerror(errno) << "] in mq_send" << std::endl;
+            sif::error << "MessageQueue::sendMessage: Size error [" <<
+            strerror(errno) << "] in mq_send" << std::endl;
 #endif
-			/*NO BREAK*/
-		default:
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+            /*NO BREAK*/
+        default:
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }

From 54e60f4ddc88a8e108588c5a8b69ae744686a295 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 31 May 2021 12:30:54 +0200
Subject: [PATCH 113/125] cleaned up a bit

---
 osal/linux/MessageQueue.cpp | 44 ++++++++++++++++---------------------
 osal/linux/MessageQueue.h   |  1 +
 2 files changed, 20 insertions(+), 25 deletions(-)

diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index 9399183d..bfd97e56 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -204,11 +204,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
             return MessageQueueIF::EMPTY;
         case EBADF: {
             //mqdes doesn't represent a valid queue open for reading.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::receive: configuration error "
-                    << strerror(errno)  << std::endl;
-#endif
-            return HasReturnvaluesIF::RETURN_FAILED;
+            return handleRecvError("EBADF");
         }
         case EINVAL: {
             /*
@@ -220,11 +216,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
              *   queue, and the QNX extended option MQ_READBUF_DYNAMIC hasn't
              *   been set in the queue's mq_flags.
              */
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::receive: EINVAL error "
-                    << strerror(errno)  << std::endl;
-#endif
-            return HasReturnvaluesIF::RETURN_FAILED;
+            return handleRecvError("EINVAL");
         }
         case EMSGSIZE: {
             /*
@@ -236,28 +228,16 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
              *   given msg_len is too short for the message that would have
              *   been received.
              */
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::receive: EMSGSIZE error "
-                    << strerror(errno)  << std::endl;
-#endif
-            return HasReturnvaluesIF::RETURN_FAILED;
+            return handleRecvError("EMSGSIZE");
         }
 
         case EINTR: {
             //The operation was interrupted by a signal.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::receiveMessage: EINTR error " << strerror(errno)  <<
-                    std::endl;
-#endif
-            return HasReturnvaluesIF::RETURN_FAILED;
+            return handleRecvError("EINTR");
         }
         case ETIMEDOUT: {
             //The operation was interrupted by a signal.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::receiveMessage: ETIMEDOUT error " << strerror(errno)  <<
-                    std::endl;
-#endif
-            return HasReturnvaluesIF::RETURN_FAILED;
+            return handleRecvError("ETIMEDOUT");
         }
 
         default:
@@ -423,3 +403,17 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
+
+ReturnValue_t MessageQueue::handleRecvError(const char * const failString) {
+    if(failString == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "MessageQueue::receiveMessage: " << failString << " error "
+            << strerror(errno)  << std::endl;
+#else
+    sif::printError("MessageQueue::receiveMessage: %s error %s\n", failString,
+            strerror(errno));
+#endif
+    return HasReturnvaluesIF::RETURN_FAILED;
+}
diff --git a/osal/linux/MessageQueue.h b/osal/linux/MessageQueue.h
index 239bbbdb..0bef0a73 100644
--- a/osal/linux/MessageQueue.h
+++ b/osal/linux/MessageQueue.h
@@ -182,6 +182,7 @@ private:
 	const size_t maxMessageSize;
 
 	ReturnValue_t handleError(mq_attr* attributes, uint32_t messageDepth);
+	ReturnValue_t handleRecvError(const char* const failString);
 };
 
 #endif /* FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ */

From 722a7b3240248cac2a93e3d4e3a09e3ec5966e9a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 31 May 2021 16:46:32 +0200
Subject: [PATCH 114/125] renamed variable

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8ba6a187..43ff71b3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -131,7 +131,7 @@ else()
 	)
 endif()
 
-foreach(INCLUDE_PATH ${FSFW_ADDITIONAL_INC_PATH})
+foreach(INCLUDE_PATH ${FSFW_ADDITIONAL_INC_PATHS})
     if(IS_ABSOLUTE ${INCLUDE_PATH})
         set(CURR_ABS_INC_PATH "${FREERTOS_PATH}")
     else()

From ad820fbe99d2fa0fb49eb71ed70611bfea7efa99 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 31 May 2021 17:00:51 +0200
Subject: [PATCH 115/125] important bugfix

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 43ff71b3..3d4053e2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -133,7 +133,7 @@ endif()
 
 foreach(INCLUDE_PATH ${FSFW_ADDITIONAL_INC_PATHS})
     if(IS_ABSOLUTE ${INCLUDE_PATH})
-        set(CURR_ABS_INC_PATH "${FREERTOS_PATH}")
+        set(CURR_ABS_INC_PATH "${INCLUDE_PATH}")
     else()
         get_filename_component(CURR_ABS_INC_PATH
             ${INCLUDE_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR})

From 38910143400e455f5184ad85be98e05638c2eea6 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Thu, 3 Jun 2021 12:29:06 +0200
Subject: [PATCH 116/125] linux error print handling improved

---
 osal/host/MessageQueue.cpp       |   7 +-
 osal/linux/BinarySemaphore.cpp   | 233 ++++++++++----------
 osal/linux/BinarySemaphore.h     |   1 +
 osal/linux/CMakeLists.txt        |   1 +
 osal/linux/CountingSemaphore.cpp |  76 ++++---
 osal/linux/MessageQueue.cpp      | 242 ++++++++++-----------
 osal/linux/MessageQueue.h        |   4 +-
 osal/linux/Mutex.cpp             |  27 +--
 osal/linux/PeriodicPosixTask.cpp |  13 +-
 osal/linux/PosixThread.cpp       | 352 +++++++++++++++----------------
 osal/linux/PosixThread.h         | 110 +++++-----
 osal/linux/unixUtility.cpp       |  32 +++
 osal/linux/unixUtility.h         |  13 ++
 13 files changed, 558 insertions(+), 553 deletions(-)
 create mode 100644 osal/linux/unixUtility.cpp
 create mode 100644 osal/linux/unixUtility.h

diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index a779bdcb..ef846f9c 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -1,7 +1,7 @@
 #include "MessageQueue.h"
 #include "QueueMapManager.h"
 
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../ipc/MutexFactory.h"
 #include "../../ipc/MutexGuard.h"
 
@@ -13,8 +13,9 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
 	auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "MessageQueue::MessageQueue:"
-		        << " Could not be created" << std::endl;
+		sif::error << "MessageQueue::MessageQueue: Could not be created" << std::endl;
+#else
+		sif::printError("MessageQueue::MessageQueue: Could not be created\n");
 #endif
 	}
 }
diff --git a/osal/linux/BinarySemaphore.cpp b/osal/linux/BinarySemaphore.cpp
index 5716e560..110b0a90 100644
--- a/osal/linux/BinarySemaphore.cpp
+++ b/osal/linux/BinarySemaphore.cpp
@@ -1,4 +1,5 @@
 #include "BinarySemaphore.h"
+#include "unixUtility.h"
 #include "../../serviceinterface/ServiceInterfacePrinter.h"
 #include "../../serviceinterface/ServiceInterfaceStream.h"
 
@@ -8,154 +9,154 @@
 
 
 BinarySemaphore::BinarySemaphore() {
-	// Using unnamed semaphores for now
-	initSemaphore();
+    // Using unnamed semaphores for now
+    initSemaphore();
 }
 
 BinarySemaphore::~BinarySemaphore() {
-	sem_destroy(&handle);
+    sem_destroy(&handle);
 }
 
 BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) {
-	initSemaphore();
+    initSemaphore();
 }
 
 BinarySemaphore& BinarySemaphore::operator =(
         BinarySemaphore&& s) {
-	initSemaphore();
-	return * this;
+    initSemaphore();
+    return * this;
 }
 
 ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType,
-		 uint32_t timeoutMs) {
-	int result = 0;
-	if(timeoutType == TimeoutType::POLLING) {
-		result = sem_trywait(&handle);
-	}
-	else if(timeoutType == TimeoutType::BLOCKING) {
-	    result = sem_wait(&handle);
-	}
-	else if(timeoutType == TimeoutType::WAITING){
-		timespec timeOut;
-		clock_gettime(CLOCK_REALTIME, &timeOut);
-		uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec;
-		nseconds += timeoutMs * 1000000;
-		timeOut.tv_sec = nseconds / 1000000000;
-		timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000;
-	    result = sem_timedwait(&handle, &timeOut);
-	    if(result != 0 and errno == EINVAL) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-	    	sif::debug << "BinarySemaphore::acquire: Invalid time value possible"
-	    			<< std::endl;
-#endif
-	    }
-	}
-	if(result == 0) {
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+        uint32_t timeoutMs) {
+    int result = 0;
+    if(timeoutType == TimeoutType::POLLING) {
+        result = sem_trywait(&handle);
+    }
+    else if(timeoutType == TimeoutType::BLOCKING) {
+        result = sem_wait(&handle);
+    }
+    else if(timeoutType == TimeoutType::WAITING){
+        timespec timeOut;
+        clock_gettime(CLOCK_REALTIME, &timeOut);
+        uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec;
+        nseconds += timeoutMs * 1000000;
+        timeOut.tv_sec = nseconds / 1000000000;
+        timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000;
+        result = sem_timedwait(&handle, &timeOut);
+        if(result != 0 and errno == EINVAL) {
+            utility::printUnixErrorGeneric(CLASS_NAME, "acquire", "sem_timedwait");
+        }
+    }
+    if(result == 0) {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
 
-	switch(errno) {
-	case(EAGAIN):
-		// Operation could not be performed without blocking (for sem_trywait)
-	case(ETIMEDOUT):
-		// Semaphore is 0
-		return SemaphoreIF::SEMAPHORE_TIMEOUT;
-	case(EINVAL):
-		// Semaphore invalid
-		return SemaphoreIF::SEMAPHORE_INVALID;
-	case(EINTR):
-		// Call was interrupted by signal handler
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "BinarySemaphore::acquire: Signal handler interrupted."
-				"Code " << strerror(errno) << std::endl;
-#endif
-		/* No break */
-	default:
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
+    switch(errno) {
+    case(EAGAIN):
+		                // Operation could not be performed without blocking (for sem_trywait)
+    case(ETIMEDOUT): {
+        // Semaphore is 0
+        utility::printUnixErrorGeneric(CLASS_NAME, "acquire", "ETIMEDOUT");
+        return SemaphoreIF::SEMAPHORE_TIMEOUT;
+    }
+    case(EINVAL): {
+        // Semaphore invalid
+        utility::printUnixErrorGeneric(CLASS_NAME, "acquire", "EINVAL");
+        return SemaphoreIF::SEMAPHORE_INVALID;
+    }
+    case(EINTR): {
+        // Call was interrupted by signal handler
+        utility::printUnixErrorGeneric(CLASS_NAME, "acquire", "EINTR");
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    default:
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 }
 
 ReturnValue_t BinarySemaphore::release() {
-	return BinarySemaphore::release(&this->handle);
+    return BinarySemaphore::release(&this->handle);
 }
 
 ReturnValue_t BinarySemaphore::release(sem_t *handle) {
-	ReturnValue_t countResult = checkCount(handle, 1);
-	if(countResult != HasReturnvaluesIF::RETURN_OK) {
-		return countResult;
-	}
+    ReturnValue_t countResult = checkCount(handle, 1);
+    if(countResult != HasReturnvaluesIF::RETURN_OK) {
+        return countResult;
+    }
 
-	int result = sem_post(handle);
-	if(result == 0) {
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+    int result = sem_post(handle);
+    if(result == 0) {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
 
-	switch(errno) {
-	case(EINVAL):
-		// Semaphore invalid
-		return SemaphoreIF::SEMAPHORE_INVALID;
-	case(EOVERFLOW):
-		// SEM_MAX_VALUE overflow. This should never happen
-	default:
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
+    switch(errno) {
+    case(EINVAL): {
+        // Semaphore invalid
+        utility::printUnixErrorGeneric(CLASS_NAME, "release", "EINVAL");
+        return SemaphoreIF::SEMAPHORE_INVALID;
+    }
+    case(EOVERFLOW): {
+        // SEM_MAX_VALUE overflow. This should never happen
+        utility::printUnixErrorGeneric(CLASS_NAME, "release", "EOVERFLOW");
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    default:
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 }
 
 uint8_t BinarySemaphore::getSemaphoreCounter() const {
-	// And another ugly cast :-D
-	return getSemaphoreCounter(const_cast<sem_t*>(&this->handle));
+    // And another ugly cast :-D
+    return getSemaphoreCounter(const_cast<sem_t*>(&this->handle));
 }
 
 uint8_t BinarySemaphore::getSemaphoreCounter(sem_t *handle) {
-	int value = 0;
-	int result = sem_getvalue(handle, &value);
-	if (result == 0) {
-		return value;
-	}
-	else if(result != 0 and errno == EINVAL) {
-		// Could be called from interrupt, use lightweight printf
-		sif::printError("BinarySemaphore::getSemaphoreCounter: "
-				"Invalid semaphore\n");
-		return 0;
-	}
-	else {
-		// This should never happen.
-		return 0;
-	}
+    int value = 0;
+    int result = sem_getvalue(handle, &value);
+    if (result == 0) {
+        return value;
+    }
+    else if(result != 0 and errno == EINVAL) {
+        // Could be called from interrupt, use lightweight printf
+        sif::printError("BinarySemaphore::getSemaphoreCounter: "
+                "Invalid semaphore\n");
+        return 0;
+    }
+    else {
+        // This should never happen.
+        return 0;
+    }
 }
 
 void BinarySemaphore::initSemaphore(uint8_t initCount) {
-	auto result = sem_init(&handle, true, initCount);
-	if(result == -1) {
-		switch(errno) {
-		case(EINVAL):
-			// Value exceeds SEM_VALUE_MAX
-		case(ENOSYS): {
-			// System does not support process-shared semaphores
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-				sif::error << "BinarySemaphore: Init failed with "
-						<< strerror(errno) << std::endl;
-#else
-				sif::printError("BinarySemaphore: Init failed with %s\n",
-						strerror(errno));
-#endif
-		}
-		}
-	}
+    auto result = sem_init(&handle, true, initCount);
+    if(result == -1) {
+        switch(errno) {
+        case(EINVAL): {
+            utility::printUnixErrorGeneric(CLASS_NAME, "initSemaphore", "EINVAL");
+            break;
+        }
+        case(ENOSYS): {
+            // System does not support process-shared semaphores
+            utility::printUnixErrorGeneric(CLASS_NAME, "initSemaphore", "ENOSYS");
+            break;
+        }
+        }
+    }
 }
 
 ReturnValue_t BinarySemaphore::checkCount(sem_t* handle, uint8_t maxCount) {
-	int value = getSemaphoreCounter(handle);
-	if(value >= maxCount) {
-		if(maxCount == 1 and value > 1) {
-			// Binary Semaphore special case.
-			// This is a config error use lightweight printf is this is called
-			// from an interrupt
-			printf("BinarySemaphore::release: Value of binary semaphore greater"
-					" than 1!\n");
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		return SemaphoreIF::SEMAPHORE_NOT_OWNED;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+    int value = getSemaphoreCounter(handle);
+    if(value >= maxCount) {
+        if(maxCount == 1 and value > 1) {
+            // Binary Semaphore special case.
+            // This is a config error use lightweight printf is this is called
+            // from an interrupt
+            printf("BinarySemaphore::release: Value of binary semaphore greater than 1!\n");
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        return SemaphoreIF::SEMAPHORE_NOT_OWNED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
diff --git a/osal/linux/BinarySemaphore.h b/osal/linux/BinarySemaphore.h
index e9bb8bb6..2e6ded15 100644
--- a/osal/linux/BinarySemaphore.h
+++ b/osal/linux/BinarySemaphore.h
@@ -76,6 +76,7 @@ public:
 	static ReturnValue_t checkCount(sem_t* handle, uint8_t maxCount);
 protected:
 	sem_t handle;
+    static constexpr const char* CLASS_NAME = "BinarySemaphore";
 };
 
 #endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */
diff --git a/osal/linux/CMakeLists.txt b/osal/linux/CMakeLists.txt
index 332fe2f4..0fb66b3e 100644
--- a/osal/linux/CMakeLists.txt
+++ b/osal/linux/CMakeLists.txt
@@ -15,6 +15,7 @@ target_sources(${LIB_FSFW_NAME}
 		TaskFactory.cpp
 		Timer.cpp
 		tcpipHelpers.cpp
+		unixUtility.cpp
 )
 
 find_package(Threads REQUIRED)
diff --git a/osal/linux/CountingSemaphore.cpp b/osal/linux/CountingSemaphore.cpp
index 07c52212..752e150b 100644
--- a/osal/linux/CountingSemaphore.cpp
+++ b/osal/linux/CountingSemaphore.cpp
@@ -1,58 +1,70 @@
-#include "../../osal/linux/CountingSemaphore.h"
+#include "CountingSemaphore.h"
+#include "unixUtility.h"
+
 #include "../../serviceinterface/ServiceInterface.h"
 
 #include <errno.h>
 
 CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount):
-		maxCount(maxCount), initCount(initCount) {
-	if(initCount > maxCount) {
+        maxCount(maxCount), initCount(initCount) {
+    if(initCount > maxCount) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "CountingSemaphoreUsingTask: Max count bigger than "
-				"intial cout. Setting initial count to max count." << std::endl;
+        sif::warning << "CountingSemaphoreUsingTask: Max count bigger than "
+                "intial cout. Setting initial count to max count" << std::endl;
+#else
+        sif::printWarning("CountingSemaphoreUsingTask: Max count bigger than "
+                "intial cout. Setting initial count to max count\n");
 #endif
-		initCount = maxCount;
-	}
+        initCount = maxCount;
+    }
 
-	initSemaphore(initCount);
+    initSemaphore(initCount);
 }
 
 CountingSemaphore::CountingSemaphore(CountingSemaphore&& other):
-		maxCount(other.maxCount), initCount(other.initCount) {
-	initSemaphore(initCount);
+		        maxCount(other.maxCount), initCount(other.initCount) {
+    initSemaphore(initCount);
 }
 
 CountingSemaphore& CountingSemaphore::operator =(
-		CountingSemaphore&& other) {
-	initSemaphore(other.initCount);
-	return * this;
+        CountingSemaphore&& other) {
+    initSemaphore(other.initCount);
+    return * this;
 }
 
 ReturnValue_t CountingSemaphore::release() {
-	ReturnValue_t result = checkCount(&handle, maxCount);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-	return CountingSemaphore::release(&this->handle);
+    ReturnValue_t result = checkCount(&handle, maxCount);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    return CountingSemaphore::release(&this->handle);
 }
 
 ReturnValue_t CountingSemaphore::release(sem_t* handle) {
-	int result = sem_post(handle);
-	if(result == 0) {
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+    int result = sem_post(handle);
+    if(result == 0) {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
 
-	switch(errno) {
-	case(EINVAL):
-		// Semaphore invalid
-		return SemaphoreIF::SEMAPHORE_INVALID;
-	case(EOVERFLOW):
-		// SEM_MAX_VALUE overflow. This should never happen
-	default:
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
+    switch(errno) {
+    case(EINVAL): {
+        // Semaphore invalid
+        utility::printUnixErrorGeneric("CountingSemaphore", "release", "EINVAL");
+        return SemaphoreIF::SEMAPHORE_INVALID;
+    }
+
+    case(EOVERFLOW): {
+        // SEM_MAX_VALUE overflow. This should never happen
+        utility::printUnixErrorGeneric("CountingSemaphore", "release", "EOVERFLOW");
+        return SemaphoreIF::SEMAPHORE_INVALID;
+    }
+
+    default:
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 }
 
 uint8_t CountingSemaphore::getMaxCount() const {
-	return maxCount;
+    return maxCount;
 }
 
diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index bfd97e56..a75dff7a 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -1,4 +1,5 @@
 #include "MessageQueue.h"
+#include "unixUtility.h"
 #include "../../serviceinterface/ServiceInterface.h"
 #include "../../objectmanager/ObjectManagerIF.h"
 
@@ -13,7 +14,6 @@
 MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize):
         id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE),
         defaultDestination(MessageQueueIF::NO_QUEUE), maxMessageSize(maxMessageSize) {
-    //debug << "MessageQueue::MessageQueue: Creating a queue" << std::endl;
     mq_attr attributes;
     this->id = 0;
     //Set attributes
@@ -30,7 +30,7 @@ MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize):
     mode_t mode = S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP | S_IROTH | S_IWOTH;
     mqd_t tempId = mq_open(name, oflag, mode, &attributes);
     if (tempId == -1) {
-        handleError(&attributes, messageDepth);
+        handleOpenError(&attributes, messageDepth);
     }
     else {
         //Successful mq_open call
@@ -41,101 +41,14 @@ MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize):
 MessageQueue::~MessageQueue() {
     int status = mq_close(this->id);
     if(status != 0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "MessageQueue::Destructor: mq_close Failed with status: "
-                << strerror(errno) <<std::endl;
-#endif
+        utility::printUnixErrorGeneric(CLASS_NAME, "~MessageQueue", "close");
     }
     status = mq_unlink(name);
     if(status != 0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "MessageQueue::Destructor: mq_unlink Failed with status: "
-                << strerror(errno) << std::endl;
-#endif
+        utility::printUnixErrorGeneric(CLASS_NAME, "~MessageQueue", "unlink");
     }
 }
 
-ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
-        uint32_t messageDepth) {
-    switch(errno) {
-    case(EINVAL): {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "MessageQueue::MessageQueue: Invalid name or attributes"
-                " for message size" << std::endl;
-#endif
-        size_t defaultMqMaxMsg = 0;
-        // Not POSIX conformant, but should work for all UNIX systems.
-        // Just an additional helpful printout :-)
-        if(std::ifstream("/proc/sys/fs/mqueue/msg_max",std::ios::in) >>
-                defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) {
-            /*
-			See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html
-			This happens if the msg_max value is not large enough
-			It is ignored if the executable is run in privileged mode.
-		    Run the unlockRealtime script or grant the mode manually by using:
-			sudo setcap 'CAP_SYS_RESOURCE=+ep' <pathToBinary>
-
-			Persistent solution for session:
-			echo <newMsgMax> | sudo tee /proc/sys/fs/mqueue/msg_max
-
-			Permanent solution:
-			sudo nano /etc/sysctl.conf
-			Append at end: fs/mqueue/msg_max = <newMsgMaxLen>
-			Apply changes with: sudo sysctl -p
-             */
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::MessageQueue: Default MQ size "
-                    << defaultMqMaxMsg << " is too small for requested size "
-                    << messageDepth << std::endl;
-            sif::error << "This error can be fixed by setting the maximum "
-                    "allowed message size higher!" << std::endl;
-#endif
-        }
-        break;
-    }
-    case(EEXIST): {
-        // An error occured during open
-        // We need to distinguish if it is caused by an already created queue
-        // There's another queue with the same name
-        // We unlink the other queue
-        int status = mq_unlink(name);
-        if (status != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "mq_unlink Failed with status: " << strerror(errno)
-													        << std::endl;
-#endif
-        }
-        else {
-            // Successful unlinking, try to open again
-            mqd_t tempId = mq_open(name,
-                    O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL,
-                    S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP, attributes);
-            if (tempId != -1) {
-                //Successful mq_open
-                this->id = tempId;
-                return HasReturnvaluesIF::RETURN_OK;
-            }
-        }
-        break;
-    }
-
-    default: {
-        // Failed either the first time or the second time
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "MessageQueue::MessageQueue: Creating Queue " << name
-                << " failed with status: " << strerror(errno) << std::endl;
-#else
-        sif::printError("MessageQueue::MessageQueue: Creating Queue %s"
-                " failed with status: %s\n", name, strerror(errno));
-#endif
-    }
-    }
-    return HasReturnvaluesIF::RETURN_FAILED;
-
-
-
-}
-
 ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
         MessageQueueMessageIF* message, bool ignoreFault) {
     return sendMessageFrom(sendTo, message, this->getId(), false);
@@ -204,7 +117,8 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
             return MessageQueueIF::EMPTY;
         case EBADF: {
             //mqdes doesn't represent a valid queue open for reading.
-            return handleRecvError("EBADF");
+            utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EBADF");
+            break;
         }
         case EINVAL: {
             /*
@@ -216,7 +130,8 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
              *   queue, and the QNX extended option MQ_READBUF_DYNAMIC hasn't
              *   been set in the queue's mq_flags.
              */
-            return handleRecvError("EINVAL");
+            utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EINVAL");
+            break;
         }
         case EMSGSIZE: {
             /*
@@ -228,23 +143,25 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
              *   given msg_len is too short for the message that would have
              *   been received.
              */
-            return handleRecvError("EMSGSIZE");
+            utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EMSGSIZE");
+            break;
         }
 
         case EINTR: {
             //The operation was interrupted by a signal.
-            return handleRecvError("EINTR");
+            utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EINTR");
+            break;
         }
         case ETIMEDOUT: {
             //The operation was interrupted by a signal.
-            return handleRecvError("ETIMEDOUT");
+            utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "ETIMEDOUT");
+            break;
         }
 
         default:
-
             return HasReturnvaluesIF::RETURN_FAILED;
         }
-
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
 }
 
@@ -259,29 +176,27 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) {
         switch(errno){
         case EBADF:
             //mqdes doesn't represent a valid message queue.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::flush configuration error, "
-            "called flush with an invalid queue ID" << std::endl;
-#endif
+            utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EBADF");
+            break;
             /*NO BREAK*/
         case EINVAL:
             //mq_attr is NULL
+            utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EINVAL");
+            break;
         default:
             return HasReturnvaluesIF::RETURN_FAILED;
         }
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
     *count = attrib.mq_curmsgs;
     attrib.mq_curmsgs = 0;
     status = mq_setattr(id,&attrib,NULL);
     if(status != 0){
-        switch(errno){
+        switch(errno) {
         case EBADF:
             //mqdes doesn't represent a valid message queue.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::flush configuration error, "
-            "called flush with an invalid queue ID" << std::endl;
-#endif
-            /*NO BREAK*/
+            utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EBADF");
+            break;
         case EINVAL:
             /*
              * This value indicates one of the following:
@@ -290,9 +205,12 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) {
              *    mq_flags includes a 0 in the MQ_MULT_NOTIFY bit. Once
              *    MQ_MULT_NOTIFY has been turned on, it may never be turned off.
              */
+            utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EINVAL");
+            break;
         default:
             return HasReturnvaluesIF::RETURN_FAILED;
         }
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
@@ -333,8 +251,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         bool ignoreFault) {
     if(message == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is "
-                "nullptr!" << std::endl;
+        sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr!" << std::endl;
+#else
+        sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr!\n");
 #endif
         return HasReturnvaluesIF::RETURN_FAILED;
     }
@@ -348,8 +267,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
     if (result != 0) {
         if(!ignoreFault){
             InternalErrorReporterIF* internalErrorReporter =
-                    objectManager->get<InternalErrorReporterIF>(
-                            objects::INTERNAL_ERROR_REPORTER);
+                    objectManager->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
             if (internalErrorReporter != NULL) {
                 internalErrorReporter->queueMessageNotSent();
             }
@@ -363,17 +281,20 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         case EBADF: {
             //mq_des doesn't represent a valid message queue descriptor,
             //or mq_des wasn't opened for writing.
+
+            utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EBADF");
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::sendMessage: Configuration error, MQ"
-                    << " destination invalid."  << std::endl;
-            sif::error << strerror(errno) << " in "
-                    <<"mq_send to: " << sendTo << " sent from "
-                    << sentFrom << std::endl;
+            sif::warning << "mq_send to: " << sendTo << " sent from "
+                    << sentFrom << "failed" << std::endl;
+#else
+            sif::printWarning("mq_send to: %d sent from %d failed\n", sendTo, sentFrom);
 #endif
             return DESTINATION_INVALID;
         }
         case EINTR:
             //The call was interrupted by a signal.
+            utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EINTR");
+            break;
         case EINVAL:
             /*
              * This value indicates one of the following:
@@ -384,36 +305,87 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
              * - MQ_PRIO_RESTRICT is set in the mq_attr of mq_des, and
              *   msg_prio is greater than the priority of the calling process.
              */
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::sendMessage: Configuration error "
-            << strerror(errno) << " in mq_send" << std::endl;
-#endif
-            /*NO BREAK*/
+            utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EINVAL");
+            break;
         case EMSGSIZE:
             // The msg_len is greater than the msgsize associated with
             //the specified queue.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "MessageQueue::sendMessage: Size error [" <<
-            strerror(errno) << "] in mq_send" << std::endl;
-#endif
-            /*NO BREAK*/
+            utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EMSGSIZE");
+            break;
         default:
             return HasReturnvaluesIF::RETURN_FAILED;
         }
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
 
-ReturnValue_t MessageQueue::handleRecvError(const char * const failString) {
-    if(failString == nullptr) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
+ReturnValue_t MessageQueue::handleOpenError(mq_attr* attributes,
+        uint32_t messageDepth) {
+    switch(errno) {
+    case(EINVAL): {
+        utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "EINVAL");
+        size_t defaultMqMaxMsg = 0;
+        // Not POSIX conformant, but should work for all UNIX systems.
+        // Just an additional helpful printout :-)
+        if(std::ifstream("/proc/sys/fs/mqueue/msg_max",std::ios::in) >>
+                defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) {
+            /*
+            See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html
+            This happens if the msg_max value is not large enough
+            It is ignored if the executable is run in privileged mode.
+            Run the unlockRealtime script or grant the mode manually by using:
+            sudo setcap 'CAP_SYS_RESOURCE=+ep' <pathToBinary>
+
+            Persistent solution for session:
+            echo <newMsgMax> | sudo tee /proc/sys/fs/mqueue/msg_max
+
+            Permanent solution:
+            sudo nano /etc/sysctl.conf
+            Append at end: fs/mqueue/msg_max = <newMsgMaxLen>
+            Apply changes with: sudo sysctl -p
+             */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "MessageQueue::receiveMessage: " << failString << " error "
-            << strerror(errno)  << std::endl;
+            sif::error << "MessageQueue::MessageQueue: Default MQ size " << defaultMqMaxMsg <<
+                    " is too small for requested size " << messageDepth << std::endl;
+            sif::error << "This error can be fixed by setting the maximum "
+                    "allowed message size higher!" << std::endl;
 #else
-    sif::printError("MessageQueue::receiveMessage: %s error %s\n", failString,
-            strerror(errno));
+            sif::printError("MessageQueue::MessageQueue: Default MQ size %d is too small for"
+                    "requested size %d\n");
+            sif::printError("This error can be fixes by setting the maximum allowed"
+                    "message size higher!\n");
 #endif
+        }
+        break;
+    }
+    case(EEXIST): {
+        // An error occured during open.
+        // We need to distinguish if it is caused by an already created queue
+        // There's another queue with the same name
+        // We unlink the other queue
+        int status = mq_unlink(name);
+        if (status != 0) {
+            utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "EEXIST");
+        }
+        else {
+            // Successful unlinking, try to open again
+            mqd_t tempId = mq_open(name,
+                    O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL,
+                    S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP, attributes);
+            if (tempId != -1) {
+                //Successful mq_open
+                this->id = tempId;
+                return HasReturnvaluesIF::RETURN_OK;
+            }
+        }
+        break;
+    }
+
+    default: {
+        // Failed either the first time or the second time
+        utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "Unknown");
+    }
+    }
     return HasReturnvaluesIF::RETURN_FAILED;
 }
diff --git a/osal/linux/MessageQueue.h b/osal/linux/MessageQueue.h
index 0bef0a73..935ee948 100644
--- a/osal/linux/MessageQueue.h
+++ b/osal/linux/MessageQueue.h
@@ -181,8 +181,8 @@ private:
 	static uint16_t queueCounter;
 	const size_t maxMessageSize;
 
-	ReturnValue_t handleError(mq_attr* attributes, uint32_t messageDepth);
-	ReturnValue_t handleRecvError(const char* const failString);
+	static constexpr const char* CLASS_NAME = "MessageQueue";
+	ReturnValue_t handleOpenError(mq_attr* attributes, uint32_t messageDepth);
 };
 
 #endif /* FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ */
diff --git a/osal/linux/Mutex.cpp b/osal/linux/Mutex.cpp
index c642b132..33e84aac 100644
--- a/osal/linux/Mutex.cpp
+++ b/osal/linux/Mutex.cpp
@@ -1,43 +1,34 @@
 #include "Mutex.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "unixUtility.h"
+
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../timemanager/Clock.h"
 
-uint8_t Mutex::count = 0;
-
-
 #include <cstring>
 #include <errno.h>
 
+uint8_t Mutex::count = 0;
+
 Mutex::Mutex() {
 	pthread_mutexattr_t mutexAttr;
 	int status = pthread_mutexattr_init(&mutexAttr);
 	if (status != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Mutex: Attribute init failed with: " << strerror(status) << std::endl;
-#endif
+	    utility::printUnixErrorGeneric("Mutex", "Mutex", "pthread_mutexattr_init");
 	}
 	status = pthread_mutexattr_setprotocol(&mutexAttr, PTHREAD_PRIO_INHERIT);
 	if (status != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Mutex: Attribute set PRIO_INHERIT failed with: " << strerror(status)
-				<< std::endl;
-#endif
+	    utility::printUnixErrorGeneric("Mutex", "Mutex", "pthread_mutexattr_setprotocol");
 	}
 	status = pthread_mutex_init(&mutex, &mutexAttr);
 	if (status != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Mutex: creation with name, id " << mutex.__data.__count
-				<< ", " << " failed with " << strerror(status) << std::endl;
-#endif
+	    utility::printUnixErrorGeneric("Mutex", "Mutex", "pthread_mutex_init");
 	}
 	// After a mutex attributes object has been used to initialize one or more
 	// mutexes, any function affecting the attributes object
 	// (including destruction) shall not affect any previously initialized mutexes.
 	status = pthread_mutexattr_destroy(&mutexAttr);
 	if (status != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Mutex: Attribute destroy failed with " << strerror(status) << std::endl;
-#endif
+	    utility::printUnixErrorGeneric("Mutex", "Mutex", "pthread_mutexattr_destroy");
 	}
 }
 
diff --git a/osal/linux/PeriodicPosixTask.cpp b/osal/linux/PeriodicPosixTask.cpp
index 630dd8e0..c60f639f 100644
--- a/osal/linux/PeriodicPosixTask.cpp
+++ b/osal/linux/PeriodicPosixTask.cpp
@@ -1,7 +1,8 @@
-#include "../../tasks/ExecutableObjectIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
-#include <errno.h>
 #include "PeriodicPosixTask.h"
+#include "../../tasks/ExecutableObjectIF.h"
+#include "../../serviceinterface/ServiceInterface.h"
+
+#include <errno.h>
 
 PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_,
 		size_t stackSize_, uint32_t period_, void(deadlineMissedFunc_)()):
@@ -28,6 +29,9 @@ ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PeriodicTask::addComponent: Invalid object. Make sure"
 				<< " it implements ExecutableObjectIF!" << std::endl;
+#else
+		sif::printError("PeriodicTask::addComponent: Invalid object. Make sure it"
+		        "implements ExecutableObjectIF!\n");
 #endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
@@ -44,9 +48,6 @@ ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) {
 
 ReturnValue_t PeriodicPosixTask::startTask(void) {
 	started = true;
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-	//sif::info << stackSize << std::endl;
-#endif
 	PosixThread::createTask(&taskEntryPoint,this);
 	return HasReturnvaluesIF::RETURN_OK;
 }
diff --git a/osal/linux/PosixThread.cpp b/osal/linux/PosixThread.cpp
index 72adfb14..36501282 100644
--- a/osal/linux/PosixThread.cpp
+++ b/osal/linux/PosixThread.cpp
@@ -1,4 +1,5 @@
 #include "PosixThread.h"
+#include "unixUtility.h"
 
 #include "../../serviceinterface/ServiceInterface.h"
 
@@ -6,263 +7,240 @@
 #include <errno.h>
 
 PosixThread::PosixThread(const char* name_, int priority_, size_t stackSize_):
-		thread(0), priority(priority_), stackSize(stackSize_) {
+        thread(0), priority(priority_), stackSize(stackSize_) {
     name[0] = '\0';
     std::strncat(name, name_, PTHREAD_MAX_NAMELEN - 1);
 }
 
 PosixThread::~PosixThread() {
-	//No deletion and no free of Stack Pointer
+    //No deletion and no free of Stack Pointer
 }
 
 ReturnValue_t PosixThread::sleep(uint64_t ns) {
-	//TODO sleep might be better with timer instead of sleep()
-	timespec time;
-	time.tv_sec = ns/1000000000;
-	time.tv_nsec = ns - time.tv_sec*1e9;
+    //TODO sleep might be better with timer instead of sleep()
+    timespec time;
+    time.tv_sec = ns/1000000000;
+    time.tv_nsec = ns - time.tv_sec*1e9;
 
-	//Remaining Time is not set here
-	int status = nanosleep(&time,NULL);
-	if(status != 0){
-		switch(errno){
-		case EINTR:
-			//The nanosleep() function was interrupted by a signal.
-			return HasReturnvaluesIF::RETURN_FAILED;
-		case EINVAL:
-			//The rqtp argument specified a nanosecond value less than zero or
-			// greater than or equal to 1000 million.
-			return HasReturnvaluesIF::RETURN_FAILED;
-		default:
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
+    //Remaining Time is not set here
+    int status = nanosleep(&time,NULL);
+    if(status != 0){
+        switch(errno){
+        case EINTR:
+            //The nanosleep() function was interrupted by a signal.
+            return HasReturnvaluesIF::RETURN_FAILED;
+        case EINVAL:
+            //The rqtp argument specified a nanosecond value less than zero or
+            // greater than or equal to 1000 million.
+            return HasReturnvaluesIF::RETURN_FAILED;
+        default:
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
 
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void PosixThread::suspend() {
-	//Wait for SIGUSR1
-	int caughtSig = 0;
-	sigset_t waitSignal;
-	sigemptyset(&waitSignal);
-	sigaddset(&waitSignal, SIGUSR1);
-	sigwait(&waitSignal, &caughtSig);
-	if (caughtSig != SIGUSR1) {
+    //Wait for SIGUSR1
+    int caughtSig = 0;
+    sigset_t waitSignal;
+    sigemptyset(&waitSignal);
+    sigaddset(&waitSignal, SIGUSR1);
+    sigwait(&waitSignal, &caughtSig);
+    if (caughtSig != SIGUSR1) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "FixedTimeslotTask: Unknown Signal received: " <<
-				caughtSig << std::endl;
+        sif::error << "FixedTimeslotTask::suspend: Unknown Signal received: " << caughtSig <<
+                std::endl;
+#else
+        sif::printError("FixedTimeslotTask::suspend: Unknown Signal received: %d\n", caughtSig);
 #endif
-	}
+    }
 }
 
 void PosixThread::resume(){
-	/* Signal the thread to start. Makes sense to call kill to start or? ;)
-	 *
-	*	According to Posix raise(signal) will call pthread_kill(pthread_self(), sig),
-	*	but as the call must be done from the thread itsself this is not possible here
-	*/
-	pthread_kill(thread,SIGUSR1);
+    /* Signal the thread to start. Makes sense to call kill to start or? ;)
+     *
+     *	According to Posix raise(signal) will call pthread_kill(pthread_self(), sig),
+     *	but as the call must be done from the thread itsself this is not possible here
+     */
+    pthread_kill(thread,SIGUSR1);
 }
 
 bool PosixThread::delayUntil(uint64_t* const prevoiusWakeTime_ms,
-		const uint64_t delayTime_ms) {
-	uint64_t nextTimeToWake_ms;
-	bool shouldDelay = false;
-	//Get current Time
-	const uint64_t currentTime_ms = getCurrentMonotonicTimeMs();
-	/* Generate the tick time at which the task wants to wake. */
-	nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms;
+        const uint64_t delayTime_ms) {
+    uint64_t nextTimeToWake_ms;
+    bool shouldDelay = false;
+    //Get current Time
+    const uint64_t currentTime_ms = getCurrentMonotonicTimeMs();
+    /* Generate the tick time at which the task wants to wake. */
+    nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms;
 
-	if (currentTime_ms < *prevoiusWakeTime_ms) {
-		/* The tick count has overflowed since this function was
+    if (currentTime_ms < *prevoiusWakeTime_ms) {
+        /* The tick count has overflowed since this function was
 		lasted called.  In this case the only time we should ever
 		actually delay is if the wake time has also overflowed,
 		and the wake time is greater than the tick time. When this
 		is the case it is as if neither time had overflowed. */
-		if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
-				&& (nextTimeToWake_ms > currentTime_ms)) {
-			shouldDelay = true;
-		}
-	} else {
-		/* The tick time has not overflowed. In this case we will
+        if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
+                && (nextTimeToWake_ms > currentTime_ms)) {
+            shouldDelay = true;
+        }
+    } else {
+        /* The tick time has not overflowed. In this case we will
 		delay if either the wake time has overflowed, and/or the
 		tick time is less than the wake time. */
-		if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
-				|| (nextTimeToWake_ms > currentTime_ms)) {
-			shouldDelay = true;
-		}
-	}
+        if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
+                || (nextTimeToWake_ms > currentTime_ms)) {
+            shouldDelay = true;
+        }
+    }
 
-	/* Update the wake time ready for the next call. */
+    /* Update the wake time ready for the next call. */
 
-	(*prevoiusWakeTime_ms) =  nextTimeToWake_ms;
+    (*prevoiusWakeTime_ms) =  nextTimeToWake_ms;
 
-	if (shouldDelay) {
-		uint64_t sleepTime = nextTimeToWake_ms - currentTime_ms;
-		PosixThread::sleep(sleepTime * 1000000ull);
-		return true;
-	}
-	//We are shifting the time in case the deadline was missed like rtems
-	(*prevoiusWakeTime_ms) = currentTime_ms;
-	return false;
+    if (shouldDelay) {
+        uint64_t sleepTime = nextTimeToWake_ms - currentTime_ms;
+        PosixThread::sleep(sleepTime * 1000000ull);
+        return true;
+    }
+    //We are shifting the time in case the deadline was missed like rtems
+    (*prevoiusWakeTime_ms) = currentTime_ms;
+    return false;
 
 }
 
 
 uint64_t PosixThread::getCurrentMonotonicTimeMs(){
-	timespec timeNow;
-	clock_gettime(CLOCK_MONOTONIC_RAW, &timeNow);
-	uint64_t currentTime_ms = (uint64_t) timeNow.tv_sec * 1000
-				+ timeNow.tv_nsec / 1000000;
+    timespec timeNow;
+    clock_gettime(CLOCK_MONOTONIC_RAW, &timeNow);
+    uint64_t currentTime_ms = (uint64_t) timeNow.tv_sec * 1000
+            + timeNow.tv_nsec / 1000000;
 
-	return currentTime_ms;
+    return currentTime_ms;
 }
 
 
 void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	//sif::debug << "PosixThread::createTask" << std::endl;
+    //sif::debug << "PosixThread::createTask" << std::endl;
 #endif
-	/*
-	 * The attr argument points to a pthread_attr_t structure whose contents
+    /*
+     * The attr argument points to a pthread_attr_t structure whose contents
        are used at thread creation time to determine attributes for the new
        thread; this structure is initialized using pthread_attr_init(3) and
        related functions.  If attr is NULL, then the thread is created with
        default attributes.
-	 */
-	pthread_attr_t attributes;
-	int status = pthread_attr_init(&attributes);
-	if(status != 0){
+     */
+    pthread_attr_t attributes;
+    int status = pthread_attr_init(&attributes);
+    if(status != 0){
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_init");
+    }
+    void* stackPointer;
+    status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize);
+    if(status != 0) {
+        if(errno == ENOMEM) {
+            size_t stackMb = stackSize/10e6;
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread attribute init failed with: " <<
-				strerror(status) << std::endl;
-#endif
-	}
-	void* stackPointer;
-	status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize);
-	if(status != 0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PosixThread::createTask: Stack init failed with: " <<
-				strerror(status) << std::endl;
-#endif
-		if(errno == ENOMEM) {
-			size_t stackMb = stackSize/10e6;
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "PosixThread::createTask: Insufficient memory for"
-					" the requested " << stackMb << " MB" << std::endl;
+            sif::error << "PosixThread::createTask: Insufficient memory for"
+                    " the requested " << stackMb << " MB" << std::endl;
 #else
-			sif::printError("PosixThread::createTask: Insufficient memory for "
-					"the requested %lu MB\n", static_cast<unsigned long>(stackMb));
+            sif::printError("PosixThread::createTask: Insufficient memory for "
+                    "the requested %lu MB\n", static_cast<unsigned long>(stackMb));
 #endif
-		}
-		else if(errno == EINVAL) {
+            utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "ENOMEM");
+        }
+        else if(errno == EINVAL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "PosixThread::createTask: Wrong alignment argument!"
-					<< std::endl;
+            sif::error << "PosixThread::createTask: Wrong alignment argument!"
+                    << std::endl;
 #else
-			sif::printError("PosixThread::createTask: "
-					"Wrong alignment argument!\n");
+            sif::printError("PosixThread::createTask: Wrong alignment argument!\n");
 #endif
-		}
-		return;
-	}
+            utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "EINVAL");
+        }
+        return;
+    }
 
-	status = pthread_attr_setstack(&attributes, stackPointer, stackSize);
-	if(status != 0){
+    status = pthread_attr_setstack(&attributes, stackPointer, stackSize);
+    if(status != 0) {
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setstack");
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PosixThread::createTask: pthread_attr_setstack "
-				" failed with: " << strerror(status) <<  std::endl;
-		sif::error << "Make sure the specified stack size is valid and is "
-				"larger than the minimum allowed stack size." << std::endl;
+        sif::warning << "Make sure the specified stack size is valid and is "
+                "larger than the minimum allowed stack size." << std::endl;
+#else
+        sif::printWarning("Make sure the specified stack size is valid and is "
+                "larger than the minimum allowed stack size.\n");
 #endif
-	}
+    }
 
-	status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);
-	if(status != 0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "Posix Thread attribute setinheritsched failed with: " <<
-					strerror(status) << std::endl;
-#endif
-	}
+    status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);
+    if(status != 0){
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setinheritsched");
+    }
 #ifndef FSFW_USE_REALTIME_FOR_LINUX
 #error "Please define FSFW_USE_REALTIME_FOR_LINUX with either 0 or 1"
 #endif
 #if FSFW_USE_REALTIME_FOR_LINUX == 1
-	// FIFO -> This needs root privileges for the process
-	status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO);
-	if(status != 0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread attribute schedule policy failed with: " <<
-				strerror(status) << std::endl;
-#endif
-	}
+    // FIFO -> This needs root privileges for the process
+    status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO);
+    if(status != 0){
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedpolicy");
+    }
 
-	sched_param scheduleParams;
-	scheduleParams.__sched_priority = priority;
-	status = pthread_attr_setschedparam(&attributes, &scheduleParams);
-	if(status != 0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread attribute schedule params failed with: " <<
-				strerror(status) << std::endl;
+    sched_param scheduleParams;
+    scheduleParams.__sched_priority = priority;
+    status = pthread_attr_setschedparam(&attributes, &scheduleParams);
+    if(status != 0){
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedparam");
+    }
 #endif
-	}
-#endif
-	//Set Signal Mask for suspend until startTask is called
-	sigset_t waitSignal;
-	sigemptyset(&waitSignal);
-	sigaddset(&waitSignal, SIGUSR1);
-	status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL);
-	if(status != 0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread sigmask failed failed with: " <<
-				strerror(status) << " errno: " << strerror(errno) << std::endl;
-#endif
-	}
+    //Set Signal Mask for suspend until startTask is called
+    sigset_t waitSignal;
+    sigemptyset(&waitSignal);
+    sigaddset(&waitSignal, SIGUSR1);
+    status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL);
+    if(status != 0){
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_sigmask");
+    }
 
-
-	status = pthread_create(&thread,&attributes,fnc_,arg_);
-	if(status != 0){
+    status = pthread_create(&thread,&attributes,fnc_,arg_);
+    if(status != 0){
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_create");
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PosixThread::createTask: Failed with: " <<
-				strerror(status) << std::endl;
-		sif::error << "For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call " <<
-		        "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set "
-		        "/etc/security/limit.conf" << std::endl;
+        sif::error << "For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call " <<
+                "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set "
+                "/etc/security/limit.conf" << std::endl;
 #else
-		sif::printError("PosixThread::createTask: Create failed with: %s\n", strerror(status));
-		sif::printError("For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call "
-		        "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set "
+        sif::printError("For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call "
+                "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set "
                 "/etc/security/limit.conf\n");
 #endif
-	}
+    }
 
-	status = pthread_setname_np(thread,name);
-	if(status != 0){
+    status = pthread_setname_np(thread,name);
+    if(status != 0){
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_setname_np");
+        if(status == ERANGE) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PosixThread::createTask: setname failed with: " <<
-				strerror(status) << std::endl;
+            sif::warning << "PosixThread::createTask: Task name length longer"
+                    " than 16 chars. Truncating.." << std::endl;
+#else
+            sif::printWarning("PosixThread::createTask: Task name length longer"
+                    " than 16 chars. Truncating..\n");
 #endif
-		if(status == ERANGE) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "PosixThread::createTask: Task name length longer"
-					" than 16 chars. Truncating.." << std::endl;
-#endif
-			name[15] = '\0';
-			status = pthread_setname_np(thread,name);
-			if(status != 0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-				sif::error << "PosixThread::createTask: Setting name"
-						" did not work.." << std::endl;
-#endif
-			}
-		}
-	}
+            name[15] = '\0';
+            status = pthread_setname_np(thread,name);
+            if(status != 0){
+                utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_setname_np");
+            }
+        }
+    }
 
-	status = pthread_attr_destroy(&attributes);
-	if(status!=0){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Posix Thread attribute destroy failed with: " <<
-				strerror(status) << std::endl;
-#endif
-	}
+    status = pthread_attr_destroy(&attributes);
+    if (status != 0) {
+        utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_destroy");
+    }
 }
diff --git a/osal/linux/PosixThread.h b/osal/linux/PosixThread.h
index 7d8d349a..9c0ad39b 100644
--- a/osal/linux/PosixThread.h
+++ b/osal/linux/PosixThread.h
@@ -9,69 +9,71 @@
 
 class PosixThread {
 public:
-	static constexpr uint8_t PTHREAD_MAX_NAMELEN = 16;
-	PosixThread(const char* name_, int priority_, size_t stackSize_);
-	virtual ~PosixThread();
-	/**
-	 * Set the Thread to sleep state
-	 * @param ns Nanosecond sleep time
-	 * @return Returns Failed if sleep fails
-	 */
-	static ReturnValue_t sleep(uint64_t ns);
-	/**
-	 * @brief Function to suspend the task until SIGUSR1 was received
-	 *
-	 *  @details Will be called in the beginning to suspend execution until startTask() is called explicitly.
-	 */
-	void suspend();
+    static constexpr uint8_t PTHREAD_MAX_NAMELEN = 16;
+    PosixThread(const char* name_, int priority_, size_t stackSize_);
+    virtual ~PosixThread();
+    /**
+     * Set the Thread to sleep state
+     * @param ns Nanosecond sleep time
+     * @return Returns Failed if sleep fails
+     */
+    static ReturnValue_t sleep(uint64_t ns);
+    /**
+     * @brief Function to suspend the task until SIGUSR1 was received
+     *
+     *  @details Will be called in the beginning to suspend execution until startTask() is called explicitly.
+     */
+    void suspend();
 
-	/**
-	 * @brief Function to allow a other thread to start the thread again from suspend state
-	 *
-	 * @details Restarts the Thread after suspend call
-	 */
-	void resume();
+    /**
+     * @brief Function to allow a other thread to start the thread again from suspend state
+     *
+     * @details Restarts the Thread after suspend call
+     */
+    void resume();
 
 
-	/**
-	 * Delay function similar to FreeRtos delayUntil function
-	 *
-	 * @param prevoiusWakeTime_ms Needs the previous wake time and returns the next wakeup time
-	 * @param delayTime_ms Time period to delay
-	 *
-	 * @return False If deadline was missed; True if task was delayed
-	 */
-	static bool delayUntil(uint64_t* const prevoiusWakeTime_ms, const uint64_t delayTime_ms);
+    /**
+     * Delay function similar to FreeRtos delayUntil function
+     *
+     * @param prevoiusWakeTime_ms Needs the previous wake time and returns the next wakeup time
+     * @param delayTime_ms Time period to delay
+     *
+     * @return False If deadline was missed; True if task was delayed
+     */
+    static bool delayUntil(uint64_t* const prevoiusWakeTime_ms, const uint64_t delayTime_ms);
 
-	/**
-	 * Returns the current time in milliseconds from CLOCK_MONOTONIC
-	 *
-	 * @return current time in milliseconds from CLOCK_MONOTONIC
-	 */
-	static uint64_t getCurrentMonotonicTimeMs();
+    /**
+     * Returns the current time in milliseconds from CLOCK_MONOTONIC
+     *
+     * @return current time in milliseconds from CLOCK_MONOTONIC
+     */
+    static uint64_t getCurrentMonotonicTimeMs();
 
 protected:
-	pthread_t thread;
+    pthread_t thread;
 
-	/**
-	 * @brief 	Function that has to be called by derived class because the
-	 *  		derived class pointer has to be valid as argument.
-	 * @details
-	 * This function creates a pthread with the given parameters. As the
-	 * function requires a pointer to the derived object it has to be called
-	 * after the this pointer of the derived object is valid.
-	 * Sets the taskEntryPoint as function to be called by new a thread.
-	 * @param fnc_ Function which will be executed by the thread.
-	 * @param arg_
-	 * argument of the taskEntryPoint function, needs to be this pointer
-	 * of derived class
-	 */
-	void createTask(void* (*fnc_)(void*),void* arg_);
+    /**
+     * @brief 	Function that has to be called by derived class because the
+     *  		derived class pointer has to be valid as argument.
+     * @details
+     * This function creates a pthread with the given parameters. As the
+     * function requires a pointer to the derived object it has to be called
+     * after the this pointer of the derived object is valid.
+     * Sets the taskEntryPoint as function to be called by new a thread.
+     * @param fnc_ Function which will be executed by the thread.
+     * @param arg_
+     * argument of the taskEntryPoint function, needs to be this pointer
+     * of derived class
+     */
+    void createTask(void* (*fnc_)(void*),void* arg_);
 
 private:
-	char name[PTHREAD_MAX_NAMELEN];
-	int priority;
-	size_t stackSize = 0;
+    char name[PTHREAD_MAX_NAMELEN];
+    int priority;
+    size_t stackSize = 0;
+
+    static constexpr const char* CLASS_NAME = "PosixThread";
 };
 
 #endif /* FRAMEWORK_OSAL_LINUX_POSIXTHREAD_H_ */
diff --git a/osal/linux/unixUtility.cpp b/osal/linux/unixUtility.cpp
new file mode 100644
index 00000000..d7aab4ba
--- /dev/null
+++ b/osal/linux/unixUtility.cpp
@@ -0,0 +1,32 @@
+#include "FSFWConfig.h"
+#include "unixUtility.h"
+#include "../../serviceinterface/ServiceInterface.h"
+
+#include <cstring>
+#include <errno.h>
+
+void utility::printUnixErrorGeneric(const char* const className,
+        const char* const function, const char* const failString,
+        sif::OutputTypes outputType) {
+    if(className == nullptr or failString == nullptr or function == nullptr) {
+        return;
+    }
+#if FSFW_VERBOSE_LEVEL >= 1
+    if(outputType == sif::OutputTypes::OUT_ERROR) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << className << "::" << function << ":"  << failString << " error: "
+            << strerror(errno)  << std::endl;
+#else
+    sif::printError("%s::%s: %s error: %s\n", className, function, failString, strerror(errno));
+#endif
+    }
+    else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << className << "::" << function << ":"  << failString << " error: "
+            << strerror(errno)  << std::endl;
+#else
+    sif::printWarning("%s::%s: %s error: %s\n", className, function, failString, strerror(errno));
+#endif
+    }
+#endif
+}
diff --git a/osal/linux/unixUtility.h b/osal/linux/unixUtility.h
new file mode 100644
index 00000000..8a964f3a
--- /dev/null
+++ b/osal/linux/unixUtility.h
@@ -0,0 +1,13 @@
+#ifndef FSFW_OSAL_LINUX_UNIXUTILITY_H_
+#define FSFW_OSAL_LINUX_UNIXUTILITY_H_
+
+#include "../../serviceinterface/serviceInterfaceDefintions.h"
+
+namespace utility {
+
+void printUnixErrorGeneric(const char* const className, const char* const function,
+        const char* const failString, sif::OutputTypes outputType = sif::OutputTypes::OUT_ERROR);
+
+}
+
+#endif /* FSFW_OSAL_LINUX_UNIXUTILITY_H_ */

From 070c3f3bbfdbddbda54b18cdb5d1c9928640a0dd Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 4 Jun 2021 14:39:21 +0200
Subject: [PATCH 117/125] added example usage

---
 serviceinterface/ServiceInterfacePrinter.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/serviceinterface/ServiceInterfacePrinter.h b/serviceinterface/ServiceInterfacePrinter.h
index 6b062108..98421444 100644
--- a/serviceinterface/ServiceInterfacePrinter.h
+++ b/serviceinterface/ServiceInterfacePrinter.h
@@ -7,6 +7,8 @@
 
 //! https://stackoverflow.com/questions/111928/is-there-a-printf-converter-to-print-in-binary-format
 //! Can be used to print out binary numbers in human-readable format.
+//! Example usage:
+//! sif::printInfo("Status register: " BYTE_TO_BINARY_PATTERN "\n",BYTE_TO_BINARY(0x1f));
 #define BYTE_TO_BINARY_PATTERN "%c%c%c%c%c%c%c%c"
 #define BYTE_TO_BINARY(byte)  \
   (byte & 0x80 ? '1' : '0'), \

From 33e7c635c57055597caf3d9e9cbafc15c7364266 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 5 Jun 2021 00:05:03 +0200
Subject: [PATCH 118/125] added hal class ids

---
 returnvalues/FwClassIds.h | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h
index 17c37a79..60cb33ac 100644
--- a/returnvalues/FwClassIds.h
+++ b/returnvalues/FwClassIds.h
@@ -72,6 +72,9 @@ enum: uint8_t {
     PUS_SERVICE_3, //PUS3
     PUS_SERVICE_9, //PUS9
     FILE_SYSTEM, //FILS
+    HAL_SPI, //HSPI
+    HAL_UART, //HURT
+    HAL_I2C, //HI2C
     FW_CLASS_ID_COUNT // [EXPORT] : [END]
 
 };

From 57699cccb741dda67fa0af0983509f0d4e15234a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 5 Jun 2021 19:52:38 +0200
Subject: [PATCH 119/125] object manager is now a singleton

---
 action/ActionHelper.cpp                       |  4 +-
 action/ActionMessage.cpp                      |  4 +-
 action/CommandActionHelper.cpp                |  9 +-
 controller/ControllerBase.cpp                 |  3 +-
 datalinklayer/MapPacketExtraction.cpp         |  9 +-
 datapoollocal/HasLocalDataPoolIF.h            |  3 +-
 datapoollocal/LocalDataPoolManager.cpp        | 13 +--
 datapoollocal/LocalPoolDataSetBase.cpp        |  3 +-
 datapoollocal/LocalPoolObjectBase.cpp         |  4 +-
 devicehandlers/ChildHandlerBase.cpp           |  3 +-
 devicehandlers/DeviceHandlerBase.cpp          | 10 +-
 .../DeviceHandlerFailureIsolation.cpp         |  3 +-
 devicehandlers/DeviceHandlerMessage.cpp       |  4 +-
 events/EventManagerIF.h                       |  4 +-
 fdir/FailureIsolationBase.cpp                 |  8 +-
 health/HealthHelper.cpp                       |  6 +-
 housekeeping/HousekeepingMessage.cpp          |  4 +-
 memory/MemoryHelper.cpp                       |  6 +-
 memory/MemoryMessage.cpp                      |  4 +-
 monitoring/LimitViolationReporter.cpp         | 23 ++---
 monitoring/MonitoringMessage.cpp              |  4 +-
 monitoring/MonitoringMessageContent.h         |  4 +-
 monitoring/TriplexMonitor.h                   |  4 +-
 objectmanager/ObjectManager.cpp               | 37 +++++---
 objectmanager/ObjectManager.h                 | 92 ++++++++++++-------
 objectmanager/ObjectManagerIF.h               | 48 ++--------
 objectmanager/SystemObject.cpp                | 12 +--
 osal/FreeRTOS/FixedTimeslotTask.cpp           |  6 +-
 osal/FreeRTOS/MessageQueue.cpp                |  6 +-
 osal/FreeRTOS/PeriodicTask.cpp                |  5 +-
 osal/common/TcpTmTcServer.cpp                 |  6 +-
 osal/common/UdpTcPollingTask.cpp              |  7 +-
 osal/host/FixedTimeslotTask.cpp               |  4 +-
 osal/host/MessageQueue.cpp                    |  8 +-
 osal/host/PeriodicTask.cpp                    |  5 +-
 osal/linux/FixedTimeslotTask.cpp              |  6 +-
 osal/linux/MessageQueue.cpp                   |  5 +-
 osal/linux/PeriodicPosixTask.cpp              | 12 ++-
 osal/rtems/FixedTimeslotTask.cpp              |  4 +-
 osal/rtems/MessageQueue.cpp                   |  9 +-
 osal/rtems/PeriodicTask.cpp                   |  3 +-
 parameters/ParameterHelper.cpp                |  5 +-
 parameters/ParameterMessage.cpp               |  7 +-
 power/Fuse.cpp                                |  4 +-
 power/PowerSwitcher.cpp                       |  6 +-
 pus/CService200ModeCommanding.cpp             |  5 +-
 pus/CService201HealthCommanding.cpp           |  8 +-
 pus/Service1TelecommandVerification.cpp       |  3 +-
 pus/Service20ParameterManagement.cpp          | 12 +--
 pus/Service20ParameterManagement.h            |  2 +-
 pus/Service2DeviceAccess.cpp                  |  3 +-
 pus/Service3Housekeeping.cpp                  |  5 +-
 pus/Service5EventReporting.cpp                |  5 +-
 pus/Service8FunctionManagement.cpp            |  5 +-
 storagemanager/LocalPool.cpp                  |  7 +-
 subsystem/Subsystem.cpp                       |  7 +-
 subsystem/SubsystemBase.cpp                   | 13 +--
 subsystem/modes/ModeSequenceMessage.cpp       |  4 +-
 tcdistribution/CCSDSDistributor.cpp           |  3 +-
 tcdistribution/PUSDistributor.cpp             |  3 +-
 thermal/Heater.cpp                            |  5 +-
 tmstorage/TmStoreMessage.cpp                  |  4 +-
 tmtcpacket/pus/TcPacketStored.cpp             |  7 +-
 tmtcpacket/pus/TmPacketBase.cpp               |  4 +-
 tmtcpacket/pus/TmPacketStoredBase.cpp         |  8 +-
 tmtcservices/CommandingServiceBase.cpp        | 10 +-
 tmtcservices/PusServiceBase.cpp               |  7 +-
 tmtcservices/TmTcBridge.cpp                   |  7 +-
 tmtcservices/VerificationReporter.cpp         |  5 +-
 unittest/tests/datapoollocal/DataSetTest.cpp  |  3 +-
 .../datapoollocal/LocalPoolManagerTest.cpp    |  3 +-
 .../datapoollocal/LocalPoolVariableTest.cpp   |  3 +-
 .../datapoollocal/LocalPoolVectorTest.cpp     |  3 +-
 .../user/unittest/core/CatchDefinitions.cpp   |  6 +-
 unittest/user/unittest/core/CatchFactory.h    |  2 +-
 75 files changed, 321 insertions(+), 277 deletions(-)

diff --git a/action/ActionHelper.cpp b/action/ActionHelper.cpp
index b2374ed6..73007ea3 100644
--- a/action/ActionHelper.cpp
+++ b/action/ActionHelper.cpp
@@ -2,7 +2,7 @@
 #include "HasActionsIF.h"
 
 #include "../ipc/MessageQueueSenderIF.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../serviceinterface/ServiceInterface.h"
 
 ActionHelper::ActionHelper(HasActionsIF* setOwner,
@@ -25,7 +25,7 @@ ReturnValue_t ActionHelper::handleActionMessage(CommandMessage* command) {
 }
 
 ReturnValue_t ActionHelper::initialize(MessageQueueIF* queueToUse_) {
-    ipcStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+    ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
     if (ipcStore == nullptr) {
         return HasReturnvaluesIF::RETURN_FAILED;
     }
diff --git a/action/ActionMessage.cpp b/action/ActionMessage.cpp
index 66c7f058..f25858af 100644
--- a/action/ActionMessage.cpp
+++ b/action/ActionMessage.cpp
@@ -1,7 +1,7 @@
 #include "ActionMessage.h"
 #include "HasActionsIF.h"
 
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../storagemanager/StorageManagerIF.h"
 
 ActionMessage::ActionMessage() {
@@ -69,7 +69,7 @@ void ActionMessage::clear(CommandMessage* message) {
     switch(message->getCommand()) {
     case EXECUTE_ACTION:
     case DATA_REPLY: {
-        StorageManagerIF *ipcStore = objectManager->get<StorageManagerIF>(
+        StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
                 objects::IPC_STORE);
         if (ipcStore != NULL) {
             ipcStore->deleteData(getStoreId(message));
diff --git a/action/CommandActionHelper.cpp b/action/CommandActionHelper.cpp
index 148b3657..31650cae 100644
--- a/action/CommandActionHelper.cpp
+++ b/action/CommandActionHelper.cpp
@@ -2,7 +2,8 @@
 #include "CommandActionHelper.h"
 #include "CommandsActionsIF.h"
 #include "HasActionsIF.h"
-#include "../objectmanager/ObjectManagerIF.h"
+
+#include "../objectmanager/ObjectManager.h"
 
 CommandActionHelper::CommandActionHelper(CommandsActionsIF *setOwner) :
         owner(setOwner), queueToUse(NULL), ipcStore(
@@ -14,7 +15,7 @@ CommandActionHelper::~CommandActionHelper() {
 
 ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo,
         ActionId_t actionId, SerializeIF *data) {
-    HasActionsIF *receiver = objectManager->get<HasActionsIF>(commandTo);
+    HasActionsIF *receiver = ObjectManager::instance()->get<HasActionsIF>(commandTo);
     if (receiver == NULL) {
         return CommandsActionsIF::OBJECT_HAS_NO_FUNCTIONS;
     }
@@ -40,7 +41,7 @@ ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo,
 //    if (commandCount != 0) {
 //        return CommandsFunctionsIF::ALREADY_COMMANDING;
 //    }
-    HasActionsIF *receiver = objectManager->get<HasActionsIF>(commandTo);
+    HasActionsIF *receiver = ObjectManager::instance()->get<HasActionsIF>(commandTo);
     if (receiver == NULL) {
         return CommandsActionsIF::OBJECT_HAS_NO_FUNCTIONS;
     }
@@ -66,7 +67,7 @@ ReturnValue_t CommandActionHelper::sendCommand(MessageQueueId_t queueId,
 }
 
 ReturnValue_t CommandActionHelper::initialize() {
-    ipcStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+    ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
     if (ipcStore == NULL) {
         return HasReturnvaluesIF::RETURN_FAILED;
     }
diff --git a/controller/ControllerBase.cpp b/controller/ControllerBase.cpp
index 89f0ff68..5a94c082 100644
--- a/controller/ControllerBase.cpp
+++ b/controller/ControllerBase.cpp
@@ -3,6 +3,7 @@
 #include "../subsystem/SubsystemBase.h"
 #include "../ipc/QueueFactory.h"
 #include "../action/HasActionsIF.h"
+#include "../objectmanager/ObjectManager.h"
 
 ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
         size_t commandQueueDepth) :
@@ -25,7 +26,7 @@ ReturnValue_t ControllerBase::initialize() {
 
     MessageQueueId_t parentQueue = 0;
     if (parentId != objects::NO_OBJECT) {
-        SubsystemBase *parent = objectManager->get<SubsystemBase>(parentId);
+        SubsystemBase *parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
         if (parent == nullptr) {
             return RETURN_FAILED;
         }
diff --git a/datalinklayer/MapPacketExtraction.cpp b/datalinklayer/MapPacketExtraction.cpp
index cdc9ae27..d377ca34 100644
--- a/datalinklayer/MapPacketExtraction.cpp
+++ b/datalinklayer/MapPacketExtraction.cpp
@@ -1,10 +1,13 @@
 #include "MapPacketExtraction.h"
+
 #include "../ipc/QueueFactory.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../storagemanager/StorageManagerIF.h"
 #include "../tmtcpacket/SpacePacketBase.h"
 #include "../tmtcservices/AcceptsTelecommandsIF.h"
 #include "../tmtcservices/TmTcMessage.h"
+#include "../objectmanager/ObjectManager.h"
+
 #include <cstring>
 
 MapPacketExtraction::MapPacketExtraction(uint8_t setMapId,
@@ -131,9 +134,9 @@ void MapPacketExtraction::clearBuffers() {
 }
 
 ReturnValue_t MapPacketExtraction::initialize() {
-	packetStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
-	AcceptsTelecommandsIF* distributor = objectManager->get<
-			AcceptsTelecommandsIF>(packetDestination);
+	packetStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
+	AcceptsTelecommandsIF* distributor = ObjectManager::instance()->
+	        get<AcceptsTelecommandsIF>(packetDestination);
 	if ((packetStore != NULL) && (distributor != NULL)) {
 		tcQueueId = distributor->getRequestQueue();
 		return RETURN_OK;
diff --git a/datapoollocal/HasLocalDataPoolIF.h b/datapoollocal/HasLocalDataPoolIF.h
index 74e372c9..6051f068 100644
--- a/datapoollocal/HasLocalDataPoolIF.h
+++ b/datapoollocal/HasLocalDataPoolIF.h
@@ -34,7 +34,8 @@ class LocalPoolObjectBase;
  * can be retrieved using the object manager, provided the target object is a SystemObject.
  * For example, the following line of code can be used to retrieve the interface
  *
- *    HasLocalDataPoolIF* poolIF = objectManager->get<HasLocalDataPoolIF>(objects::SOME_OBJECT);
+ *    HasLocalDataPoolIF* poolIF = ObjectManager::instance()->
+ *             get<HasLocalDataPoolIF>(objects::SOME_OBJECT);
  *    if(poolIF != nullptr) {
  *        doSomething()
  *    }
diff --git a/datapoollocal/LocalDataPoolManager.cpp b/datapoollocal/LocalDataPoolManager.cpp
index dbe68ff1..1cbf8201 100644
--- a/datapoollocal/LocalDataPoolManager.cpp
+++ b/datapoollocal/LocalDataPoolManager.cpp
@@ -6,6 +6,7 @@
 #include "internal/HasLocalDpIFManagerAttorney.h"
 
 #include "../housekeeping/HousekeepingSetPacket.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../housekeeping/HousekeepingSnapshot.h"
 #include "../housekeeping/AcceptsHkPacketsIF.h"
 #include "../timemanager/CCSDSTime.h"
@@ -52,7 +53,7 @@ ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) {
 	}
 	hkQueue = queueToUse;
 
-	ipcStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+	ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
 	if(ipcStore == nullptr) {
 		/* Error, all destinations invalid */
 		printWarningOrError(sif::OutputTypes::OUT_ERROR,
@@ -63,8 +64,8 @@ ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) {
 
 
 	if(defaultHkDestination != objects::NO_OBJECT) {
-		AcceptsHkPacketsIF* hkPacketReceiver =
-				objectManager->get<AcceptsHkPacketsIF>(defaultHkDestination);
+		AcceptsHkPacketsIF* hkPacketReceiver = ObjectManager::instance()->
+		        get<AcceptsHkPacketsIF>(defaultHkDestination);
 		if(hkPacketReceiver != nullptr) {
 			hkDestinationId = hkPacketReceiver->getHkQueue();
 		}
@@ -360,8 +361,8 @@ void LocalDataPoolManager::resetHkUpdateResetHelper() {
 ReturnValue_t LocalDataPoolManager::subscribeForPeriodicPacket(sid_t sid,
 		bool enableReporting, float collectionInterval, bool isDiagnostics,
 		object_id_t packetDestination) {
-	AcceptsHkPacketsIF* hkReceiverObject =
-			objectManager->get<AcceptsHkPacketsIF>(packetDestination);
+	AcceptsHkPacketsIF* hkReceiverObject = ObjectManager::instance()->
+	        get<AcceptsHkPacketsIF>(packetDestination);
 	if(hkReceiverObject == nullptr) {
 		printWarningOrError(sif::OutputTypes::OUT_WARNING,
 				"subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
@@ -391,7 +392,7 @@ ReturnValue_t LocalDataPoolManager::subscribeForUpdatePacket(sid_t sid,
 		bool isDiagnostics, bool reportingEnabled,
 		object_id_t packetDestination) {
 	AcceptsHkPacketsIF* hkReceiverObject =
-			objectManager->get<AcceptsHkPacketsIF>(packetDestination);
+	        ObjectManager::instance()->get<AcceptsHkPacketsIF>(packetDestination);
 	if(hkReceiverObject == nullptr) {
 		printWarningOrError(sif::OutputTypes::OUT_WARNING,
 				"subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
diff --git a/datapoollocal/LocalPoolDataSetBase.cpp b/datapoollocal/LocalPoolDataSetBase.cpp
index a72e9db1..a7a7e6c8 100644
--- a/datapoollocal/LocalPoolDataSetBase.cpp
+++ b/datapoollocal/LocalPoolDataSetBase.cpp
@@ -3,6 +3,7 @@
 #include "internal/HasLocalDpIFUserAttorney.h"
 
 #include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../globalfunctions/bitutility.h"
 #include "../datapoollocal/LocalDataPoolManager.h"
 #include "../housekeeping/PeriodicHousekeepingHelper.h"
@@ -45,7 +46,7 @@ LocalPoolDataSetBase::LocalPoolDataSetBase(HasLocalDataPoolIF *hkOwner,
 LocalPoolDataSetBase::LocalPoolDataSetBase(sid_t sid, PoolVariableIF** registeredVariablesArray,
         const size_t maxNumberOfVariables):
         PoolDataSetBase(registeredVariablesArray, maxNumberOfVariables)  {
-    HasLocalDataPoolIF* hkOwner = objectManager->get<HasLocalDataPoolIF>(
+    HasLocalDataPoolIF* hkOwner = ObjectManager::instance()->get<HasLocalDataPoolIF>(
             sid.objectId);
     if(hkOwner != nullptr) {
         AccessPoolManagerIF* accessor = HasLocalDpIFUserAttorney::getAccessorHandle(hkOwner);
diff --git a/datapoollocal/LocalPoolObjectBase.cpp b/datapoollocal/LocalPoolObjectBase.cpp
index b6db0608..6920749b 100644
--- a/datapoollocal/LocalPoolObjectBase.cpp
+++ b/datapoollocal/LocalPoolObjectBase.cpp
@@ -4,7 +4,7 @@
 #include "HasLocalDataPoolIF.h"
 #include "internal/HasLocalDpIFUserAttorney.h"
 
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 
 
 LocalPoolObjectBase::LocalPoolObjectBase(lp_id_t poolId, HasLocalDataPoolIF* hkOwner,
@@ -43,7 +43,7 @@ LocalPoolObjectBase::LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId,
                 "which is the NO_PARAMETER value!\n");
 #endif
     }
-    HasLocalDataPoolIF* hkOwner = objectManager->get<HasLocalDataPoolIF>(poolOwner);
+    HasLocalDataPoolIF* hkOwner = ObjectManager::instance()->get<HasLocalDataPoolIF>(poolOwner);
     if(hkOwner == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "LocalPoolVariable: The supplied pool owner did not implement the correct "
diff --git a/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp
index d4ef67ad..628ea3e0 100644
--- a/devicehandlers/ChildHandlerBase.cpp
+++ b/devicehandlers/ChildHandlerBase.cpp
@@ -1,6 +1,5 @@
 #include "ChildHandlerBase.h"
 #include "../subsystem/SubsystemBase.h"
-#include "../subsystem/SubsystemBase.h"
 
 ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId,
 		object_id_t deviceCommunication, CookieIF * cookie,
@@ -30,7 +29,7 @@ ReturnValue_t ChildHandlerBase::initialize() {
 	MessageQueueId_t parentQueue = 0;
 
 	if (parentId != objects::NO_OBJECT) {
-		SubsystemBase *parent = objectManager->get<SubsystemBase>(parentId);
+		SubsystemBase *parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
 		if (parent == NULL) {
 			return RETURN_FAILED;
 		}
diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 1623a1ac..43660616 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -119,7 +119,7 @@ ReturnValue_t DeviceHandlerBase::initialize() {
         return result;
     }
 
-    communicationInterface = objectManager->get<DeviceCommunicationIF>(
+    communicationInterface = ObjectManager::instance()->get<DeviceCommunicationIF>(
             deviceCommunicationId);
     if (communicationInterface == nullptr) {
         printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
@@ -136,7 +136,7 @@ ReturnValue_t DeviceHandlerBase::initialize() {
         return result;
     }
 
-    IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+    IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
     if (IPCStore == nullptr) {
         printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
                 ObjectManagerIF::CHILD_INIT_FAILED, "IPC Store not set up");
@@ -144,8 +144,8 @@ ReturnValue_t DeviceHandlerBase::initialize() {
     }
 
     if(rawDataReceiverId != objects::NO_OBJECT) {
-        AcceptsDeviceResponsesIF *rawReceiver = objectManager->get<
-                AcceptsDeviceResponsesIF>(rawDataReceiverId);
+        AcceptsDeviceResponsesIF *rawReceiver = ObjectManager::instance()->
+                get<AcceptsDeviceResponsesIF>(rawDataReceiverId);
 
         if (rawReceiver == nullptr) {
             printWarningOrError(sif::OutputTypes::OUT_ERROR,
@@ -164,7 +164,7 @@ ReturnValue_t DeviceHandlerBase::initialize() {
     }
 
     if(powerSwitcherId != objects::NO_OBJECT) {
-        powerSwitcher = objectManager->get<PowerSwitchIF>(powerSwitcherId);
+        powerSwitcher = ObjectManager::instance()->get<PowerSwitchIF>(powerSwitcherId);
         if (powerSwitcher == nullptr) {
             printWarningOrError(sif::OutputTypes::OUT_ERROR,
                     "initialize", ObjectManagerIF::CHILD_INIT_FAILED,
diff --git a/devicehandlers/DeviceHandlerFailureIsolation.cpp b/devicehandlers/DeviceHandlerFailureIsolation.cpp
index ba118090..b0708a59 100644
--- a/devicehandlers/DeviceHandlerFailureIsolation.cpp
+++ b/devicehandlers/DeviceHandlerFailureIsolation.cpp
@@ -1,6 +1,7 @@
 #include "DeviceHandlerFailureIsolation.h"
 
 #include "../devicehandlers/DeviceHandlerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../modes/HasModesIF.h"
 #include "../health/HealthTableIF.h"
 #include "../power/Fuse.h"
@@ -175,7 +176,7 @@ ReturnValue_t DeviceHandlerFailureIsolation::initialize() {
 #endif
 		return result;
 	}
-	ConfirmsFailuresIF* power = objectManager->get<ConfirmsFailuresIF>(
+	ConfirmsFailuresIF* power = ObjectManager::instance()->get<ConfirmsFailuresIF>(
 			powerConfirmationId);
 	if (power != nullptr) {
 		powerConfirmation = power->getEventReceptionQueue();
diff --git a/devicehandlers/DeviceHandlerMessage.cpp b/devicehandlers/DeviceHandlerMessage.cpp
index cb9043db..69c9deb9 100644
--- a/devicehandlers/DeviceHandlerMessage.cpp
+++ b/devicehandlers/DeviceHandlerMessage.cpp
@@ -1,5 +1,5 @@
 #include "DeviceHandlerMessage.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 
 store_address_t DeviceHandlerMessage::getStoreAddress(
 		const CommandMessage* message) {
@@ -70,7 +70,7 @@ void DeviceHandlerMessage::clear(CommandMessage* message) {
 	case REPLY_RAW_COMMAND:
 	case REPLY_RAW_REPLY:
 	case REPLY_DIRECT_COMMAND_DATA: {
-		StorageManagerIF *ipcStore = objectManager->get<StorageManagerIF>(
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
 				objects::IPC_STORE);
 		if (ipcStore != nullptr) {
 			ipcStore->deleteData(getStoreAddress(message));
diff --git a/events/EventManagerIF.h b/events/EventManagerIF.h
index ea22f8ae..0ba126a2 100644
--- a/events/EventManagerIF.h
+++ b/events/EventManagerIF.h
@@ -3,7 +3,7 @@
 
 #include "EventMessage.h"
 #include "eventmatching/eventmatching.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../ipc/MessageQueueSenderIF.h"
 #include "../ipc/MessageQueueIF.h"
 #include "../serviceinterface/ServiceInterface.h"
@@ -43,7 +43,7 @@ public:
 	static void triggerEvent(EventMessage* message,
 	        MessageQueueId_t sentFrom = 0) {
 		if (eventmanagerQueue == MessageQueueIF::NO_QUEUE) {
-			EventManagerIF *eventmanager = objectManager->get<EventManagerIF>(
+			EventManagerIF *eventmanager = ObjectManager::instance()->get<EventManagerIF>(
 					objects::EVENT_MANAGER);
 			if (eventmanager == nullptr) {
 #if FSFW_VERBOSE_LEVEL >= 1
diff --git a/fdir/FailureIsolationBase.cpp b/fdir/FailureIsolationBase.cpp
index 69cb0f01..764fc918 100644
--- a/fdir/FailureIsolationBase.cpp
+++ b/fdir/FailureIsolationBase.cpp
@@ -3,7 +3,7 @@
 #include "../health/HasHealthIF.h"
 #include "../health/HealthMessage.h"
 #include "../ipc/QueueFactory.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 
 FailureIsolationBase::FailureIsolationBase(object_id_t owner,
 		object_id_t parent, uint8_t messageDepth, uint8_t parameterDomainBase) :
@@ -18,7 +18,7 @@ FailureIsolationBase::~FailureIsolationBase() {
 }
 
 ReturnValue_t FailureIsolationBase::initialize() {
-	EventManagerIF* manager = objectManager->get<EventManagerIF>(
+	EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
 			objects::EVENT_MANAGER);
 	if (manager == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -36,7 +36,7 @@ ReturnValue_t FailureIsolationBase::initialize() {
 		if (result != HasReturnvaluesIF::RETURN_OK) {
 			return result;
 		}
-		owner = objectManager->get<HasHealthIF>(ownerId);
+		owner = ObjectManager::instance()->get<HasHealthIF>(ownerId);
 		if (owner == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "FailureIsolationBase::intialize: Owner object "
@@ -46,7 +46,7 @@ ReturnValue_t FailureIsolationBase::initialize() {
 		}
 	}
 	if (faultTreeParent != objects::NO_OBJECT) {
-		ConfirmsFailuresIF* parentIF = objectManager->get<ConfirmsFailuresIF>(
+		ConfirmsFailuresIF* parentIF = ObjectManager::instance()->get<ConfirmsFailuresIF>(
 				faultTreeParent);
 		if (parentIF == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/health/HealthHelper.cpp b/health/HealthHelper.cpp
index 231d616e..28419108 100644
--- a/health/HealthHelper.cpp
+++ b/health/HealthHelper.cpp
@@ -1,5 +1,5 @@
 #include "HealthHelper.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 
 HealthHelper::HealthHelper(HasHealthIF* owner, object_id_t objectId) :
 		objectId(objectId), owner(owner) {
@@ -37,8 +37,8 @@ void HealthHelper::setParentQueue(MessageQueueId_t parentQueue) {
 }
 
 ReturnValue_t HealthHelper::initialize() {
-	healthTable = objectManager->get<HealthTableIF>(objects::HEALTH_TABLE);
-	eventSender = objectManager->get<EventReportingProxyIF>(objectId);
+	healthTable = ObjectManager::instance()->get<HealthTableIF>(objects::HEALTH_TABLE);
+	eventSender = ObjectManager::instance()->get<EventReportingProxyIF>(objectId);
 
 	if (healthTable == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/housekeeping/HousekeepingMessage.cpp b/housekeeping/HousekeepingMessage.cpp
index 90ca73c8..71f7ff17 100644
--- a/housekeeping/HousekeepingMessage.cpp
+++ b/housekeeping/HousekeepingMessage.cpp
@@ -1,6 +1,6 @@
 #include "HousekeepingMessage.h"
 
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include <cstring>
 
 HousekeepingMessage::~HousekeepingMessage() {}
@@ -161,7 +161,7 @@ void HousekeepingMessage::clear(CommandMessage* message) {
     case(UPDATE_SNAPSHOT_VARIABLE): {
         store_address_t storeId;
         getHkDataReply(message, &storeId);
-        StorageManagerIF *ipcStore = objectManager->get<StorageManagerIF>(
+        StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
                 objects::IPC_STORE);
         if (ipcStore != nullptr) {
             ipcStore->deleteData(storeId);
diff --git a/memory/MemoryHelper.cpp b/memory/MemoryHelper.cpp
index 42ac2654..d83a9fab 100644
--- a/memory/MemoryHelper.cpp
+++ b/memory/MemoryHelper.cpp
@@ -2,9 +2,9 @@
 #include "MemoryMessage.h"
 
 #include "../globalfunctions/CRC.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../serialize/EndianConverter.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 
 MemoryHelper::MemoryHelper(HasMemoryIF* workOnThis,
 		MessageQueueIF* useThisQueue):
@@ -187,7 +187,7 @@ ReturnValue_t MemoryHelper::initialize(MessageQueueIF* queueToUse_) {
 }
 
 ReturnValue_t MemoryHelper::initialize() {
-	ipcStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+	ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
 	if (ipcStore != nullptr) {
 		return RETURN_OK;
 	} else {
diff --git a/memory/MemoryMessage.cpp b/memory/MemoryMessage.cpp
index 94fa4691..1f050ef8 100644
--- a/memory/MemoryMessage.cpp
+++ b/memory/MemoryMessage.cpp
@@ -1,6 +1,6 @@
 #include "MemoryMessage.h"
 
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 
 uint32_t MemoryMessage::getAddress(const CommandMessage* message) {
 	return message->getParameter();
@@ -44,7 +44,7 @@ void MemoryMessage::clear(CommandMessage* message) {
 	switch (message->getCommand()) {
 	case CMD_MEMORY_LOAD:
 	case REPLY_MEMORY_DUMP: {
-		StorageManagerIF *ipcStore = objectManager->get<StorageManagerIF>(
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
 				objects::IPC_STORE);
 		if (ipcStore != NULL) {
 			ipcStore->deleteData(getStoreID(message));
diff --git a/monitoring/LimitViolationReporter.cpp b/monitoring/LimitViolationReporter.cpp
index c531a6e6..2de1e008 100644
--- a/monitoring/LimitViolationReporter.cpp
+++ b/monitoring/LimitViolationReporter.cpp
@@ -1,13 +1,8 @@
-/**
- * @file	LimitViolationReporter.cpp
- * @brief	This file defines the LimitViolationReporter class.
- * @date	17.07.2014
- * @author	baetz
- */
 #include "LimitViolationReporter.h"
 #include "MonitoringIF.h"
 #include "ReceivesMonitoringReportsIF.h"
-#include "../objectmanager/ObjectManagerIF.h"
+
+#include "../objectmanager/ObjectManager.h"
 #include "../serialize/SerializeAdapter.h"
 
 ReturnValue_t LimitViolationReporter::sendLimitViolationReport(const SerializeIF* data) {
@@ -16,7 +11,7 @@ ReturnValue_t LimitViolationReporter::sendLimitViolationReport(const SerializeIF
 		return result;
 	}
 	store_address_t storeId;
-	uint8_t* dataTarget = NULL;
+	uint8_t* dataTarget = nullptr;
 	size_t maxSize = data->getSerializedSize();
 	if (maxSize > MonitoringIF::VIOLATION_REPORT_MAX_SIZE) {
 		return MonitoringIF::INVALID_SIZE;
@@ -38,16 +33,16 @@ ReturnValue_t LimitViolationReporter::sendLimitViolationReport(const SerializeIF
 
 ReturnValue_t LimitViolationReporter::checkClassLoaded() {
 	if (reportQueue == 0) {
-		ReceivesMonitoringReportsIF* receiver = objectManager->get<
+		ReceivesMonitoringReportsIF* receiver = ObjectManager::instance()->get<
 				ReceivesMonitoringReportsIF>(reportingTarget);
-		if (receiver == NULL) {
+		if (receiver == nullptr) {
 			return ObjectManagerIF::NOT_FOUND;
 		}
 		reportQueue = receiver->getCommandQueue();
 	}
-	if (ipcStore == NULL) {
-		ipcStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
-		if (ipcStore == NULL) {
+	if (ipcStore == nullptr) {
+		ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+		if (ipcStore == nullptr) {
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
 	}
@@ -56,5 +51,5 @@ ReturnValue_t LimitViolationReporter::checkClassLoaded() {
 
 //Lazy initialization.
 MessageQueueId_t LimitViolationReporter::reportQueue = 0;
-StorageManagerIF* LimitViolationReporter::ipcStore = NULL;
+StorageManagerIF* LimitViolationReporter::ipcStore = nullptr;
 object_id_t LimitViolationReporter::reportingTarget = 0;
diff --git a/monitoring/MonitoringMessage.cpp b/monitoring/MonitoringMessage.cpp
index 8caa27ae..6e5f49cc 100644
--- a/monitoring/MonitoringMessage.cpp
+++ b/monitoring/MonitoringMessage.cpp
@@ -1,5 +1,5 @@
 #include "MonitoringMessage.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 
 MonitoringMessage::~MonitoringMessage() {
 }
@@ -25,7 +25,7 @@ void MonitoringMessage::clear(CommandMessage* message) {
 	message->setCommand(CommandMessage::CMD_NONE);
 	switch (message->getCommand()) {
 	case MonitoringMessage::LIMIT_VIOLATION_REPORT: {
-		StorageManagerIF *ipcStore = objectManager->get<StorageManagerIF>(
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
 				objects::IPC_STORE);
 		if (ipcStore != NULL) {
 			ipcStore->deleteData(getStoreId(message));
diff --git a/monitoring/MonitoringMessageContent.h b/monitoring/MonitoringMessageContent.h
index 0314d7ed..1d5f9c92 100644
--- a/monitoring/MonitoringMessageContent.h
+++ b/monitoring/MonitoringMessageContent.h
@@ -5,7 +5,7 @@
 #include "MonitoringIF.h"
 
 #include "../datapoollocal/localPoolDefinitions.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../serialize/SerialBufferAdapter.h"
 #include "../serialize/SerialFixedArrayListAdapter.h"
 #include "../serialize/SerializeElement.h"
@@ -71,7 +71,7 @@ private:
 	}
 	bool checkAndSetStamper() {
 		if (timeStamper == nullptr) {
-			timeStamper = objectManager->get<TimeStamperIF>( timeStamperId );
+			timeStamper = ObjectManager::instance()->get<TimeStamperIF>( timeStamperId );
 			if ( timeStamper == nullptr ) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 				sif::error << "MonitoringReportContent::checkAndSetStamper: "
diff --git a/monitoring/TriplexMonitor.h b/monitoring/TriplexMonitor.h
index d9ee8305..295a6174 100644
--- a/monitoring/TriplexMonitor.h
+++ b/monitoring/TriplexMonitor.h
@@ -5,7 +5,7 @@
 #include "../datapool/PIDReaderList.h"
 #include "../health/HealthTableIF.h"
 #include "../parameters/HasParametersIF.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 
 
 //SHOULDDO: This is by far not perfect. Could be merged with new Monitor classes. But still, it's over-engineering.
@@ -64,7 +64,7 @@ public:
 		return result;
 	}
 	ReturnValue_t initialize() {
-		healthTable = objectManager->get<HealthTableIF>(objects::HEALTH_TABLE);
+		healthTable = ObjectManager::instance()->get<HealthTableIF>(objects::HEALTH_TABLE);
 		if (healthTable == NULL) {
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
diff --git a/objectmanager/ObjectManager.cpp b/objectmanager/ObjectManager.cpp
index 3c2be532..a0cc3423 100644
--- a/objectmanager/ObjectManager.cpp
+++ b/objectmanager/ObjectManager.cpp
@@ -6,11 +6,23 @@
 #endif
 #include <cstdlib>
 
-ObjectManager::ObjectManager( void (*setProducer)() ):
-		produceObjects(setProducer) {
-	//There's nothing special to do in the constructor.
+ObjectManager* ObjectManager::objManagerInstance = nullptr;
+
+ObjectManager* ObjectManager::instance() {
+    if(objManagerInstance == nullptr) {
+        objManagerInstance = new ObjectManager();
+    }
+    return objManagerInstance;
 }
 
+void ObjectManager::setObjectFactoryFunction(produce_function_t objFactoryFunc, void *factoryArgs) {
+    this->objectFactoryFunction = objFactoryFunc;
+    this->factoryArgs = factoryArgs;
+}
+
+
+ObjectManager::ObjectManager() {}
+
 
 ObjectManager::~ObjectManager() {
 	for (auto const& iter : objectList) {
@@ -28,10 +40,13 @@ ReturnValue_t ObjectManager::insert( object_id_t id, SystemObjectIF* object) {
 		return this->RETURN_OK;
 	} else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "ObjectManager::insert: Object id " << std::hex
-		           << static_cast<uint32_t>(id) << std::dec
-				   << " is already in use!" << std::endl;
-		sif::error << "Terminating program." << std::endl;
+		sif::error << "ObjectManager::insert: Object ID " << std::hex <<
+		        static_cast<uint32_t>(id) << std::dec << " is already in use!" << std::endl;
+		sif::error << "Terminating program" << std::endl;
+#else
+		sif::printError("ObjectManager::insert: Object ID 0x%08x is already in use!\n",
+		        static_cast<unsigned int>(id));
+		sif::printError("Terminating program");
 #endif
 		//This is very severe and difficult to handle in other places.
 		std::exit(INSERTION_FAILED);
@@ -66,12 +81,8 @@ SystemObjectIF* ObjectManager::getSystemObject( object_id_t id ) {
 	}
 }
 
-ObjectManager::ObjectManager() : produceObjects(nullptr) {
-
-}
-
 void ObjectManager::initialize() {
-	if(produceObjects == nullptr) {
+	if(objectFactoryFunction == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "ObjectManager::initialize: Passed produceObjects "
 				"functions is nullptr!" << std::endl;
@@ -80,7 +91,7 @@ void ObjectManager::initialize() {
 #endif
 		return;
 	}
-	this->produceObjects();
+	objectFactoryFunction(factoryArgs);
 	ReturnValue_t result = RETURN_FAILED;
 	uint32_t errorCount = 0;
 	for (auto const& it : objectList) {
diff --git a/objectmanager/ObjectManager.h b/objectmanager/ObjectManager.h
index 69a74f73..1d4c3185 100644
--- a/objectmanager/ObjectManager.h
+++ b/objectmanager/ObjectManager.h
@@ -5,6 +5,7 @@
 #include "SystemObjectIF.h"
 #include <map>
 
+
 /**
  * @brief	This class implements a global object manager.
  * @details	This manager handles a list of available objects with system-wide
@@ -19,44 +20,65 @@
  * @author	Bastian Baetz
  */
 class ObjectManager : public ObjectManagerIF {
-private:
-	//comparison?
-	/**
-	 * @brief	This is the map of all initialized objects in the manager.
-	 * @details	Objects in the List must inherit the SystemObjectIF.
-	 */
-	std::map<object_id_t, SystemObjectIF*> objectList;
-protected:
-	SystemObjectIF* getSystemObject( object_id_t id );
-	/**
-	 * @brief	This attribute is initialized with the factory function
-	 * 			that creates new objects.
-	 * @details	The function is called if an object was requested with
-	 * 			getSystemObject, but not found in objectList.
-	 * @param 	The id of the object to be created.
-	 * @return	Returns a pointer to the newly created object or NULL.
-	 */
-	void (*produceObjects)();
 public:
-	/**
-	 * @brief	Apart from setting the producer function, nothing special
-	 * 			happens in the constructor.
-	 * @param setProducer	A pointer to a factory function.
-	 */
-	ObjectManager( void (*produce)() );
-	ObjectManager();
-	/**
-	 *	@brief	In the class's destructor, all objects in the list are deleted.
-	 */
-	// SHOULDDO: If, for some reason, deleting an ObjectManager instance is
-	// required, check if this works.
-	virtual ~ObjectManager( void );
-	ReturnValue_t insert( object_id_t id, SystemObjectIF* object );
-	ReturnValue_t remove( object_id_t id );
-	void initialize();
-	void printList();
+
+    using produce_function_t = void (*) (void* args);
+
+    /**
+     * Returns the single instance of TaskFactory.
+     * The implementation of #instance is found in its subclasses.
+     * Thus, we choose link-time variability of the  instance.
+     */
+    static ObjectManager* instance();
+
+    void setObjectFactoryFunction(produce_function_t prodFunc, void* args);
+
+    template <typename T> T* get( object_id_t id );
+
+    /**
+     *  @brief  In the class's destructor, all objects in the list are deleted.
+     */
+    virtual ~ObjectManager();
+    ReturnValue_t insert(object_id_t id, SystemObjectIF* object) override;
+    ReturnValue_t remove(object_id_t id) override;
+    void initialize() override;
+    void printList() override;
+
+protected:
+    SystemObjectIF* getSystemObject(object_id_t id) override;
+    /**
+     * @brief   This attribute is initialized with the factory function
+     *          that creates new objects.
+     * @details The function is called if an object was requested with
+     *          getSystemObject, but not found in objectList.
+     * @param   The id of the object to be created.
+     * @return  Returns a pointer to the newly created object or NULL.
+     */
+    produce_function_t objectFactoryFunction = nullptr;
+    void* factoryArgs = nullptr;
+
+private:
+    ObjectManager();
+
+    /**
+     * @brief	This is the map of all initialized objects in the manager.
+     * @details	Objects in the List must inherit the SystemObjectIF.
+     */
+    std::map<object_id_t, SystemObjectIF*> objectList;
+    static ObjectManager* objManagerInstance;
 };
 
+/**
+ * @brief   This is the forward declaration of the global objectManager instance.
+ */
+// SHOULDDO: maybe put this in the glob namespace to explicitely mark it global?
+//extern ObjectManagerIF *objectManager;
 
+/*Documentation can be found in the class method declaration above.*/
+template <typename T>
+T* ObjectManager::get( object_id_t id ) {
+    SystemObjectIF* temp = this->getSystemObject(id);
+    return dynamic_cast<T*>(temp);
+}
 
 #endif /* FSFW_OBJECTMANAGER_OBJECTMANAGER_H_ */
diff --git a/objectmanager/ObjectManagerIF.h b/objectmanager/ObjectManagerIF.h
index 61e6f423..561ff352 100644
--- a/objectmanager/ObjectManagerIF.h
+++ b/objectmanager/ObjectManagerIF.h
@@ -8,11 +8,11 @@
 
 /**
  * @brief	This class provides an interface to the global object manager.
- * @details	This manager handles a list of available objects with system-wide
- * 			relevance, such as device handlers, and TM/TC services. They can be
- * 			inserted, removed and retrieved from the list. On getting the
- * 			object, the call checks if the object implements the requested
- * 			interface.
+ * @details
+ * This manager handles a list of available objects with system-wide relevance, such as device
+ * handlers, and TM/TC services. They can be inserted, removed and retrieved from the list.
+ * On getting the object, the call checks if the object implements the requested interface.
+ * This interface does not specify a getter function because templates can't be used in interfaces.
  * @author	Bastian Baetz
  * @ingroup system_objects
  */
@@ -21,7 +21,8 @@ public:
 	static constexpr uint8_t INTERFACE_ID = CLASS_ID::OBJECT_MANAGER_IF;
 	static constexpr ReturnValue_t INSERTION_FAILED = MAKE_RETURN_CODE( 1 );
 	static constexpr ReturnValue_t NOT_FOUND = MAKE_RETURN_CODE( 2 );
-	static constexpr ReturnValue_t CHILD_INIT_FAILED = MAKE_RETURN_CODE( 3 ); //!< Can be used if the initialization of a SystemObject failed.
+	//!< Can be used if the initialization of a SystemObject failed.
+	static constexpr ReturnValue_t CHILD_INIT_FAILED = MAKE_RETURN_CODE( 3 );
 	static constexpr ReturnValue_t INTERNAL_ERR_REPORTER_UNINIT = MAKE_RETURN_CODE( 4 );
 
 protected:
@@ -49,22 +50,11 @@ public:
 	 * 			@li RETURN_OK in case the object was successfully inserted
 	 */
 	virtual ReturnValue_t insert( object_id_t id, SystemObjectIF* object ) = 0;
-	/**
-	 * @brief	With the get call, interfaces of an object can be retrieved in
-	 * 			a type-safe manner.
-	 * @details With the template-based call, the object list is searched with the
-	 * 			getSystemObject method and afterwards it is checked, if the object
-	 * 			implements the requested interface (with a dynamic_cast).
-	 * @param id	The object id of the requested object.
-	 * @return	The method returns a pointer to an object implementing the
-	 * 			requested interface, or NULL.
-	 */
-	template <typename T> T* get( object_id_t id );
 	/**
 	 * @brief	With this call, an object is removed from the list.
 	 * @param id	The object id of the object to be removed.
-	 * @return	\li NOT_FOUND in case the object was not found
-	 * 			\li RETURN_OK in case the object was successfully removed
+	 * @return	@li NOT_FOUND in case the object was not found
+	 * 			@li RETURN_OK in case the object was successfully removed
 	 */
 	virtual ReturnValue_t remove( object_id_t id ) = 0;
 	virtual void initialize() = 0;
@@ -75,24 +65,4 @@ public:
 	virtual void printList() = 0;
 };
 
-
-/**
- * @brief	This is the forward declaration of the global objectManager instance.
- */
-// SHOULDDO: maybe put this in the glob namespace to explicitely mark it global?
-extern ObjectManagerIF *objectManager;
-
-/*Documentation can be found in the class method declaration above.*/
-template <typename T>
-T* ObjectManagerIF::get( object_id_t id ) {
-	if(objectManager == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "ObjectManagerIF: Global object manager has not "
-				"been initialized yet!" << std::endl;
-#endif
-	}
-	SystemObjectIF* temp = this->getSystemObject(id);
-	return dynamic_cast<T*>(temp);
-}
-
 #endif /* OBJECTMANAGERIF_H_ */
diff --git a/objectmanager/SystemObject.cpp b/objectmanager/SystemObject.cpp
index 9040002c..123bbe65 100644
--- a/objectmanager/SystemObject.cpp
+++ b/objectmanager/SystemObject.cpp
@@ -4,18 +4,14 @@
 
 SystemObject::SystemObject(object_id_t setObjectId, bool doRegister) :
 		objectId(setObjectId), registered(doRegister) {
-	if (registered) {
-	    if(objectManager != nullptr) {
-	        objectManager->insert(objectId, this);
-	    }
-	}
+    if (registered) {
+        ObjectManager::instance()->insert(objectId, this);
+    }
 }
 
 SystemObject::~SystemObject() {
 	if (registered) {
-	    if(objectManager != nullptr) {
-	        objectManager->remove(objectId);
-	    }
+	    ObjectManager::instance()->remove(objectId);
 	}
 }
 
diff --git a/osal/FreeRTOS/FixedTimeslotTask.cpp b/osal/FreeRTOS/FixedTimeslotTask.cpp
index aa7e6c59..a722c958 100644
--- a/osal/FreeRTOS/FixedTimeslotTask.cpp
+++ b/osal/FreeRTOS/FixedTimeslotTask.cpp
@@ -1,6 +1,7 @@
 #include "FixedTimeslotTask.h"
 
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
 
 uint32_t FixedTimeslotTask::deadlineMissedCount = 0;
 const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = configMINIMAL_STACK_SIZE;
@@ -66,8 +67,7 @@ ReturnValue_t FixedTimeslotTask::startTask() {
 
 ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
 		uint32_t slotTimeMs, int8_t executionStep) {
-    ExecutableObjectIF* handler =
-            objectManager->get<ExecutableObjectIF>(componentId);
+    ExecutableObjectIF* handler = ObjectManager::instance()->get<ExecutableObjectIF>(componentId);
 	if (handler != nullptr) {
 		pst.addSlot(componentId, slotTimeMs, executionStep, handler, this);
 		return HasReturnvaluesIF::RETURN_OK;
diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index b5c9035d..d8929923 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -1,7 +1,7 @@
 #include "MessageQueue.h"
 #include "QueueMapManager.h"
-#include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
 
 MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
 		maxMessageSize(maxMessageSize) {
@@ -66,7 +66,7 @@ QueueHandle_t MessageQueue::getNativeQueueHandle() {
 ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) {
 	if (result != pdPASS) {
 		if (not ignoreFault) {
-			InternalErrorReporterIF* internalErrorReporter = objectManager->
+			InternalErrorReporterIF* internalErrorReporter = ObjectManager::instance()->
 			        get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
 			if (internalErrorReporter != nullptr) {
 				internalErrorReporter->queueMessageNotSent();
diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp
index 3e830c7f..42d6681d 100644
--- a/osal/FreeRTOS/PeriodicTask.cpp
+++ b/osal/FreeRTOS/PeriodicTask.cpp
@@ -1,6 +1,7 @@
 #include "PeriodicTask.h"
 
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
 PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority,
@@ -100,7 +101,7 @@ void PeriodicTask::taskFunctionality() {
 }
 
 ReturnValue_t PeriodicTask::addComponent(object_id_t object) {
-	ExecutableObjectIF* newObject = objectManager->get<ExecutableObjectIF>(
+	ExecutableObjectIF* newObject = ObjectManager::instance()->get<ExecutableObjectIF>(
 			object);
 	if (newObject == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/osal/common/TcpTmTcServer.cpp b/osal/common/TcpTmTcServer.cpp
index 28fab422..38f72647 100644
--- a/osal/common/TcpTmTcServer.cpp
+++ b/osal/common/TcpTmTcServer.cpp
@@ -6,7 +6,7 @@
 #include "../../container/SharedRingBuffer.h"
 #include "../../ipc/MessageQueueSenderIF.h"
 #include "../../ipc/MutexGuard.h"
-#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../objectmanager/ObjectManager.h"
 
 #include "../../serviceinterface/ServiceInterface.h"
 #include "../../tmtcservices/TmTcMessage.h"
@@ -41,7 +41,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
         return result;
     }
 
-    tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
+    tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
     if (tcStore == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "TcpTmTcServer::initialize: TC store uninitialized!" << std::endl;
@@ -51,7 +51,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
         return ObjectManagerIF::CHILD_INIT_FAILED;
     }
 
-    tmtcBridge = objectManager->get<TcpTmTcBridge>(tmtcBridgeId);
+    tmtcBridge = ObjectManager::instance()->get<TcpTmTcBridge>(tmtcBridgeId);
 
     int retval = 0;
     struct addrinfo *addrResult = nullptr;
diff --git a/osal/common/UdpTcPollingTask.cpp b/osal/common/UdpTcPollingTask.cpp
index 877e7883..4453e1bc 100644
--- a/osal/common/UdpTcPollingTask.cpp
+++ b/osal/common/UdpTcPollingTask.cpp
@@ -2,7 +2,8 @@
 #include "tcpipHelpers.h"
 #include "../../platform.h"
 #include "../../globalfunctions/arrayprinter.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../objectmanager/ObjectManager.h"
 
 #ifdef PLATFORM_WIN
 #include <winsock2.h>
@@ -116,7 +117,7 @@ ReturnValue_t UdpTcPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
 }
 
 ReturnValue_t UdpTcPollingTask::initialize() {
-    tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
+    tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
     if (tcStore == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "UdpTcPollingTask::initialize: TC store uninitialized!" << std::endl;
@@ -124,7 +125,7 @@ ReturnValue_t UdpTcPollingTask::initialize() {
         return ObjectManagerIF::CHILD_INIT_FAILED;
     }
 
-    tmtcBridge = objectManager->get<UdpTmTcBridge>(tmtcBridgeId);
+    tmtcBridge = ObjectManager::instance()->get<UdpTmTcBridge>(tmtcBridgeId);
     if(tmtcBridge == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "UdpTcPollingTask::initialize: Invalid TMTC bridge object!" <<
diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp
index 55f37499..3ad191e5 100644
--- a/osal/host/FixedTimeslotTask.cpp
+++ b/osal/host/FixedTimeslotTask.cpp
@@ -1,9 +1,11 @@
 #include "taskHelpers.h"
+
 #include "../../platform.h"
 #include "../../osal/host/FixedTimeslotTask.h"
 #include "../../ipc/MutexFactory.h"
 #include "../../osal/host/Mutex.h"
 #include "../../osal/host/FixedTimeslotTask.h"
+#include "../../objectmanager/ObjectManager.h"
 #include "../../serviceinterface/ServiceInterface.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
@@ -110,7 +112,7 @@ void FixedTimeslotTask::taskFunctionality() {
 
 ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
         uint32_t slotTimeMs, int8_t executionStep) {
-    ExecutableObjectIF* executableObject = objectManager->
+    ExecutableObjectIF* executableObject = ObjectManager::instance()->
             get<ExecutableObjectIF>(componentId);
     if (executableObject != nullptr) {
         pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep,
diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index a779bdcb..0c16d808 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -1,7 +1,8 @@
 #include "MessageQueue.h"
 #include "QueueMapManager.h"
 
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../objectmanager/ObjectManager.h"
 #include "../../ipc/MutexFactory.h"
 #include "../../ipc/MutexGuard.h"
 
@@ -121,9 +122,8 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 			QueueMapManager::instance()->getMessageQueue(sendTo));
 	if(targetQueue == nullptr) {
 		if(not ignoreFault) {
-			InternalErrorReporterIF* internalErrorReporter =
-					objectManager->get<InternalErrorReporterIF>(
-							objects::INTERNAL_ERROR_REPORTER);
+			InternalErrorReporterIF* internalErrorReporter = ObjectManager::instance()->
+			        get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
 			if (internalErrorReporter != nullptr) {
 				internalErrorReporter->queueMessageNotSent();
 			}
diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp
index 4b3fa626..180272d0 100644
--- a/osal/host/PeriodicTask.cpp
+++ b/osal/host/PeriodicTask.cpp
@@ -4,7 +4,8 @@
 
 #include "../../platform.h"
 #include "../../ipc/MutexFactory.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
 #include <thread>
@@ -103,7 +104,7 @@ void PeriodicTask::taskFunctionality() {
 }
 
 ReturnValue_t PeriodicTask::addComponent(object_id_t object) {
-	ExecutableObjectIF* newObject = objectManager->get<ExecutableObjectIF>(
+	ExecutableObjectIF* newObject = ObjectManager::instance()->get<ExecutableObjectIF>(
 			object);
 	if (newObject == nullptr) {
 		return HasReturnvaluesIF::RETURN_FAILED;
diff --git a/osal/linux/FixedTimeslotTask.cpp b/osal/linux/FixedTimeslotTask.cpp
index a545eeb7..c60c287a 100644
--- a/osal/linux/FixedTimeslotTask.cpp
+++ b/osal/linux/FixedTimeslotTask.cpp
@@ -1,5 +1,7 @@
 #include "FixedTimeslotTask.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
 
 #include <limits.h>
 
@@ -40,7 +42,7 @@ uint32_t FixedTimeslotTask::getPeriodMs() const {
 ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
 		uint32_t slotTimeMs, int8_t executionStep) {
 	ExecutableObjectIF* executableObject =
-			objectManager->get<ExecutableObjectIF>(componentId);
+	        ObjectManager::instance()->get<ExecutableObjectIF>(componentId);
 	if (executableObject != nullptr) {
 		pst.addSlot(componentId, slotTimeMs, executionStep,
 				executableObject,this);
diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index 12774a58..0abb7a35 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -1,6 +1,7 @@
 #include "MessageQueue.h"
+
 #include "../../serviceinterface/ServiceInterface.h"
-#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../objectmanager/ObjectManager.h"
 
 #include <fstream>
 
@@ -351,7 +352,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 	if (result != 0) {
 		if(!ignoreFault){
 			InternalErrorReporterIF* internalErrorReporter =
-					objectManager->get<InternalErrorReporterIF>(
+			        ObjectManager::instance()->get<InternalErrorReporterIF>(
 						objects::INTERNAL_ERROR_REPORTER);
 			if (internalErrorReporter != NULL) {
 				internalErrorReporter->queueMessageNotSent();
diff --git a/osal/linux/PeriodicPosixTask.cpp b/osal/linux/PeriodicPosixTask.cpp
index 630dd8e0..956d4fdf 100644
--- a/osal/linux/PeriodicPosixTask.cpp
+++ b/osal/linux/PeriodicPosixTask.cpp
@@ -1,8 +1,12 @@
-#include "../../tasks/ExecutableObjectIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
-#include <errno.h>
 #include "PeriodicPosixTask.h"
 
+#include "../../objectmanager/ObjectManager.h"
+#include "../../tasks/ExecutableObjectIF.h"
+#include "../../serviceinterface/ServiceInterface.h"
+
+#include <errno.h>
+
+
 PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_,
 		size_t stackSize_, uint32_t period_, void(deadlineMissedFunc_)()):
 		PosixThread(name_, priority_, stackSize_), objectList(), started(false),
@@ -22,7 +26,7 @@ void* PeriodicPosixTask::taskEntryPoint(void* arg) {
 }
 
 ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) {
-	ExecutableObjectIF* newObject = objectManager->get<ExecutableObjectIF>(
+	ExecutableObjectIF* newObject = ObjectManager::instance()->get<ExecutableObjectIF>(
 			object);
 	if (newObject == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/osal/rtems/FixedTimeslotTask.cpp b/osal/rtems/FixedTimeslotTask.cpp
index 3a3be6b3..19960a4c 100644
--- a/osal/rtems/FixedTimeslotTask.cpp
+++ b/osal/rtems/FixedTimeslotTask.cpp
@@ -3,7 +3,7 @@
 
 #include "../../tasks/FixedSequenceSlot.h"
 #include "../../objectmanager/SystemObjectIF.h"
-#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../objectmanager/ObjectManager.h"
 #include "../../returnvalues/HasReturnvaluesIF.h"
 #include "../../serviceinterface/ServiceInterface.h"
 
@@ -81,7 +81,7 @@ ReturnValue_t FixedTimeslotTask::startTask() {
 
 ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
         uint32_t slotTimeMs, int8_t executionStep) {
-    ExecutableObjectIF* object = objectManager->get<ExecutableObjectIF>(componentId);
+    ExecutableObjectIF* object = ObjectManager::instance()->get<ExecutableObjectIF>(componentId);
     if (object != nullptr) {
         pst.addSlot(componentId, slotTimeMs, executionStep, object, this);
         return HasReturnvaluesIF::RETURN_OK;
diff --git a/osal/rtems/MessageQueue.cpp b/osal/rtems/MessageQueue.cpp
index 717b80dd..e8128e90 100644
--- a/osal/rtems/MessageQueue.cpp
+++ b/osal/rtems/MessageQueue.cpp
@@ -1,8 +1,11 @@
-#include "../../serviceinterface/ServiceInterfaceStream.h"
-#include "../../objectmanager/ObjectManagerIF.h"
 #include "MessageQueue.h"
 #include "RtemsBasic.h"
+
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../objectmanager/ObjectManager.h"
+
 #include <cstring>
+
 MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
 		id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(nullptr) {
 	rtems_name name = ('Q' << 24) + (queueCounter++ << 8);
@@ -94,7 +97,7 @@ ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
 	//TODO: Check if we're in ISR.
 	if (result != RTEMS_SUCCESSFUL && !ignoreFault) {
 		if (internalErrorReporter == nullptr) {
-			internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
+			internalErrorReporter = ObjectManager::instance()->get<InternalErrorReporterIF>(
 					objects::INTERNAL_ERROR_REPORTER);
 		}
 		if (internalErrorReporter != nullptr) {
diff --git a/osal/rtems/PeriodicTask.cpp b/osal/rtems/PeriodicTask.cpp
index 067983cb..58717344 100644
--- a/osal/rtems/PeriodicTask.cpp
+++ b/osal/rtems/PeriodicTask.cpp
@@ -1,6 +1,7 @@
 #include "PeriodicTask.h"
 
 #include "../../serviceinterface/ServiceInterface.h"
+#include "../../objectmanager/ObjectManager.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
 PeriodicTask::PeriodicTask(const char *name, rtems_task_priority setPriority,
@@ -68,7 +69,7 @@ void PeriodicTask::taskFunctionality() {
 }
 
 ReturnValue_t PeriodicTask::addComponent(object_id_t object) {
-    ExecutableObjectIF* newObject = objectManager->get<ExecutableObjectIF>(object);
+    ExecutableObjectIF* newObject = ObjectManager::instance()->get<ExecutableObjectIF>(object);
     if (newObject == nullptr) {
         return HasReturnvaluesIF::RETURN_FAILED;
     }
diff --git a/parameters/ParameterHelper.cpp b/parameters/ParameterHelper.cpp
index e80c2c47..694ec5a4 100644
--- a/parameters/ParameterHelper.cpp
+++ b/parameters/ParameterHelper.cpp
@@ -1,6 +1,7 @@
 #include "ParameterHelper.h"
 #include "ParameterMessage.h"
-#include "../objectmanager/ObjectManagerIF.h"
+
+#include "../objectmanager/ObjectManager.h"
 
 ParameterHelper::ParameterHelper(ReceivesParameterMessagesIF* owner):
         owner(owner) {}
@@ -124,7 +125,7 @@ ReturnValue_t ParameterHelper::sendParameter(MessageQueueId_t to, uint32_t id,
 ReturnValue_t ParameterHelper::initialize() {
     ownerQueueId = owner->getCommandQueue();
 
-    storage = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+    storage = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
     if (storage == nullptr) {
         return ObjectManagerIF::CHILD_INIT_FAILED;
     }
diff --git a/parameters/ParameterMessage.cpp b/parameters/ParameterMessage.cpp
index 88a45c80..8a5835ff 100644
--- a/parameters/ParameterMessage.cpp
+++ b/parameters/ParameterMessage.cpp
@@ -1,5 +1,6 @@
-#include "../parameters/ParameterMessage.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "ParameterMessage.h"
+
+#include "../objectmanager/ObjectManager.h"
 
 ParameterId_t ParameterMessage::getParameterId(const CommandMessage* message) {
 	return message->getParameter();
@@ -51,7 +52,7 @@ void ParameterMessage::clear(CommandMessage* message) {
 	switch (message->getCommand()) {
 	case CMD_PARAMETER_LOAD:
 	case REPLY_PARAMETER_DUMP: {
-		StorageManagerIF *ipcStore = objectManager->get<StorageManagerIF>(
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
 				objects::IPC_STORE);
 		if (ipcStore != NULL) {
 			ipcStore->deleteData(getStoreId(message));
diff --git a/power/Fuse.cpp b/power/Fuse.cpp
index 91da5388..0cb1385b 100644
--- a/power/Fuse.cpp
+++ b/power/Fuse.cpp
@@ -2,7 +2,7 @@
 
 #include "../monitoring/LimitViolationReporter.h"
 #include "../monitoring/MonitoringMessageContent.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../serialize/SerialFixedArrayListAdapter.h"
 #include "../ipc/QueueFactory.h"
 
@@ -44,7 +44,7 @@ ReturnValue_t Fuse::initialize() {
 	if (result != RETURN_OK) {
 		return result;
 	}
-	powerIF = objectManager->get<PowerSwitchIF>(powerSwitchId);
+	powerIF = ObjectManager::instance()->get<PowerSwitchIF>(powerSwitchId);
 	if (powerIF == NULL) {
 		return RETURN_FAILED;
 	}
diff --git a/power/PowerSwitcher.cpp b/power/PowerSwitcher.cpp
index ed37998e..642a2697 100644
--- a/power/PowerSwitcher.cpp
+++ b/power/PowerSwitcher.cpp
@@ -1,7 +1,7 @@
 #include "PowerSwitcher.h"
 
-#include "../objectmanager/ObjectManagerIF.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../serviceinterface/ServiceInterface.h"
 
 PowerSwitcher::PowerSwitcher(uint8_t setSwitch1, uint8_t setSwitch2,
 		PowerSwitcher::State_t setStartState):
@@ -10,7 +10,7 @@ PowerSwitcher::PowerSwitcher(uint8_t setSwitch1, uint8_t setSwitch2,
 }
 
 ReturnValue_t PowerSwitcher::initialize(object_id_t powerSwitchId) {
-	power = objectManager->get<PowerSwitchIF>(powerSwitchId);
+	power = ObjectManager::instance()->get<PowerSwitchIF>(powerSwitchId);
 	if (power == nullptr) {
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
diff --git a/pus/CService200ModeCommanding.cpp b/pus/CService200ModeCommanding.cpp
index 70caadd1..d178b3a9 100644
--- a/pus/CService200ModeCommanding.cpp
+++ b/pus/CService200ModeCommanding.cpp
@@ -2,7 +2,8 @@
 #include "servicepackets/Service200Packets.h"
 
 #include "../modes/HasModesIF.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../serialize/SerialLinkedListAdapter.h"
 #include "../modes/ModeMessage.h"
 
@@ -40,7 +41,7 @@ ReturnValue_t CService200ModeCommanding::getMessageQueueAndObject(
 
 ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
         MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
-    HasModesIF * destination = objectManager->get<HasModesIF>(*objectId);
+    HasModesIF * destination = ObjectManager::instance()->get<HasModesIF>(*objectId);
     if(destination == nullptr) {
         return CommandingServiceBase::INVALID_OBJECT;
 
diff --git a/pus/CService201HealthCommanding.cpp b/pus/CService201HealthCommanding.cpp
index ca761f14..52a8a603 100644
--- a/pus/CService201HealthCommanding.cpp
+++ b/pus/CService201HealthCommanding.cpp
@@ -1,9 +1,11 @@
 #include "CService201HealthCommanding.h"
+#include "servicepackets/Service201Packets.h"
 
 #include "../health/HasHealthIF.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../health/HealthMessage.h"
-#include "servicepackets/Service201Packets.h"
+
 
 CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId,
         uint16_t apid, uint8_t serviceId, uint8_t numParallelCommands,
@@ -43,7 +45,7 @@ ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(
 
 ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue(
         MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
-    HasHealthIF * destination = objectManager->get<HasHealthIF>(*objectId);
+    HasHealthIF * destination = ObjectManager::instance()->get<HasHealthIF>(*objectId);
     if(destination == nullptr) {
         return CommandingServiceBase::INVALID_OBJECT;
     }
diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index 7ef08de7..bef7b6b1 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -2,6 +2,7 @@
 #include "servicepackets/Service1Packets.h"
 
 #include "../ipc/QueueFactory.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../tmtcservices/PusVerificationReport.h"
 #include "../tmtcpacket/pus/TmPacketStored.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
@@ -99,7 +100,7 @@ ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
 
 ReturnValue_t Service1TelecommandVerification::initialize() {
 	// Get target object for TC verification messages
-	AcceptsTelemetryIF* funnel = objectManager->
+	AcceptsTelemetryIF* funnel = ObjectManager::instance()->
 			get<AcceptsTelemetryIF>(targetDestination);
 	if(funnel == nullptr){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/pus/Service20ParameterManagement.cpp b/pus/Service20ParameterManagement.cpp
index 90e96650..8ebc6db0 100644
--- a/pus/Service20ParameterManagement.cpp
+++ b/pus/Service20ParameterManagement.cpp
@@ -1,11 +1,11 @@
 #include "Service20ParameterManagement.h"
 #include "servicepackets/Service20Packets.h"
 
-#include <fsfw/serviceinterface/ServiceInterface.h>
-#include <fsfw/parameters/HasParametersIF.h>
-#include <fsfw/parameters/ParameterMessage.h>
-#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
-#include <tmtc/pusIds.h>
+#include "../serviceinterface/ServiceInterface.h"
+#include "../parameters/HasParametersIF.h"
+#include "../parameters/ParameterMessage.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../parameters/ReceivesParameterMessagesIF.h"
 
 
 Service20ParameterManagement::Service20ParameterManagement(object_id_t objectId, uint16_t apid,
@@ -65,7 +65,7 @@ ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue
         MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
     // check ReceivesParameterMessagesIF property of target
     ReceivesParameterMessagesIF* possibleTarget =
-            objectManager->get<ReceivesParameterMessagesIF>(*objectId);
+            ObjectManager::instance()->get<ReceivesParameterMessagesIF>(*objectId);
     if(possibleTarget == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "Service20ParameterManagement::checkInterfaceAndAcquire"
diff --git a/pus/Service20ParameterManagement.h b/pus/Service20ParameterManagement.h
index 488edfb5..5370bfcb 100644
--- a/pus/Service20ParameterManagement.h
+++ b/pus/Service20ParameterManagement.h
@@ -1,7 +1,7 @@
 #ifndef FSFW_PUS_SERVICE20PARAMETERMANAGEMENT_H_
 #define FSFW_PUS_SERVICE20PARAMETERMANAGEMENT_H_
 
-#include <fsfw/tmtcservices/CommandingServiceBase.h>
+#include "../tmtcservices/CommandingServiceBase.h"
 
 /**
  * @brief   PUS Service 20 Parameter Service implementation
diff --git a/pus/Service2DeviceAccess.cpp b/pus/Service2DeviceAccess.cpp
index 72db82df..4cf75d32 100644
--- a/pus/Service2DeviceAccess.cpp
+++ b/pus/Service2DeviceAccess.cpp
@@ -1,6 +1,7 @@
 #include "Service2DeviceAccess.h"
 #include "servicepackets/Service2Packets.h"
 
+#include "../objectmanager/ObjectManager.h"
 #include "../devicehandlers/DeviceHandlerIF.h"
 #include "../storagemanager/StorageManagerIF.h"
 #include "../devicehandlers/DeviceHandlerMessage.h"
@@ -47,7 +48,7 @@ ReturnValue_t Service2DeviceAccess::getMessageQueueAndObject(
 ReturnValue_t Service2DeviceAccess::checkInterfaceAndAcquireMessageQueue(
 		MessageQueueId_t * messageQueueToSet, object_id_t *objectId) {
 	DeviceHandlerIF* possibleTarget =
-			objectManager->get<DeviceHandlerIF>(*objectId);
+	        ObjectManager::instance()->get<DeviceHandlerIF>(*objectId);
 	if(possibleTarget == nullptr) {
 		return CommandingServiceBase::INVALID_OBJECT;
 	}
diff --git a/pus/Service3Housekeeping.cpp b/pus/Service3Housekeeping.cpp
index c4f80c2a..6b1275b3 100644
--- a/pus/Service3Housekeeping.cpp
+++ b/pus/Service3Housekeeping.cpp
@@ -1,7 +1,8 @@
 #include "Service3Housekeeping.h"
 #include "servicepackets/Service3Packets.h"
-#include "../datapoollocal/HasLocalDataPoolIF.h"
 
+#include "../objectmanager/ObjectManager.h"
+#include "../datapoollocal/HasLocalDataPoolIF.h"
 
 Service3Housekeeping::Service3Housekeeping(object_id_t objectId, uint16_t apid,
         uint8_t serviceId):
@@ -56,7 +57,7 @@ ReturnValue_t Service3Housekeeping::checkInterfaceAndAcquireMessageQueue(
         MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
     // check HasLocalDataPoolIF property of target
     HasLocalDataPoolIF* possibleTarget =
-            objectManager->get<HasLocalDataPoolIF>(*objectId);
+            ObjectManager::instance()->get<HasLocalDataPoolIF>(*objectId);
     if(possibleTarget == nullptr){
         return CommandingServiceBase::INVALID_OBJECT;
     }
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index 62eefcb3..272cc203 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -1,7 +1,8 @@
 #include "Service5EventReporting.h"
 #include "servicepackets/Service5Packets.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../events/EventManagerIF.h"
 #include "../ipc/QueueFactory.h"
 #include "../tmtcpacket/pus/TmPacketStored.h"
@@ -89,7 +90,7 @@ ReturnValue_t Service5EventReporting::handleRequest(uint8_t subservice) {
 // In addition to the default PUSServiceBase initialization, this service needs
 // to be registered to the event manager to listen for events.
 ReturnValue_t Service5EventReporting::initialize() {
-	EventManagerIF* manager = objectManager->get<EventManagerIF>(
+	EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
 				objects::EVENT_MANAGER);
 	if (manager == NULL) {
 			return RETURN_FAILED;
diff --git a/pus/Service8FunctionManagement.cpp b/pus/Service8FunctionManagement.cpp
index 54187a82..77b7dc80 100644
--- a/pus/Service8FunctionManagement.cpp
+++ b/pus/Service8FunctionManagement.cpp
@@ -1,11 +1,12 @@
 #include "Service8FunctionManagement.h"
 #include "servicepackets/Service8Packets.h"
 
+#include "../objectmanager/ObjectManager.h"
 #include "../objectmanager/SystemObjectIF.h"
 #include "../action/HasActionsIF.h"
 #include "../devicehandlers/DeviceHandlerIF.h"
 #include "../serialize/SerializeAdapter.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 
 Service8FunctionManagement::Service8FunctionManagement(object_id_t objectId,
 		uint16_t apid, uint8_t serviceId, uint8_t numParallelCommands,
@@ -41,7 +42,7 @@ ReturnValue_t Service8FunctionManagement::getMessageQueueAndObject(
 ReturnValue_t Service8FunctionManagement::checkInterfaceAndAcquireMessageQueue(
 		MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
 	// check HasActionIF property of target
-	HasActionsIF* possibleTarget = objectManager->get<HasActionsIF>(*objectId);
+	HasActionsIF* possibleTarget = ObjectManager::instance()->get<HasActionsIF>(*objectId);
 	if(possibleTarget == nullptr){
 		return CommandingServiceBase::INVALID_OBJECT;
 	}
diff --git a/storagemanager/LocalPool.cpp b/storagemanager/LocalPool.cpp
index 2b733548..41c9250a 100644
--- a/storagemanager/LocalPool.cpp
+++ b/storagemanager/LocalPool.cpp
@@ -1,5 +1,8 @@
 #include "LocalPool.h"
-#include <FSFWConfig.h>
+#include "FSFWConfig.h"
+
+#include "../objectmanager/ObjectManager.h"
+
 #include <cstring>
 
 LocalPool::LocalPool(object_id_t setObjectId, const LocalPoolConfig& poolConfig,
@@ -185,7 +188,7 @@ ReturnValue_t LocalPool::initialize() {
     if (result != RETURN_OK) {
         return result;
     }
-    internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
+    internalErrorReporter = ObjectManager::instance()->get<InternalErrorReporterIF>(
             objects::INTERNAL_ERROR_REPORTER);
     if (internalErrorReporter == nullptr){
         return ObjectManagerIF::INTERNAL_ERR_REPORTER_UNINIT;
diff --git a/subsystem/Subsystem.cpp b/subsystem/Subsystem.cpp
index 4de6906c..dffad034 100644
--- a/subsystem/Subsystem.cpp
+++ b/subsystem/Subsystem.cpp
@@ -1,6 +1,7 @@
 #include "Subsystem.h"
+
 #include "../health/HealthMessage.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../serialize/SerialArrayListAdapter.h"
 #include "../serialize/SerialFixedArrayListAdapter.h"
 #include "../serialize/SerializeElement.h"
@@ -477,13 +478,13 @@ ReturnValue_t Subsystem::initialize() {
 		return result;
 	}
 
-	IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+	IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
 	if (IPCStore == NULL) {
 		return RETURN_FAILED;
 	}
 
 #if FSFW_USE_MODESTORE == 1
-	modeStore = objectManager->get<ModeStoreIF>(objects::MODE_STORE);
+	modeStore = ObjectManager::instance()->get<ModeStoreIF>(objects::MODE_STORE);
 
 	if (modeStore == nullptr) {
 		return RETURN_FAILED;
diff --git a/subsystem/SubsystemBase.cpp b/subsystem/SubsystemBase.cpp
index 565e0712..0d459324 100644
--- a/subsystem/SubsystemBase.cpp
+++ b/subsystem/SubsystemBase.cpp
@@ -1,6 +1,7 @@
-#include "../serviceinterface/ServiceInterfaceStream.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
-#include "../subsystem/SubsystemBase.h"
+#include "SubsystemBase.h"
+
+#include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../ipc/QueueFactory.h"
 
 SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent,
@@ -19,10 +20,10 @@ SubsystemBase::~SubsystemBase() {
 ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
 	ChildInfo info;
 
-	HasModesIF *child = objectManager->get<HasModesIF>(objectId);
+	HasModesIF *child = ObjectManager::instance()->get<HasModesIF>(objectId);
 	// This is a rather ugly hack to have the changedHealth info for all
 	// children available.
-	HasHealthIF* healthChild = objectManager->get<HasHealthIF>(objectId);
+	HasHealthIF* healthChild = ObjectManager::instance()->get<HasHealthIF>(objectId);
 	if (child == nullptr) {
 		if (healthChild == nullptr) {
 			return CHILD_DOESNT_HAVE_MODES;
@@ -174,7 +175,7 @@ ReturnValue_t SubsystemBase::initialize() {
 	}
 
 	if (parentId != objects::NO_OBJECT) {
-		SubsystemBase *parent = objectManager->get<SubsystemBase>(parentId);
+		SubsystemBase *parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
 		if (parent == nullptr) {
 			return RETURN_FAILED;
 		}
diff --git a/subsystem/modes/ModeSequenceMessage.cpp b/subsystem/modes/ModeSequenceMessage.cpp
index 7733098e..749a90bf 100644
--- a/subsystem/modes/ModeSequenceMessage.cpp
+++ b/subsystem/modes/ModeSequenceMessage.cpp
@@ -1,6 +1,6 @@
 #include "ModeSequenceMessage.h"
 
-#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../objectmanager/ObjectManager.h"
 #include "../../storagemanager/StorageManagerIF.h"
 
 void ModeSequenceMessage::setModeSequenceMessage(CommandMessage* message,
@@ -50,7 +50,7 @@ void ModeSequenceMessage::clear(CommandMessage *message) {
 	case TABLE_LIST:
 	case TABLE:
 	case SEQUENCE: {
-		StorageManagerIF *ipcStore = objectManager->get<StorageManagerIF>(
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
 		        objects::IPC_STORE);
 		if (ipcStore != nullptr){
 			ipcStore->deleteData(ModeSequenceMessage::getStoreAddress(message));
diff --git a/tcdistribution/CCSDSDistributor.cpp b/tcdistribution/CCSDSDistributor.cpp
index 62cbfbf2..7380866a 100644
--- a/tcdistribution/CCSDSDistributor.cpp
+++ b/tcdistribution/CCSDSDistributor.cpp
@@ -1,5 +1,6 @@
 #include "CCSDSDistributor.h"
 
+#include "../objectmanager/ObjectManager.h"
 #include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/SpacePacketBase.h"
 
@@ -86,7 +87,7 @@ uint16_t CCSDSDistributor::getIdentifier() {
 
 ReturnValue_t CCSDSDistributor::initialize() {
 	ReturnValue_t status = this->TcDistributor::initialize();
-	this->tcStore = objectManager->get<StorageManagerIF>( objects::TC_STORE );
+	this->tcStore = ObjectManager::instance()->get<StorageManagerIF>( objects::TC_STORE );
 	if (this->tcStore == nullptr) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index abdd1f8d..0fac9ba0 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -1,6 +1,7 @@
 #include "CCSDSDistributorIF.h"
 #include "PUSDistributor.h"
 
+#include "../objectmanager/ObjectManager.h"
 #include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
@@ -125,7 +126,7 @@ ReturnValue_t PUSDistributor::initialize() {
     }
 
     CCSDSDistributorIF* ccsdsDistributor =
-            objectManager->get<CCSDSDistributorIF>(packetSource);
+            ObjectManager::instance()->get<CCSDSDistributorIF>(packetSource);
     if (ccsdsDistributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "PUSDistributor::initialize: Packet source invalid" << std::endl;
diff --git a/thermal/Heater.cpp b/thermal/Heater.cpp
index 77049438..f97cb543 100644
--- a/thermal/Heater.cpp
+++ b/thermal/Heater.cpp
@@ -1,5 +1,6 @@
 #include "Heater.h"
 
+#include "../objectmanager/ObjectManager.h"
 #include "../devicehandlers/DeviceHandlerFailureIsolation.h"
 #include "../power/Fuse.h"
 #include "../ipc/QueueFactory.h"
@@ -239,7 +240,7 @@ ReturnValue_t Heater::initialize() {
         return result;
     }
 
-    EventManagerIF* manager = objectManager->get<EventManagerIF>(
+    EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
             objects::EVENT_MANAGER);
     if (manager == NULL) {
         return HasReturnvaluesIF::RETURN_FAILED;
@@ -249,7 +250,7 @@ ReturnValue_t Heater::initialize() {
         return result;
     }
 
-    ConfirmsFailuresIF* pcdu = objectManager->get<ConfirmsFailuresIF>(
+    ConfirmsFailuresIF* pcdu = ObjectManager::instance()->get<ConfirmsFailuresIF>(
             DeviceHandlerFailureIsolation::powerConfirmationId);
     if (pcdu == NULL) {
         return HasReturnvaluesIF::RETURN_FAILED;
diff --git a/tmstorage/TmStoreMessage.cpp b/tmstorage/TmStoreMessage.cpp
index 033cbb1d..11af6121 100644
--- a/tmstorage/TmStoreMessage.cpp
+++ b/tmstorage/TmStoreMessage.cpp
@@ -1,5 +1,5 @@
 #include "TmStoreMessage.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 
 TmStoreMessage::~TmStoreMessage() {
 	
@@ -64,7 +64,7 @@ void TmStoreMessage::clear(CommandMessage* cmd) {
 	case INDEX_REPORT:
 	case DELETE_STORE_CONTENT_TIME:
 	case DOWNLINK_STORE_CONTENT_TIME: {
-		StorageManagerIF *ipcStore = objectManager->get<StorageManagerIF>(
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
 				objects::IPC_STORE);
 		if (ipcStore != NULL) {
 			ipcStore->deleteData(getStoreId(cmd));
diff --git a/tmtcpacket/pus/TcPacketStored.cpp b/tmtcpacket/pus/TcPacketStored.cpp
index f320386c..36fa8d11 100644
--- a/tmtcpacket/pus/TcPacketStored.cpp
+++ b/tmtcpacket/pus/TcPacketStored.cpp
@@ -1,6 +1,7 @@
 #include "TcPacketStored.h"
-#include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
 
 #include <cstring>
 
@@ -63,7 +64,7 @@ ReturnValue_t TcPacketStored::deletePacket() {
 
 bool TcPacketStored::checkAndSetStore() {
 	if (this->store == nullptr) {
-		this->store = objectManager->get<StorageManagerIF>(objects::TC_STORE);
+		this->store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
 		if (this->store == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "TcPacketStored::TcPacketStored: TC Store not found!"
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp
index 25193c92..acd69b65 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/TmPacketBase.cpp
@@ -2,7 +2,7 @@
 
 #include "../../globalfunctions/CRC.h"
 #include "../../globalfunctions/arrayprinter.h"
-#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../objectmanager/ObjectManager.h"
 #include "../../serviceinterface/ServiceInterface.h"
 #include "../../timemanager/CCSDSTime.h"
 
@@ -53,7 +53,7 @@ void TmPacketBase::print() {
 
 bool TmPacketBase::checkAndSetStamper() {
     if (timeStamper == NULL) {
-        timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
+        timeStamper = ObjectManager::instance()->get<TimeStamperIF>(timeStamperId);
         if (timeStamper == NULL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::warning << "TmPacketBase::checkAndSetStamper: Stamper not found!" << std::endl;
diff --git a/tmtcpacket/pus/TmPacketStoredBase.cpp b/tmtcpacket/pus/TmPacketStoredBase.cpp
index 3ab31a80..eeaa938d 100644
--- a/tmtcpacket/pus/TmPacketStoredBase.cpp
+++ b/tmtcpacket/pus/TmPacketStoredBase.cpp
@@ -1,7 +1,7 @@
 #include "TmPacketStoredBase.h"
 
-#include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tmtcservices/TmTcMessage.h"
 
 #include <cstring>
@@ -48,7 +48,7 @@ void TmPacketStoredBase::setStoreAddress(store_address_t setAddress) {
 
 bool TmPacketStoredBase::checkAndSetStore() {
     if (store == nullptr) {
-        store = objectManager->get<StorageManagerIF>(objects::TM_STORE);
+        store = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
         if (store == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::error << "TmPacketStored::TmPacketStored: TM Store not found!"
@@ -83,7 +83,7 @@ ReturnValue_t TmPacketStoredBase::sendPacket(MessageQueueId_t destination,
 
 void TmPacketStoredBase::checkAndReportLostTm() {
     if (internalErrorReporter == nullptr) {
-        internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
+        internalErrorReporter = ObjectManager::instance()->get<InternalErrorReporterIF>(
                 objects::INTERNAL_ERROR_REPORTER);
     }
     if (internalErrorReporter != nullptr) {
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index fbd29468..863cba4f 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -4,7 +4,7 @@
 #include <FSFWConfig.h>
 
 #include "../tcdistribution/PUSDistributorIF.h"
-#include "../objectmanager/ObjectManagerIF.h"
+#include "../objectmanager/ObjectManager.h"
 #include "../ipc/QueueFactory.h"
 #include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcpacket/pus/TmPacketStored.h"
@@ -68,12 +68,12 @@ ReturnValue_t CommandingServiceBase::initialize() {
 	    packetDestination = defaultPacketDestination;
 	}
 	AcceptsTelemetryIF* packetForwarding =
-			objectManager->get<AcceptsTelemetryIF>(packetDestination);
+	        ObjectManager::instance()->get<AcceptsTelemetryIF>(packetDestination);
 
 	if(packetSource == objects::NO_OBJECT) {
 	    packetSource = defaultPacketSource;
 	}
-	PUSDistributorIF* distributor = objectManager->get<PUSDistributorIF>(
+	PUSDistributorIF* distributor = ObjectManager::instance()->get<PUSDistributorIF>(
 			packetSource);
 
 	if (packetForwarding == nullptr or distributor == nullptr) {
@@ -88,8 +88,8 @@ ReturnValue_t CommandingServiceBase::initialize() {
 	requestQueue->setDefaultDestination(
 			packetForwarding->getReportReceptionQueue());
 
-	IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
-	TCStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
+	IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+	TCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
 
 	if (IPCStore == nullptr or TCStore == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/tmtcservices/PusServiceBase.cpp b/tmtcservices/PusServiceBase.cpp
index 0a5cb202..811c9bcb 100644
--- a/tmtcservices/PusServiceBase.cpp
+++ b/tmtcservices/PusServiceBase.cpp
@@ -3,7 +3,8 @@
 #include "PusVerificationReport.h"
 #include "TmTcMessage.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../tcdistribution/PUSDistributorIF.h"
 #include "../ipc/QueueFactory.h"
 
@@ -105,9 +106,9 @@ ReturnValue_t PusServiceBase::initialize() {
 	if (result != RETURN_OK) {
 		return result;
 	}
-	AcceptsTelemetryIF* destService = objectManager->get<AcceptsTelemetryIF>(
+	AcceptsTelemetryIF* destService = ObjectManager::instance()->get<AcceptsTelemetryIF>(
 			packetDestination);
-	PUSDistributorIF* distributor = objectManager->get<PUSDistributorIF>(
+	PUSDistributorIF* distributor = ObjectManager::instance()->get<PUSDistributorIF>(
 			packetSource);
 	if (destService == nullptr or distributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/tmtcservices/TmTcBridge.cpp b/tmtcservices/TmTcBridge.cpp
index 1257ef89..7198bc76 100644
--- a/tmtcservices/TmTcBridge.cpp
+++ b/tmtcservices/TmTcBridge.cpp
@@ -1,5 +1,6 @@
 #include "TmTcBridge.h"
 
+#include "../objectmanager/ObjectManager.h"
 #include "../ipc/QueueFactory.h"
 #include "../serviceinterface/ServiceInterface.h"
 #include "../globalfunctions/arrayprinter.h"
@@ -53,7 +54,7 @@ ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(
 }
 
 ReturnValue_t TmTcBridge::initialize() {
-	tcStore = objectManager->get<StorageManagerIF>(tcStoreId);
+	tcStore = ObjectManager::instance()->get<StorageManagerIF>(tcStoreId);
 	if (tcStore == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcBridge::initialize: TC store invalid. Make sure"
@@ -61,7 +62,7 @@ ReturnValue_t TmTcBridge::initialize() {
 #endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
-	tmStore = objectManager->get<StorageManagerIF>(tmStoreId);
+	tmStore = ObjectManager::instance()->get<StorageManagerIF>(tmStoreId);
 	if (tmStore == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcBridge::initialize: TM store invalid. Make sure"
@@ -70,7 +71,7 @@ ReturnValue_t TmTcBridge::initialize() {
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 	AcceptsTelecommandsIF* tcDistributor =
-			objectManager->get<AcceptsTelecommandsIF>(tcDestination);
+	        ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
 	if (tcDistributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcBridge::initialize: TC Distributor invalid"
diff --git a/tmtcservices/VerificationReporter.cpp b/tmtcservices/VerificationReporter.cpp
index ff6f54f9..998cbfb6 100644
--- a/tmtcservices/VerificationReporter.cpp
+++ b/tmtcservices/VerificationReporter.cpp
@@ -2,8 +2,9 @@
 #include "AcceptsVerifyMessageIF.h"
 #include "PusVerificationReport.h"
 
+#include "../objectmanager/ObjectManager.h"
 #include "../ipc/MessageQueueIF.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/frameworkObjects.h"
 
 object_id_t VerificationReporter::messageReceiver =
@@ -104,7 +105,7 @@ void VerificationReporter::initialize() {
 #endif
 		return;
 	}
-	AcceptsVerifyMessageIF* temp = objectManager->get<AcceptsVerifyMessageIF>(
+	AcceptsVerifyMessageIF* temp = ObjectManager::instance()->get<AcceptsVerifyMessageIF>(
 			messageReceiver);
 	if (temp == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/unittest/tests/datapoollocal/DataSetTest.cpp b/unittest/tests/datapoollocal/DataSetTest.cpp
index d0b13e86..b8748eb4 100644
--- a/unittest/tests/datapoollocal/DataSetTest.cpp
+++ b/unittest/tests/datapoollocal/DataSetTest.cpp
@@ -3,6 +3,7 @@
 #include <catch2/catch_test_macros.hpp>
 #include <catch2/catch_approx.hpp>
 
+#include <fsfw/objectmanager/ObjectManager.h>
 #include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
 #include <fsfw/datapoollocal/SharedLocalDataSet.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
@@ -12,7 +13,7 @@
 #include <unittest/core/CatchDefinitions.h>
 
 TEST_CASE("DataSetTest" , "[DataSetTest]") {
-    LocalPoolOwnerBase* poolOwner = objectManager->
+    LocalPoolOwnerBase* poolOwner = ObjectManager::instance()->
             get<LocalPoolOwnerBase>(objects::TEST_LOCAL_POOL_OWNER_BASE);
     REQUIRE(poolOwner != nullptr);
     REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK);
diff --git a/unittest/tests/datapoollocal/LocalPoolManagerTest.cpp b/unittest/tests/datapoollocal/LocalPoolManagerTest.cpp
index 52485b01..4a4d08fb 100644
--- a/unittest/tests/datapoollocal/LocalPoolManagerTest.cpp
+++ b/unittest/tests/datapoollocal/LocalPoolManagerTest.cpp
@@ -3,6 +3,7 @@
 #include <catch2/catch_test_macros.hpp>
 #include <catch2/catch_approx.hpp>
 
+#include <fsfw/objectmanager/ObjectManager.h>
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
@@ -14,7 +15,7 @@
 
 
 TEST_CASE("LocalPoolManagerTest" , "[LocManTest]") {
-    LocalPoolOwnerBase* poolOwner = objectManager->
+    LocalPoolOwnerBase* poolOwner = ObjectManager::instance()->
             get<LocalPoolOwnerBase>(objects::TEST_LOCAL_POOL_OWNER_BASE);
     REQUIRE(poolOwner != nullptr);
     REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK);
diff --git a/unittest/tests/datapoollocal/LocalPoolVariableTest.cpp b/unittest/tests/datapoollocal/LocalPoolVariableTest.cpp
index 980ffda1..514d8125 100644
--- a/unittest/tests/datapoollocal/LocalPoolVariableTest.cpp
+++ b/unittest/tests/datapoollocal/LocalPoolVariableTest.cpp
@@ -1,12 +1,13 @@
 #include "LocalPoolOwnerBase.h"
 
 #include <catch2/catch_test_macros.hpp>
+#include <fsfw/objectmanager/ObjectManager.h>
 #include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
 #include <unittest/core/CatchDefinitions.h>
 
 
 TEST_CASE("LocalPoolVariable" , "[LocPoolVarTest]") {
-    LocalPoolOwnerBase* poolOwner = objectManager->
+    LocalPoolOwnerBase* poolOwner = ObjectManager::instance()->
             get<LocalPoolOwnerBase>(objects::TEST_LOCAL_POOL_OWNER_BASE);
     REQUIRE(poolOwner != nullptr);
     REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK);
diff --git a/unittest/tests/datapoollocal/LocalPoolVectorTest.cpp b/unittest/tests/datapoollocal/LocalPoolVectorTest.cpp
index db76fc00..5b3dd105 100644
--- a/unittest/tests/datapoollocal/LocalPoolVectorTest.cpp
+++ b/unittest/tests/datapoollocal/LocalPoolVectorTest.cpp
@@ -1,11 +1,12 @@
 #include "LocalPoolOwnerBase.h"
 
 #include <catch2/catch_test_macros.hpp>
+#include <fsfw/objectmanager/ObjectManager.h>
 #include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
 #include <unittest/core/CatchDefinitions.h>
 
 TEST_CASE("LocalPoolVector" , "[LocPoolVecTest]") {
-    LocalPoolOwnerBase* poolOwner = objectManager->
+    LocalPoolOwnerBase* poolOwner = ObjectManager::instance()->
             get<LocalPoolOwnerBase>(objects::TEST_LOCAL_POOL_OWNER_BASE);
     REQUIRE(poolOwner != nullptr);
     REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK);
diff --git a/unittest/user/unittest/core/CatchDefinitions.cpp b/unittest/user/unittest/core/CatchDefinitions.cpp
index bae02875..c44a561e 100644
--- a/unittest/user/unittest/core/CatchDefinitions.cpp
+++ b/unittest/user/unittest/core/CatchDefinitions.cpp
@@ -1,10 +1,10 @@
 #include "CatchDefinitions.h"
 #include <fsfw/serviceinterface/ServiceInterface.h>
-#include <fsfw/objectmanager/ObjectManagerIF.h>
+#include <fsfw/objectmanager/ObjectManager.h>
 
 StorageManagerIF* tglob::getIpcStoreHandle() {
-	if(objectManager != nullptr) {
-		return objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+	if(ObjectManager::instance() != nullptr) {
+		return ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
 	} else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Global object manager uninitialized" << std::endl;
diff --git a/unittest/user/unittest/core/CatchFactory.h b/unittest/user/unittest/core/CatchFactory.h
index f06e7ae5..024f762e 100644
--- a/unittest/user/unittest/core/CatchFactory.h
+++ b/unittest/user/unittest/core/CatchFactory.h
@@ -8,7 +8,7 @@ namespace Factory {
 	 * @brief 	Creates all SystemObject elements which are persistent
 	 * 			during execution.
 	 */
-	void produce();
+	void produce(void* args);
 	void setStaticFrameworkObjectIds();
 
 }

From 145dd33fb1e467ef7370ccd44671a91d96c60e26 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 5 Jun 2021 20:30:36 +0200
Subject: [PATCH 120/125] fixed merge conflict

---
 osal/linux/MessageQueue.cpp | 43 ++-----------------------------------
 1 file changed, 2 insertions(+), 41 deletions(-)

diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index 75376798..3c151143 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -252,48 +252,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         bool ignoreFault) {
     if(message == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-<<<<<<< HEAD
-		sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is "
-				"nullptr!" << std::endl;
-#endif
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	message->setSender(sentFrom);
-	int result = mq_send(sendTo,
-			reinterpret_cast<const char*>(message->getBuffer()),
-			message->getMessageSize(),0);
-
-	//TODO: Check if we're in ISR.
-	if (result != 0) {
-		if(!ignoreFault){
-			InternalErrorReporterIF* internalErrorReporter =
-			        ObjectManager::instance()->get<InternalErrorReporterIF>(
-						objects::INTERNAL_ERROR_REPORTER);
-			if (internalErrorReporter != NULL) {
-				internalErrorReporter->queueMessageNotSent();
-			}
-		}
-		switch(errno){
-		case EAGAIN:
-			//The O_NONBLOCK flag was set when opening the queue, or the
-			//MQ_NONBLOCK flag was set in its attributes, and the
-			//specified queue is full.
-			return MessageQueueIF::FULL;
-		case EBADF: {
-			//mq_des doesn't represent a valid message queue descriptor,
-			//or mq_des wasn't opened for writing.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::sendMessage: Configuration error, MQ"
-					<< " destination invalid."  << std::endl;
-			sif::error << strerror(errno) << " in "
-					<<"mq_send to: " << sendTo << " sent from "
-					<< sentFrom << std::endl;
-=======
         sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr!" << std::endl;
 #else
         sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr!\n");
->>>>>>> 38910143400e455f5184ad85be98e05638c2eea6
 #endif
         return HasReturnvaluesIF::RETURN_FAILED;
     }
@@ -306,8 +267,8 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
     //TODO: Check if we're in ISR.
     if (result != 0) {
         if(!ignoreFault){
-            InternalErrorReporterIF* internalErrorReporter =
-                    objectManager->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
+            InternalErrorReporterIF* internalErrorReporter = ObjectManager::instance()->
+                    get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
             if (internalErrorReporter != NULL) {
                 internalErrorReporter->queueMessageNotSent();
             }

From 0157681471f16bf9f6412c1f27d1739c9ac1e6c9 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 8 Jun 2021 14:27:21 +0200
Subject: [PATCH 121/125] removed obsolete code

---
 objectmanager/ObjectManager.h | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/objectmanager/ObjectManager.h b/objectmanager/ObjectManager.h
index 1d4c3185..2ca52028 100644
--- a/objectmanager/ObjectManager.h
+++ b/objectmanager/ObjectManager.h
@@ -68,12 +68,6 @@ private:
     static ObjectManager* objManagerInstance;
 };
 
-/**
- * @brief   This is the forward declaration of the global objectManager instance.
- */
-// SHOULDDO: maybe put this in the glob namespace to explicitely mark it global?
-//extern ObjectManagerIF *objectManager;
-
 /*Documentation can be found in the class method declaration above.*/
 template <typename T>
 T* ObjectManager::get( object_id_t id ) {

From 75adf52d282f26e92d5640e8bd6f285ef0f74f79 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 8 Jun 2021 14:30:35 +0200
Subject: [PATCH 122/125] small update

---
 objectmanager/ObjectManager.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/objectmanager/ObjectManager.h b/objectmanager/ObjectManager.h
index 2ca52028..9f5f0c37 100644
--- a/objectmanager/ObjectManager.h
+++ b/objectmanager/ObjectManager.h
@@ -68,7 +68,7 @@ private:
     static ObjectManager* objManagerInstance;
 };
 
-/*Documentation can be found in the class method declaration above.*/
+// Documentation can be found in the class method declaration above
 template <typename T>
 T* ObjectManager::get( object_id_t id ) {
     SystemObjectIF* temp = this->getSystemObject(id);

From 1630682548a8775bd0c293b3c76c29e120de5bf0 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 11 Jun 2021 14:52:09 +0200
Subject: [PATCH 123/125] added function to set color

---
 serviceinterface/ServiceInterfaceBuffer.cpp | 4 ++++
 serviceinterface/ServiceInterfaceBuffer.h   | 1 +
 serviceinterface/ServiceInterfaceStream.cpp | 4 ++++
 serviceinterface/ServiceInterfaceStream.h   | 2 ++
 4 files changed, 11 insertions(+)

diff --git a/serviceinterface/ServiceInterfaceBuffer.cpp b/serviceinterface/ServiceInterfaceBuffer.cpp
index ccc051c3..a6a2cb71 100644
--- a/serviceinterface/ServiceInterfaceBuffer.cpp
+++ b/serviceinterface/ServiceInterfaceBuffer.cpp
@@ -171,6 +171,10 @@ bool ServiceInterfaceBuffer::crAdditionEnabled() const {
     return addCrToPreamble;
 }
 
+void ServiceInterfaceBuffer::setAsciiColorPrefix(std::string colorPrefix) {
+    this->colorPrefix = colorPrefix;
+}
+
 #ifdef UT699
 #include "../osal/rtems/Interrupt.h"
 
diff --git a/serviceinterface/ServiceInterfaceBuffer.h b/serviceinterface/ServiceInterfaceBuffer.h
index 9d3ce069..ad1148a2 100644
--- a/serviceinterface/ServiceInterfaceBuffer.h
+++ b/serviceinterface/ServiceInterfaceBuffer.h
@@ -48,6 +48,7 @@ private:
 
 #if FSFW_COLORED_OUTPUT == 1
 	std::string colorPrefix;
+	void setAsciiColorPrefix(std::string colorPrefix);
 #endif
 
 	// For EOF detection
diff --git a/serviceinterface/ServiceInterfaceStream.cpp b/serviceinterface/ServiceInterfaceStream.cpp
index ad14cd04..be8dc770 100644
--- a/serviceinterface/ServiceInterfaceStream.cpp
+++ b/serviceinterface/ServiceInterfaceStream.cpp
@@ -19,5 +19,9 @@ bool ServiceInterfaceStream::crAdditionEnabled() const {
     return streambuf.crAdditionEnabled();
 }
 
+bool ServiceInterfaceStream::setAsciiColorPrefix(std::string asciiColorCode) {
+    streambuf.setAsciiColorPrefix(asciiColorCode);
+}
+
 #endif
 
diff --git a/serviceinterface/ServiceInterfaceStream.h b/serviceinterface/ServiceInterfaceStream.h
index 0ea44f0b..3c788b3c 100644
--- a/serviceinterface/ServiceInterfaceStream.h
+++ b/serviceinterface/ServiceInterfaceStream.h
@@ -46,6 +46,8 @@ public:
 	 */
 	bool crAdditionEnabled() const;
 
+	bool setAsciiColorPrefix(std::string asciiColorCode);
+
 protected:
 	ServiceInterfaceBuffer streambuf;
 };

From 589e64fc467cbf5cac5e721a896564c811cd25c6 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 11 Jun 2021 15:05:43 +0200
Subject: [PATCH 124/125] void function now

---
 serviceinterface/ServiceInterfaceStream.cpp | 2 +-
 serviceinterface/ServiceInterfaceStream.h   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/serviceinterface/ServiceInterfaceStream.cpp b/serviceinterface/ServiceInterfaceStream.cpp
index be8dc770..80942b88 100644
--- a/serviceinterface/ServiceInterfaceStream.cpp
+++ b/serviceinterface/ServiceInterfaceStream.cpp
@@ -19,7 +19,7 @@ bool ServiceInterfaceStream::crAdditionEnabled() const {
     return streambuf.crAdditionEnabled();
 }
 
-bool ServiceInterfaceStream::setAsciiColorPrefix(std::string asciiColorCode) {
+void ServiceInterfaceStream::setAsciiColorPrefix(std::string asciiColorCode) {
     streambuf.setAsciiColorPrefix(asciiColorCode);
 }
 
diff --git a/serviceinterface/ServiceInterfaceStream.h b/serviceinterface/ServiceInterfaceStream.h
index 3c788b3c..aceddb22 100644
--- a/serviceinterface/ServiceInterfaceStream.h
+++ b/serviceinterface/ServiceInterfaceStream.h
@@ -46,7 +46,7 @@ public:
 	 */
 	bool crAdditionEnabled() const;
 
-	bool setAsciiColorPrefix(std::string asciiColorCode);
+	void setAsciiColorPrefix(std::string asciiColorCode);
 
 protected:
 	ServiceInterfaceBuffer streambuf;

From 73bae057bddf236f89a18e8d99b866c887d4d3b6 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 12 Jun 2021 15:03:18 +0200
Subject: [PATCH 125/125] default color is magneta now for wanring

---
 datapoollocal/LocalDataPoolManager.cpp      | 1465 ++++++++++---------
 serviceinterface/ServiceInterfaceBuffer.cpp |    2 +-
 2 files changed, 736 insertions(+), 731 deletions(-)

diff --git a/datapoollocal/LocalDataPoolManager.cpp b/datapoollocal/LocalDataPoolManager.cpp
index 1cbf8201..71997b9b 100644
--- a/datapoollocal/LocalDataPoolManager.cpp
+++ b/datapoollocal/LocalDataPoolManager.cpp
@@ -20,23 +20,23 @@
 object_id_t LocalDataPoolManager::defaultHkDestination = objects::PUS_SERVICE_3_HOUSEKEEPING;
 
 LocalDataPoolManager::LocalDataPoolManager(HasLocalDataPoolIF* owner, MessageQueueIF* queueToUse,
-		bool appendValidityBuffer):
-		appendValidityBuffer(appendValidityBuffer) {
-	if(owner == nullptr) {
-		printWarningOrError(sif::OutputTypes::OUT_WARNING,
-				"LocalDataPoolManager", HasReturnvaluesIF::RETURN_FAILED,
-				"Invalid supplied owner");
-		return;
-	}
-	this->owner = owner;
-	mutex = MutexFactory::instance()->createMutex();
-	if(mutex == nullptr) {
-		printWarningOrError(sif::OutputTypes::OUT_ERROR,
-				"LocalDataPoolManager", HasReturnvaluesIF::RETURN_FAILED,
-				"Could not create mutex");
-	}
+        bool appendValidityBuffer):
+		        appendValidityBuffer(appendValidityBuffer) {
+    if(owner == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "LocalDataPoolManager", HasReturnvaluesIF::RETURN_FAILED,
+                "Invalid supplied owner");
+        return;
+    }
+    this->owner = owner;
+    mutex = MutexFactory::instance()->createMutex();
+    if(mutex == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                "LocalDataPoolManager", HasReturnvaluesIF::RETURN_FAILED,
+                "Could not create mutex");
+    }
 
-	hkQueue = queueToUse;
+    hkQueue = queueToUse;
 }
 
 LocalDataPoolManager::~LocalDataPoolManager() {
@@ -46,898 +46,903 @@ LocalDataPoolManager::~LocalDataPoolManager() {
 }
 
 ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) {
-	if(queueToUse == nullptr) {
-		/* Error, all destinations invalid */
-		printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
-		        QUEUE_OR_DESTINATION_INVALID);
-	}
-	hkQueue = queueToUse;
+    if(queueToUse == nullptr) {
+        /* Error, all destinations invalid */
+        printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
+                QUEUE_OR_DESTINATION_INVALID);
+    }
+    hkQueue = queueToUse;
 
-	ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
-	if(ipcStore == nullptr) {
-		/* Error, all destinations invalid */
-		printWarningOrError(sif::OutputTypes::OUT_ERROR,
-				"initialize", HasReturnvaluesIF::RETURN_FAILED,
-				"Could not set IPC store.");
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
+    ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+    if(ipcStore == nullptr) {
+        /* Error, all destinations invalid */
+        printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                "initialize", HasReturnvaluesIF::RETURN_FAILED,
+                "Could not set IPC store.");
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 
 
-	if(defaultHkDestination != objects::NO_OBJECT) {
-		AcceptsHkPacketsIF* hkPacketReceiver = ObjectManager::instance()->
-		        get<AcceptsHkPacketsIF>(defaultHkDestination);
-		if(hkPacketReceiver != nullptr) {
-			hkDestinationId = hkPacketReceiver->getHkQueue();
-		}
-		else {
-			printWarningOrError(sif::OutputTypes::OUT_ERROR,
-					"initialize", QUEUE_OR_DESTINATION_INVALID);
-			return QUEUE_OR_DESTINATION_INVALID;
-		}
-	}
+    if(defaultHkDestination != objects::NO_OBJECT) {
+        AcceptsHkPacketsIF* hkPacketReceiver = ObjectManager::instance()->
+                get<AcceptsHkPacketsIF>(defaultHkDestination);
+        if(hkPacketReceiver != nullptr) {
+            hkDestinationId = hkPacketReceiver->getHkQueue();
+        }
+        else {
+            printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                    "initialize", QUEUE_OR_DESTINATION_INVALID);
+            return QUEUE_OR_DESTINATION_INVALID;
+        }
+    }
 
-	return HasReturnvaluesIF::RETURN_OK;
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::initializeAfterTaskCreation(
-		uint8_t nonDiagInvlFactor) {
-	setNonDiagnosticIntervalFactor(nonDiagInvlFactor);
-	return initializeHousekeepingPoolEntriesOnce();
+        uint8_t nonDiagInvlFactor) {
+    setNonDiagnosticIntervalFactor(nonDiagInvlFactor);
+    return initializeHousekeepingPoolEntriesOnce();
 }
 
 ReturnValue_t LocalDataPoolManager::initializeHousekeepingPoolEntriesOnce() {
-	if(not mapInitialized) {
-		ReturnValue_t result = owner->initializeLocalDataPool(localPoolMap,
-				*this);
-		if(result == HasReturnvaluesIF::RETURN_OK) {
-			mapInitialized = true;
-		}
-		return result;
-	}
+    if(not mapInitialized) {
+        ReturnValue_t result = owner->initializeLocalDataPool(localPoolMap,
+                *this);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            mapInitialized = true;
+        }
+        return result;
+    }
 
-	printWarningOrError(sif::OutputTypes::OUT_WARNING,
-			"initialize", HasReturnvaluesIF::RETURN_FAILED,
-			"The map should only be initialized once");
-	return HasReturnvaluesIF::RETURN_OK;
+    printWarningOrError(sif::OutputTypes::OUT_WARNING,
+            "initialize", HasReturnvaluesIF::RETURN_FAILED,
+            "The map should only be initialized once");
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::performHkOperation() {
-	ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
-	for(auto& receiver: hkReceivers) {
-		switch(receiver.reportingType) {
-		case(ReportingType::PERIODIC): {
-			if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
-				/* Periodic packets shall only be generated from datasets */
-				continue;
-			}
-			performPeriodicHkGeneration(receiver);
-			break;
-		}
-		case(ReportingType::UPDATE_HK): {
-			handleHkUpdate(receiver, status);
-			break;
-		}
-		case(ReportingType::UPDATE_NOTIFICATION): {
-			handleNotificationUpdate(receiver, status);
-			break;
-		}
-		case(ReportingType::UPDATE_SNAPSHOT): {
-			handleNotificationSnapshot(receiver, status);
-			break;
-		}
-		default:
-			// This should never happen.
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-	}
-	resetHkUpdateResetHelper();
-	return status;
+    ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
+    for(auto& receiver: hkReceivers) {
+        switch(receiver.reportingType) {
+        case(ReportingType::PERIODIC): {
+            if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
+                /* Periodic packets shall only be generated from datasets */
+                continue;
+            }
+            performPeriodicHkGeneration(receiver);
+            break;
+        }
+        case(ReportingType::UPDATE_HK): {
+            handleHkUpdate(receiver, status);
+            break;
+        }
+        case(ReportingType::UPDATE_NOTIFICATION): {
+            handleNotificationUpdate(receiver, status);
+            break;
+        }
+        case(ReportingType::UPDATE_SNAPSHOT): {
+            handleNotificationSnapshot(receiver, status);
+            break;
+        }
+        default:
+            // This should never happen.
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+    }
+    resetHkUpdateResetHelper();
+    return status;
 }
 
 ReturnValue_t LocalDataPoolManager::handleHkUpdate(HkReceiver& receiver,
-		ReturnValue_t& status) {
-	if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
-		/* Update packets shall only be generated from datasets. */
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
-			receiver.dataId.sid);
-	if(dataSet == nullptr) {
+        ReturnValue_t& status) {
+    if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
+        /* Update packets shall only be generated from datasets. */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
+            receiver.dataId.sid);
+    if(dataSet == nullptr) {
         return DATASET_NOT_FOUND;
-	}
-	if(dataSet->hasChanged()) {
-		/* Prepare and send update notification */
-		ReturnValue_t result = generateHousekeepingPacket(
-				receiver.dataId.sid, dataSet, true);
-		if(result != HasReturnvaluesIF::RETURN_OK) {
-			status = result;
-		}
-	}
-	handleChangeResetLogic(receiver.dataType, receiver.dataId,
-			dataSet);
-	return HasReturnvaluesIF::RETURN_OK;
+    }
+    if(dataSet->hasChanged()) {
+        /* Prepare and send update notification */
+        ReturnValue_t result = generateHousekeepingPacket(
+                receiver.dataId.sid, dataSet, true);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            status = result;
+        }
+    }
+    handleChangeResetLogic(receiver.dataType, receiver.dataId,
+            dataSet);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::handleNotificationUpdate(HkReceiver& receiver,
-		ReturnValue_t& status) {
-	MarkChangedIF* toReset = nullptr;
-	if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
-		LocalPoolObjectBase* poolObj = HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
-				receiver.dataId.localPoolId);
-		if(poolObj == nullptr) {
-			printWarningOrError(sif::OutputTypes::OUT_WARNING,
-					"handleNotificationUpdate", POOLOBJECT_NOT_FOUND);
-			return POOLOBJECT_NOT_FOUND;
-		}
-		if(poolObj->hasChanged()) {
-			/* Prepare and send update notification. */
-			CommandMessage notification;
-			HousekeepingMessage::setUpdateNotificationVariableCommand(&notification,
-			        gp_id_t(owner->getObjectId(), receiver.dataId.localPoolId));
-			ReturnValue_t result = hkQueue->sendMessage(receiver.destinationQueue, &notification);
-			if(result != HasReturnvaluesIF::RETURN_OK) {
-				status = result;
-			}
-			toReset = poolObj;
-		}
+        ReturnValue_t& status) {
+    MarkChangedIF* toReset = nullptr;
+    if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
+        LocalPoolObjectBase* poolObj = HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
+                receiver.dataId.localPoolId);
+        if(poolObj == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "handleNotificationUpdate", POOLOBJECT_NOT_FOUND);
+            return POOLOBJECT_NOT_FOUND;
+        }
+        if(poolObj->hasChanged()) {
+            /* Prepare and send update notification. */
+            CommandMessage notification;
+            HousekeepingMessage::setUpdateNotificationVariableCommand(&notification,
+                    gp_id_t(owner->getObjectId(), receiver.dataId.localPoolId));
+            ReturnValue_t result = hkQueue->sendMessage(receiver.destinationQueue, &notification);
+            if(result != HasReturnvaluesIF::RETURN_OK) {
+                status = result;
+            }
+            toReset = poolObj;
+        }
 
-	}
-	else {
-		LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
-				receiver.dataId.sid);
-		if(dataSet == nullptr) {
-			printWarningOrError(sif::OutputTypes::OUT_WARNING,
-					"handleNotificationUpdate", DATASET_NOT_FOUND);
-			return DATASET_NOT_FOUND;
-		}
-		if(dataSet->hasChanged()) {
-			/* Prepare and send update notification */
-			CommandMessage notification;
-			HousekeepingMessage::setUpdateNotificationSetCommand(&notification,
-			        receiver.dataId.sid);
-			ReturnValue_t result = hkQueue->sendMessage(
-					receiver.destinationQueue, &notification);
-			if(result != HasReturnvaluesIF::RETURN_OK) {
-				status = result;
-			}
-			toReset = dataSet;
-		}
-	}
-	if(toReset != nullptr) {
-		handleChangeResetLogic(receiver.dataType, receiver.dataId, toReset);
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+    }
+    else {
+        LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
+                receiver.dataId.sid);
+        if(dataSet == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "handleNotificationUpdate", DATASET_NOT_FOUND);
+            return DATASET_NOT_FOUND;
+        }
+        if(dataSet->hasChanged()) {
+            /* Prepare and send update notification */
+            CommandMessage notification;
+            HousekeepingMessage::setUpdateNotificationSetCommand(&notification,
+                    receiver.dataId.sid);
+            ReturnValue_t result = hkQueue->sendMessage(
+                    receiver.destinationQueue, &notification);
+            if(result != HasReturnvaluesIF::RETURN_OK) {
+                status = result;
+            }
+            toReset = dataSet;
+        }
+    }
+    if(toReset != nullptr) {
+        handleChangeResetLogic(receiver.dataType, receiver.dataId, toReset);
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(
-		HkReceiver& receiver, ReturnValue_t& status) {
-	MarkChangedIF* toReset = nullptr;
-	/* Check whether data has changed and send messages in case it has */
-	if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
-		LocalPoolObjectBase* poolObj = HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
-				receiver.dataId.localPoolId);
-		if(poolObj == nullptr) {
-			printWarningOrError(sif::OutputTypes::OUT_WARNING,
-					"handleNotificationSnapshot", POOLOBJECT_NOT_FOUND);
-			return POOLOBJECT_NOT_FOUND;
-		}
+        HkReceiver& receiver, ReturnValue_t& status) {
+    MarkChangedIF* toReset = nullptr;
+    /* Check whether data has changed and send messages in case it has */
+    if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
+        LocalPoolObjectBase* poolObj = HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
+                receiver.dataId.localPoolId);
+        if(poolObj == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "handleNotificationSnapshot", POOLOBJECT_NOT_FOUND);
+            return POOLOBJECT_NOT_FOUND;
+        }
 
-		if (not poolObj->hasChanged()) {
-			return HasReturnvaluesIF::RETURN_OK;
-		}
+        if (not poolObj->hasChanged()) {
+            return HasReturnvaluesIF::RETURN_OK;
+        }
 
-		/* Prepare and send update snapshot */
-		timeval now;
-		Clock::getClock_timeval(&now);
-		CCSDSTime::CDS_short cds;
-		CCSDSTime::convertToCcsds(&cds, &now);
-		HousekeepingSnapshot updatePacket(reinterpret_cast<uint8_t*>(&cds), sizeof(cds),
-		        HasLocalDpIFManagerAttorney::getPoolObjectHandle(
-		                owner,receiver.dataId.localPoolId));
+        /* Prepare and send update snapshot */
+        timeval now;
+        Clock::getClock_timeval(&now);
+        CCSDSTime::CDS_short cds;
+        CCSDSTime::convertToCcsds(&cds, &now);
+        HousekeepingSnapshot updatePacket(reinterpret_cast<uint8_t*>(&cds), sizeof(cds),
+                HasLocalDpIFManagerAttorney::getPoolObjectHandle(
+                        owner,receiver.dataId.localPoolId));
 
-		store_address_t storeId;
-		ReturnValue_t result = addUpdateToStore(updatePacket, storeId);
-		if(result != HasReturnvaluesIF::RETURN_OK) {
-			return result;
-		}
+        store_address_t storeId;
+        ReturnValue_t result = addUpdateToStore(updatePacket, storeId);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
 
-		CommandMessage notification;
-		HousekeepingMessage::setUpdateSnapshotVariableCommand(&notification,
-				gp_id_t(owner->getObjectId(), receiver.dataId.localPoolId), storeId);
-		result = hkQueue->sendMessage(receiver.destinationQueue,
-				&notification);
-		if (result != HasReturnvaluesIF::RETURN_OK) {
-			status = result;
-		}
-		toReset = poolObj;
-	}
-	else {
-		LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
-				receiver.dataId.sid);
-		if(dataSet == nullptr) {
-			printWarningOrError(sif::OutputTypes::OUT_WARNING,
-					"handleNotificationSnapshot", DATASET_NOT_FOUND);
-			return DATASET_NOT_FOUND;
-		}
+        CommandMessage notification;
+        HousekeepingMessage::setUpdateSnapshotVariableCommand(&notification,
+                gp_id_t(owner->getObjectId(), receiver.dataId.localPoolId), storeId);
+        result = hkQueue->sendMessage(receiver.destinationQueue,
+                &notification);
+        if (result != HasReturnvaluesIF::RETURN_OK) {
+            status = result;
+        }
+        toReset = poolObj;
+    }
+    else {
+        LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
+                receiver.dataId.sid);
+        if(dataSet == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "handleNotificationSnapshot", DATASET_NOT_FOUND);
+            return DATASET_NOT_FOUND;
+        }
 
-		if(not dataSet->hasChanged()) {
-			return HasReturnvaluesIF::RETURN_OK;
-		}
+        if(not dataSet->hasChanged()) {
+            return HasReturnvaluesIF::RETURN_OK;
+        }
 
-		/* Prepare and send update snapshot */
-		timeval now;
-		Clock::getClock_timeval(&now);
-		CCSDSTime::CDS_short cds;
-		CCSDSTime::convertToCcsds(&cds, &now);
-		HousekeepingSnapshot updatePacket(reinterpret_cast<uint8_t*>(&cds),
-				sizeof(cds), HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
-						receiver.dataId.sid));
+        /* Prepare and send update snapshot */
+        timeval now;
+        Clock::getClock_timeval(&now);
+        CCSDSTime::CDS_short cds;
+        CCSDSTime::convertToCcsds(&cds, &now);
+        HousekeepingSnapshot updatePacket(reinterpret_cast<uint8_t*>(&cds),
+                sizeof(cds), HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
+                        receiver.dataId.sid));
 
-		store_address_t storeId;
-		ReturnValue_t result = addUpdateToStore(updatePacket, storeId);
-		if(result != HasReturnvaluesIF::RETURN_OK) {
-			return result;
-		}
+        store_address_t storeId;
+        ReturnValue_t result = addUpdateToStore(updatePacket, storeId);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
 
-		CommandMessage notification;
-		HousekeepingMessage::setUpdateSnapshotSetCommand(
-				&notification, receiver.dataId.sid, storeId);
-		result = hkQueue->sendMessage(receiver.destinationQueue, &notification);
-		if(result != HasReturnvaluesIF::RETURN_OK) {
-			status = result;
-		}
-		toReset = dataSet;
+        CommandMessage notification;
+        HousekeepingMessage::setUpdateSnapshotSetCommand(
+                &notification, receiver.dataId.sid, storeId);
+        result = hkQueue->sendMessage(receiver.destinationQueue, &notification);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            status = result;
+        }
+        toReset = dataSet;
 
-	}
-	if(toReset != nullptr) {
-		handleChangeResetLogic(receiver.dataType,
-				receiver.dataId, toReset);
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+    }
+    if(toReset != nullptr) {
+        handleChangeResetLogic(receiver.dataType,
+                receiver.dataId, toReset);
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::addUpdateToStore(
-		HousekeepingSnapshot& updatePacket, store_address_t& storeId) {
-	size_t updatePacketSize = updatePacket.getSerializedSize();
-	uint8_t *storePtr = nullptr;
-	ReturnValue_t result = ipcStore->getFreeElement(&storeId,
-			updatePacket.getSerializedSize(), &storePtr);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-	size_t serializedSize = 0;
-	result = updatePacket.serialize(&storePtr, &serializedSize,
-			updatePacketSize, SerializeIF::Endianness::MACHINE);
-	return result;;
+        HousekeepingSnapshot& updatePacket, store_address_t& storeId) {
+    size_t updatePacketSize = updatePacket.getSerializedSize();
+    uint8_t *storePtr = nullptr;
+    ReturnValue_t result = ipcStore->getFreeElement(&storeId,
+            updatePacket.getSerializedSize(), &storePtr);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    size_t serializedSize = 0;
+    result = updatePacket.serialize(&storePtr, &serializedSize,
+            updatePacketSize, SerializeIF::Endianness::MACHINE);
+    return result;;
 }
 
 void LocalDataPoolManager::handleChangeResetLogic(
-		DataType type, DataId dataId, MarkChangedIF* toReset) {
-	if(hkUpdateResetList == nullptr) {
-		/* Config error */
-		return;
-	}
-	HkUpdateResetList& listRef = *hkUpdateResetList;
-	for(auto& changeInfo: listRef) {
-		if(changeInfo.dataType != type) {
-			continue;
-		}
-		if((changeInfo.dataType == DataType::DATA_SET) and
-				(changeInfo.dataId.sid != dataId.sid)) {
-			continue;
-		}
-		if((changeInfo.dataType == DataType::LOCAL_POOL_VARIABLE) and
-				(changeInfo.dataId.localPoolId != dataId.localPoolId)) {
-			continue;
-		}
+        DataType type, DataId dataId, MarkChangedIF* toReset) {
+    if(hkUpdateResetList == nullptr) {
+        /* Config error */
+        return;
+    }
+    HkUpdateResetList& listRef = *hkUpdateResetList;
+    for(auto& changeInfo: listRef) {
+        if(changeInfo.dataType != type) {
+            continue;
+        }
+        if((changeInfo.dataType == DataType::DATA_SET) and
+                (changeInfo.dataId.sid != dataId.sid)) {
+            continue;
+        }
+        if((changeInfo.dataType == DataType::LOCAL_POOL_VARIABLE) and
+                (changeInfo.dataId.localPoolId != dataId.localPoolId)) {
+            continue;
+        }
 
-		/* Only one update recipient, we can reset changes status immediately */
-		if(changeInfo.updateCounter <= 1) {
-			toReset->setChanged(false);
-		}
-		/* All recipients have been notified, reset the changed flag */
-		else if(changeInfo.currentUpdateCounter <= 1) {
-			toReset->setChanged(false);
-			changeInfo.currentUpdateCounter = 0;
-		}
-		/* Not all recipiens have been notified yet, decrement */
-		else {
-			changeInfo.currentUpdateCounter--;
-		}
-		return;
-	}
+        /* Only one update recipient, we can reset changes status immediately */
+        if(changeInfo.updateCounter <= 1) {
+            toReset->setChanged(false);
+        }
+        /* All recipients have been notified, reset the changed flag */
+        else if(changeInfo.currentUpdateCounter <= 1) {
+            toReset->setChanged(false);
+            changeInfo.currentUpdateCounter = 0;
+        }
+        /* Not all recipiens have been notified yet, decrement */
+        else {
+            changeInfo.currentUpdateCounter--;
+        }
+        return;
+    }
 }
 
 void LocalDataPoolManager::resetHkUpdateResetHelper() {
-	if(hkUpdateResetList == nullptr) {
-		return;
-	}
+    if(hkUpdateResetList == nullptr) {
+        return;
+    }
 
-	for(auto& changeInfo: *hkUpdateResetList) {
-		changeInfo.currentUpdateCounter = changeInfo.updateCounter;
-	}
+    for(auto& changeInfo: *hkUpdateResetList) {
+        changeInfo.currentUpdateCounter = changeInfo.updateCounter;
+    }
 }
 
 ReturnValue_t LocalDataPoolManager::subscribeForPeriodicPacket(sid_t sid,
-		bool enableReporting, float collectionInterval, bool isDiagnostics,
-		object_id_t packetDestination) {
-	AcceptsHkPacketsIF* hkReceiverObject = ObjectManager::instance()->
-	        get<AcceptsHkPacketsIF>(packetDestination);
-	if(hkReceiverObject == nullptr) {
-		printWarningOrError(sif::OutputTypes::OUT_WARNING,
-				"subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
-		return QUEUE_OR_DESTINATION_INVALID;
-	}
+        bool enableReporting, float collectionInterval, bool isDiagnostics,
+        object_id_t packetDestination) {
+    AcceptsHkPacketsIF* hkReceiverObject = ObjectManager::instance()->
+            get<AcceptsHkPacketsIF>(packetDestination);
+    if(hkReceiverObject == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
+    }
 
-	struct HkReceiver hkReceiver;
-	hkReceiver.dataId.sid = sid;
-	hkReceiver.reportingType = ReportingType::PERIODIC;
-	hkReceiver.dataType = DataType::DATA_SET;
-	hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
+    struct HkReceiver hkReceiver;
+    hkReceiver.dataId.sid = sid;
+    hkReceiver.reportingType = ReportingType::PERIODIC;
+    hkReceiver.dataType = DataType::DATA_SET;
+    hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
 
-	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
-	if(dataSet != nullptr) {
-		LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enableReporting);
-		LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
-		LocalPoolDataSetAttorney::initializePeriodicHelper(*dataSet, collectionInterval,
-				owner->getPeriodicOperationFrequency());
-	}
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet != nullptr) {
+        LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enableReporting);
+        LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
+        LocalPoolDataSetAttorney::initializePeriodicHelper(*dataSet, collectionInterval,
+                owner->getPeriodicOperationFrequency());
+    }
 
-	hkReceivers.push_back(hkReceiver);
-	return HasReturnvaluesIF::RETURN_OK;
+    hkReceivers.push_back(hkReceiver);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 
 ReturnValue_t LocalDataPoolManager::subscribeForUpdatePacket(sid_t sid,
-		bool isDiagnostics, bool reportingEnabled,
-		object_id_t packetDestination) {
-	AcceptsHkPacketsIF* hkReceiverObject =
-	        ObjectManager::instance()->get<AcceptsHkPacketsIF>(packetDestination);
-	if(hkReceiverObject == nullptr) {
-		printWarningOrError(sif::OutputTypes::OUT_WARNING,
-				"subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
-		return QUEUE_OR_DESTINATION_INVALID;
-	}
+        bool isDiagnostics, bool reportingEnabled,
+        object_id_t packetDestination) {
+    AcceptsHkPacketsIF* hkReceiverObject =
+            ObjectManager::instance()->get<AcceptsHkPacketsIF>(packetDestination);
+    if(hkReceiverObject == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
+    }
 
-	struct HkReceiver hkReceiver;
-	hkReceiver.dataId.sid = sid;
-	hkReceiver.reportingType = ReportingType::UPDATE_HK;
-	hkReceiver.dataType = DataType::DATA_SET;
-	hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
+    struct HkReceiver hkReceiver;
+    hkReceiver.dataId.sid = sid;
+    hkReceiver.reportingType = ReportingType::UPDATE_HK;
+    hkReceiver.dataType = DataType::DATA_SET;
+    hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
 
-	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
-	if(dataSet != nullptr) {
-		LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, true);
-		LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
-	}
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet != nullptr) {
+        LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, true);
+        LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
+    }
 
-	hkReceivers.push_back(hkReceiver);
+    hkReceivers.push_back(hkReceiver);
 
-	handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
-	return HasReturnvaluesIF::RETURN_OK;
+    handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::subscribeForSetUpdateMessage(
-		const uint32_t setId, object_id_t destinationObject,
-		MessageQueueId_t targetQueueId, bool generateSnapshot) {
-	struct HkReceiver hkReceiver;
-	hkReceiver.dataType = DataType::DATA_SET;
-	hkReceiver.dataId.sid = sid_t(owner->getObjectId(), setId);
-	hkReceiver.destinationQueue = targetQueueId;
-	hkReceiver.objectId = destinationObject;
-	if(generateSnapshot) {
-		hkReceiver.reportingType = ReportingType::UPDATE_SNAPSHOT;
-	}
-	else {
-		hkReceiver.reportingType = ReportingType::UPDATE_NOTIFICATION;
-	}
+        const uint32_t setId, object_id_t destinationObject,
+        MessageQueueId_t targetQueueId, bool generateSnapshot) {
+    struct HkReceiver hkReceiver;
+    hkReceiver.dataType = DataType::DATA_SET;
+    hkReceiver.dataId.sid = sid_t(owner->getObjectId(), setId);
+    hkReceiver.destinationQueue = targetQueueId;
+    hkReceiver.objectId = destinationObject;
+    if(generateSnapshot) {
+        hkReceiver.reportingType = ReportingType::UPDATE_SNAPSHOT;
+    }
+    else {
+        hkReceiver.reportingType = ReportingType::UPDATE_NOTIFICATION;
+    }
 
-	hkReceivers.push_back(hkReceiver);
+    hkReceivers.push_back(hkReceiver);
 
-	handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
-	return HasReturnvaluesIF::RETURN_OK;
+    handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::subscribeForVariableUpdateMessage(
-		const lp_id_t localPoolId, object_id_t destinationObject,
-		MessageQueueId_t targetQueueId, bool generateSnapshot) {
-	struct HkReceiver hkReceiver;
-	hkReceiver.dataType = DataType::LOCAL_POOL_VARIABLE;
-	hkReceiver.dataId.localPoolId = localPoolId;
-	hkReceiver.destinationQueue = targetQueueId;
-	hkReceiver.objectId = destinationObject;
-	if(generateSnapshot) {
-		hkReceiver.reportingType = ReportingType::UPDATE_SNAPSHOT;
-	}
-	else {
-		hkReceiver.reportingType = ReportingType::UPDATE_NOTIFICATION;
-	}
+        const lp_id_t localPoolId, object_id_t destinationObject,
+        MessageQueueId_t targetQueueId, bool generateSnapshot) {
+    struct HkReceiver hkReceiver;
+    hkReceiver.dataType = DataType::LOCAL_POOL_VARIABLE;
+    hkReceiver.dataId.localPoolId = localPoolId;
+    hkReceiver.destinationQueue = targetQueueId;
+    hkReceiver.objectId = destinationObject;
+    if(generateSnapshot) {
+        hkReceiver.reportingType = ReportingType::UPDATE_SNAPSHOT;
+    }
+    else {
+        hkReceiver.reportingType = ReportingType::UPDATE_NOTIFICATION;
+    }
 
-	hkReceivers.push_back(hkReceiver);
+    hkReceivers.push_back(hkReceiver);
 
-	handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
-	return HasReturnvaluesIF::RETURN_OK;
+    handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void LocalDataPoolManager::handleHkUpdateResetListInsertion(DataType dataType,
-		DataId dataId) {
-	if(hkUpdateResetList == nullptr) {
-		hkUpdateResetList = new std::vector<struct HkUpdateResetHelper>();
-	}
+        DataId dataId) {
+    if(hkUpdateResetList == nullptr) {
+        hkUpdateResetList = new std::vector<struct HkUpdateResetHelper>();
+    }
 
-	for(auto& updateResetStruct: *hkUpdateResetList) {
-		if(dataType == DataType::DATA_SET) {
-			if(updateResetStruct.dataId.sid == dataId.sid) {
-				updateResetStruct.updateCounter++;
-				updateResetStruct.currentUpdateCounter++;
-				return;
-			}
-		}
-		else {
-			if(updateResetStruct.dataId.localPoolId == dataId.localPoolId) {
-				updateResetStruct.updateCounter++;
-				updateResetStruct.currentUpdateCounter++;
-				return;
-			}
-		}
+    for(auto& updateResetStruct: *hkUpdateResetList) {
+        if(dataType == DataType::DATA_SET) {
+            if(updateResetStruct.dataId.sid == dataId.sid) {
+                updateResetStruct.updateCounter++;
+                updateResetStruct.currentUpdateCounter++;
+                return;
+            }
+        }
+        else {
+            if(updateResetStruct.dataId.localPoolId == dataId.localPoolId) {
+                updateResetStruct.updateCounter++;
+                updateResetStruct.currentUpdateCounter++;
+                return;
+            }
+        }
 
-	}
-	HkUpdateResetHelper hkUpdateResetHelper;
-	hkUpdateResetHelper.currentUpdateCounter = 1;
-	hkUpdateResetHelper.updateCounter = 1;
-	hkUpdateResetHelper.dataType = dataType;
-	if(dataType == DataType::DATA_SET) {
-		hkUpdateResetHelper.dataId.sid = dataId.sid;
-	}
-	else {
-		hkUpdateResetHelper.dataId.localPoolId = dataId.localPoolId;
-	}
-	hkUpdateResetList->push_back(hkUpdateResetHelper);
+    }
+    HkUpdateResetHelper hkUpdateResetHelper;
+    hkUpdateResetHelper.currentUpdateCounter = 1;
+    hkUpdateResetHelper.updateCounter = 1;
+    hkUpdateResetHelper.dataType = dataType;
+    if(dataType == DataType::DATA_SET) {
+        hkUpdateResetHelper.dataId.sid = dataId.sid;
+    }
+    else {
+        hkUpdateResetHelper.dataId.localPoolId = dataId.localPoolId;
+    }
+    hkUpdateResetList->push_back(hkUpdateResetHelper);
 }
 
 ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(
-		CommandMessage* message) {
-	Command_t command = message->getCommand();
-	sid_t sid = HousekeepingMessage::getSid(message);
-	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
-	switch(command) {
-	// Houskeeping interface handling.
-	case(HousekeepingMessage::ENABLE_PERIODIC_DIAGNOSTICS_GENERATION): {
-		result = togglePeriodicGeneration(sid, true, true);
-		break;
-	}
+        CommandMessage* message) {
+    Command_t command = message->getCommand();
+    sid_t sid = HousekeepingMessage::getSid(message);
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    switch(command) {
+    // Houskeeping interface handling.
+    case(HousekeepingMessage::ENABLE_PERIODIC_DIAGNOSTICS_GENERATION): {
+        result = togglePeriodicGeneration(sid, true, true);
+        break;
+    }
 
-	case(HousekeepingMessage::DISABLE_PERIODIC_DIAGNOSTICS_GENERATION): {
-		result = togglePeriodicGeneration(sid, false, true);
-		break;
-	}
+    case(HousekeepingMessage::DISABLE_PERIODIC_DIAGNOSTICS_GENERATION): {
+        result = togglePeriodicGeneration(sid, false, true);
+        break;
+    }
 
-	case(HousekeepingMessage::ENABLE_PERIODIC_HK_REPORT_GENERATION): {
-		result = togglePeriodicGeneration(sid, true, false);
-		break;
-	}
+    case(HousekeepingMessage::ENABLE_PERIODIC_HK_REPORT_GENERATION): {
+        result = togglePeriodicGeneration(sid, true, false);
+        break;
+    }
 
-	case(HousekeepingMessage::DISABLE_PERIODIC_HK_REPORT_GENERATION): {
-		result = togglePeriodicGeneration(sid, false, false);
-		break;
-	}
+    case(HousekeepingMessage::DISABLE_PERIODIC_HK_REPORT_GENERATION): {
+        result = togglePeriodicGeneration(sid, false, false);
+        break;
+    }
 
-	case(HousekeepingMessage::REPORT_DIAGNOSTICS_REPORT_STRUCTURES): {
-		result = generateSetStructurePacket(sid, true);
-		if(result == HasReturnvaluesIF::RETURN_OK) {
-		    return result;
-		}
-		break;
-	}
-
-	case(HousekeepingMessage::REPORT_HK_REPORT_STRUCTURES): {
-		result = generateSetStructurePacket(sid, false);
+    case(HousekeepingMessage::REPORT_DIAGNOSTICS_REPORT_STRUCTURES): {
+        result = generateSetStructurePacket(sid, true);
         if(result == HasReturnvaluesIF::RETURN_OK) {
             return result;
         }
         break;
-	}
-	case(HousekeepingMessage::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL):
-	case(HousekeepingMessage::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL): {
-		float newCollIntvl = 0;
-		HousekeepingMessage::getCollectionIntervalModificationCommand(message,
-				&newCollIntvl);
-		if(command == HousekeepingMessage::
-				MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL) {
-			result = changeCollectionInterval(sid, newCollIntvl, true);
-		}
-		else {
-			result = changeCollectionInterval(sid, newCollIntvl, false);
-		}
-		break;
-	}
+    }
 
-	case(HousekeepingMessage::GENERATE_ONE_PARAMETER_REPORT):
-	case(HousekeepingMessage::GENERATE_ONE_DIAGNOSTICS_REPORT): {
-		LocalPoolDataSetBase* dataSet =HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
-		if(command == HousekeepingMessage::GENERATE_ONE_PARAMETER_REPORT
-				and LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
-			result = WRONG_HK_PACKET_TYPE;
-			break;
-		}
-		else if(command == HousekeepingMessage::GENERATE_ONE_DIAGNOSTICS_REPORT
-				and not LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
+    case(HousekeepingMessage::REPORT_HK_REPORT_STRUCTURES): {
+        result = generateSetStructurePacket(sid, false);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+        break;
+    }
+    case(HousekeepingMessage::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL):
+    case(HousekeepingMessage::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL): {
+        float newCollIntvl = 0;
+        HousekeepingMessage::getCollectionIntervalModificationCommand(message,
+                &newCollIntvl);
+        if(command == HousekeepingMessage::
+                MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL) {
+            result = changeCollectionInterval(sid, newCollIntvl, true);
+        }
+        else {
+            result = changeCollectionInterval(sid, newCollIntvl, false);
+        }
+        break;
+    }
+
+    case(HousekeepingMessage::GENERATE_ONE_PARAMETER_REPORT):
+    case(HousekeepingMessage::GENERATE_ONE_DIAGNOSTICS_REPORT): {
+        LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+        if(dataSet == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING, "handleHousekeepingMessage",
+                    DATASET_NOT_FOUND);
+            return DATASET_NOT_FOUND;
+        }
+        if(command == HousekeepingMessage::GENERATE_ONE_PARAMETER_REPORT
+                and LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
             result = WRONG_HK_PACKET_TYPE;
             break;
-		}
-		return generateHousekeepingPacket(HousekeepingMessage::getSid(message),
-				dataSet, true);
-	}
+        }
+        else if(command == HousekeepingMessage::GENERATE_ONE_DIAGNOSTICS_REPORT
+                and not LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
+            result = WRONG_HK_PACKET_TYPE;
+            break;
+        }
+        return generateHousekeepingPacket(HousekeepingMessage::getSid(message),
+                dataSet, true);
+    }
 
-	/* Notification handling */
-	case(HousekeepingMessage::UPDATE_NOTIFICATION_SET): {
-		owner->handleChangedDataset(sid);
-		return HasReturnvaluesIF::RETURN_OK;
-	}
-	case(HousekeepingMessage::UPDATE_NOTIFICATION_VARIABLE): {
-		gp_id_t globPoolId = HousekeepingMessage::getUpdateNotificationVariableCommand(message);
-		owner->handleChangedPoolVariable(globPoolId);
-		return HasReturnvaluesIF::RETURN_OK;
-	}
-	case(HousekeepingMessage::UPDATE_SNAPSHOT_SET): {
-		store_address_t storeId;
-		HousekeepingMessage::getUpdateSnapshotSetCommand(message, &storeId);
-		bool clearMessage = true;
-		owner->handleChangedDataset(sid, storeId, &clearMessage);
-		if(clearMessage) {
-	        message->clear();
-		}
-		return HasReturnvaluesIF::RETURN_OK;
-	}
-	case(HousekeepingMessage::UPDATE_SNAPSHOT_VARIABLE): {
-		store_address_t storeId;
-		gp_id_t globPoolId = HousekeepingMessage::getUpdateSnapshotVariableCommand(message,
-		        &storeId);
-		bool clearMessage = true;
-		owner->handleChangedPoolVariable(globPoolId, storeId, &clearMessage);
-		if(clearMessage) {
-	        message->clear();
-		}
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+    /* Notification handling */
+    case(HousekeepingMessage::UPDATE_NOTIFICATION_SET): {
+        owner->handleChangedDataset(sid);
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(HousekeepingMessage::UPDATE_NOTIFICATION_VARIABLE): {
+        gp_id_t globPoolId = HousekeepingMessage::getUpdateNotificationVariableCommand(message);
+        owner->handleChangedPoolVariable(globPoolId);
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(HousekeepingMessage::UPDATE_SNAPSHOT_SET): {
+        store_address_t storeId;
+        HousekeepingMessage::getUpdateSnapshotSetCommand(message, &storeId);
+        bool clearMessage = true;
+        owner->handleChangedDataset(sid, storeId, &clearMessage);
+        if(clearMessage) {
+            message->clear();
+        }
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(HousekeepingMessage::UPDATE_SNAPSHOT_VARIABLE): {
+        store_address_t storeId;
+        gp_id_t globPoolId = HousekeepingMessage::getUpdateSnapshotVariableCommand(message,
+                &storeId);
+        bool clearMessage = true;
+        owner->handleChangedPoolVariable(globPoolId, storeId, &clearMessage);
+        if(clearMessage) {
+            message->clear();
+        }
+        return HasReturnvaluesIF::RETURN_OK;
+    }
 
-	default:
-		return CommandMessageIF::UNKNOWN_COMMAND;
-	}
+    default:
+        return CommandMessageIF::UNKNOWN_COMMAND;
+    }
 
-	CommandMessage reply;
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		HousekeepingMessage::setHkRequestFailureReply(&reply, sid, result);
-	}
-	else {
-		HousekeepingMessage::setHkRequestSuccessReply(&reply, sid);
-	}
-	hkQueue->sendMessage(hkDestinationId, &reply);
-	return result;
+    CommandMessage reply;
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        HousekeepingMessage::setHkRequestFailureReply(&reply, sid, result);
+    }
+    else {
+        HousekeepingMessage::setHkRequestSuccessReply(&reply, sid);
+    }
+    hkQueue->sendMessage(hkDestinationId, &reply);
+    return result;
 }
 
 ReturnValue_t LocalDataPoolManager::printPoolEntry(
-		lp_id_t localPoolId) {
-	auto poolIter = localPoolMap.find(localPoolId);
-	if (poolIter == localPoolMap.end()) {
-		printWarningOrError(sif::OutputTypes::OUT_WARNING, "printPoolEntry",
-				localpool::POOL_ENTRY_NOT_FOUND);
-		return localpool::POOL_ENTRY_NOT_FOUND;
-	}
-	poolIter->second->print();
-	return HasReturnvaluesIF::RETURN_OK;
+        lp_id_t localPoolId) {
+    auto poolIter = localPoolMap.find(localPoolId);
+    if (poolIter == localPoolMap.end()) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING, "printPoolEntry",
+                localpool::POOL_ENTRY_NOT_FOUND);
+        return localpool::POOL_ENTRY_NOT_FOUND;
+    }
+    poolIter->second->print();
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 MutexIF* LocalDataPoolManager::getMutexHandle() {
-	return mutex;
+    return mutex;
 }
 
 HasLocalDataPoolIF* LocalDataPoolManager::getOwner() {
-	return owner;
+    return owner;
 }
 
 ReturnValue_t LocalDataPoolManager::generateHousekeepingPacket(sid_t sid,
-		LocalPoolDataSetBase* dataSet, bool forDownlink,
-		MessageQueueId_t destination) {
-	if(dataSet == nullptr) {
-		/* Configuration error. */
-		printWarningOrError(sif::OutputTypes::OUT_WARNING,
-				"generateHousekeepingPacket",
-				DATASET_NOT_FOUND);
-		return DATASET_NOT_FOUND;
-	}
+        LocalPoolDataSetBase* dataSet, bool forDownlink,
+        MessageQueueId_t destination) {
+    if(dataSet == nullptr) {
+        /* Configuration error. */
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "generateHousekeepingPacket",
+                DATASET_NOT_FOUND);
+        return DATASET_NOT_FOUND;
+    }
 
-	store_address_t storeId;
-	HousekeepingPacketDownlink hkPacket(sid, dataSet);
-	size_t serializedSize = 0;
-	ReturnValue_t result = serializeHkPacketIntoStore(hkPacket, storeId,
-			forDownlink, &serializedSize);
-	if(result != HasReturnvaluesIF::RETURN_OK or serializedSize == 0) {
-		return result;
-	}
+    store_address_t storeId;
+    HousekeepingPacketDownlink hkPacket(sid, dataSet);
+    size_t serializedSize = 0;
+    ReturnValue_t result = serializeHkPacketIntoStore(hkPacket, storeId,
+            forDownlink, &serializedSize);
+    if(result != HasReturnvaluesIF::RETURN_OK or serializedSize == 0) {
+        return result;
+    }
 
-	/* Now we set a HK message and send it the HK packet destination. */
-	CommandMessage hkMessage;
-	if(LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
-		HousekeepingMessage::setHkDiagnosticsReply(&hkMessage, sid, storeId);
-	}
-	else {
-		HousekeepingMessage::setHkReportReply(&hkMessage, sid, storeId);
-	}
+    /* Now we set a HK message and send it the HK packet destination. */
+    CommandMessage hkMessage;
+    if(LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
+        HousekeepingMessage::setHkDiagnosticsReply(&hkMessage, sid, storeId);
+    }
+    else {
+        HousekeepingMessage::setHkReportReply(&hkMessage, sid, storeId);
+    }
 
-	if(hkQueue == nullptr) {
-		/* Error, no queue available to send packet with. */
-		printWarningOrError(sif::OutputTypes::OUT_WARNING,
-				"generateHousekeepingPacket",
-				QUEUE_OR_DESTINATION_INVALID);
-		return QUEUE_OR_DESTINATION_INVALID;
-	}
-	if(destination == MessageQueueIF::NO_QUEUE) {
-		if(hkDestinationId == MessageQueueIF::NO_QUEUE) {
-			/* Error, all destinations invalid */
-			printWarningOrError(sif::OutputTypes::OUT_WARNING,
-					"generateHousekeepingPacket",
-					QUEUE_OR_DESTINATION_INVALID);
-		}
-		destination = hkDestinationId;
-	}
+    if(hkQueue == nullptr) {
+        /* Error, no queue available to send packet with. */
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "generateHousekeepingPacket",
+                QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
+    }
+    if(destination == MessageQueueIF::NO_QUEUE) {
+        if(hkDestinationId == MessageQueueIF::NO_QUEUE) {
+            /* Error, all destinations invalid */
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "generateHousekeepingPacket",
+                    QUEUE_OR_DESTINATION_INVALID);
+        }
+        destination = hkDestinationId;
+    }
 
-	return hkQueue->sendMessage(destination, &hkMessage);
+    return hkQueue->sendMessage(destination, &hkMessage);
 }
 
 ReturnValue_t LocalDataPoolManager::serializeHkPacketIntoStore(
-		HousekeepingPacketDownlink& hkPacket,
-		store_address_t& storeId, bool forDownlink,
-		size_t* serializedSize) {
-	uint8_t* dataPtr = nullptr;
-	const size_t maxSize = hkPacket.getSerializedSize();
-	ReturnValue_t result = ipcStore->getFreeElement(&storeId,
-			maxSize, &dataPtr);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
+        HousekeepingPacketDownlink& hkPacket,
+        store_address_t& storeId, bool forDownlink,
+        size_t* serializedSize) {
+    uint8_t* dataPtr = nullptr;
+    const size_t maxSize = hkPacket.getSerializedSize();
+    ReturnValue_t result = ipcStore->getFreeElement(&storeId,
+            maxSize, &dataPtr);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
 
-	if(forDownlink) {
-		return hkPacket.serialize(&dataPtr, serializedSize, maxSize,
-				SerializeIF::Endianness::BIG);
-	}
-	return hkPacket.serialize(&dataPtr, serializedSize, maxSize,
-			SerializeIF::Endianness::MACHINE);
+    if(forDownlink) {
+        return hkPacket.serialize(&dataPtr, serializedSize, maxSize,
+                SerializeIF::Endianness::BIG);
+    }
+    return hkPacket.serialize(&dataPtr, serializedSize, maxSize,
+            SerializeIF::Endianness::MACHINE);
 }
 
 void LocalDataPoolManager::setNonDiagnosticIntervalFactor(
-		uint8_t nonDiagInvlFactor) {
-	this->nonDiagnosticIntervalFactor = nonDiagInvlFactor;
+        uint8_t nonDiagInvlFactor) {
+    this->nonDiagnosticIntervalFactor = nonDiagInvlFactor;
 }
 
 void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
-	sid_t sid = receiver.dataId.sid;
-	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
-	if(dataSet == nullptr) {
-		printWarningOrError(sif::OutputTypes::OUT_WARNING,
-				"performPeriodicHkGeneration",
-				DATASET_NOT_FOUND);
-		return;
-	}
+    sid_t sid = receiver.dataId.sid;
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "performPeriodicHkGeneration",
+                DATASET_NOT_FOUND);
+        return;
+    }
 
-	if(not LocalPoolDataSetAttorney::getReportingEnabled(*dataSet)) {
-		return;
-	}
+    if(not LocalPoolDataSetAttorney::getReportingEnabled(*dataSet)) {
+        return;
+    }
 
-	PeriodicHousekeepingHelper* periodicHelper =
-			LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet);
+    PeriodicHousekeepingHelper* periodicHelper =
+            LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet);
 
-	if(periodicHelper == nullptr) {
-		/* Configuration error */
-		return;
-	}
+    if(periodicHelper == nullptr) {
+        /* Configuration error */
+        return;
+    }
 
-	if(not periodicHelper->checkOpNecessary()) {
-		return;
-	}
+    if(not periodicHelper->checkOpNecessary()) {
+        return;
+    }
 
-	ReturnValue_t result = generateHousekeepingPacket(
-			sid, dataSet, true);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		/* Configuration error */
+    ReturnValue_t result = generateHousekeepingPacket(
+            sid, dataSet, true);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        /* Configuration error */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "LocalDataPoolManager::performHkOperation: HK generation failed." <<
-		        std::endl;
+        sif::warning << "LocalDataPoolManager::performHkOperation: HK generation failed." <<
+                std::endl;
 #else
-		sif::printWarning("LocalDataPoolManager::performHkOperation: HK generation failed.\n");
+        sif::printWarning("LocalDataPoolManager::performHkOperation: HK generation failed.\n");
 #endif
-	}
+    }
 }
 
 
 ReturnValue_t LocalDataPoolManager::togglePeriodicGeneration(sid_t sid,
-		bool enable, bool isDiagnostics) {
-	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+        bool enable, bool isDiagnostics) {
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
     if(dataSet == nullptr) {
         printWarningOrError(sif::OutputTypes::OUT_WARNING, "togglePeriodicGeneration",
                 DATASET_NOT_FOUND);
         return DATASET_NOT_FOUND;
     }
 
-	if((LocalPoolDataSetAttorney::isDiagnostics(*dataSet) and not isDiagnostics) or
-			(not LocalPoolDataSetAttorney::isDiagnostics(*dataSet) and isDiagnostics)) {
-		return WRONG_HK_PACKET_TYPE;
-	}
+    if((LocalPoolDataSetAttorney::isDiagnostics(*dataSet) and not isDiagnostics) or
+            (not LocalPoolDataSetAttorney::isDiagnostics(*dataSet) and isDiagnostics)) {
+        return WRONG_HK_PACKET_TYPE;
+    }
 
-	if((LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and enable) or
-			(not LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and not enable)) {
-		return REPORTING_STATUS_UNCHANGED;
-	}
+    if((LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and enable) or
+            (not LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and not enable)) {
+        return REPORTING_STATUS_UNCHANGED;
+    }
 
-	LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enable);
-	return HasReturnvaluesIF::RETURN_OK;
+    LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enable);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::changeCollectionInterval(sid_t sid,
-		float newCollectionInterval, bool isDiagnostics) {
-	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
-	if(dataSet == nullptr) {
+        float newCollectionInterval, bool isDiagnostics) {
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet == nullptr) {
         printWarningOrError(sif::OutputTypes::OUT_WARNING, "changeCollectionInterval",
                 DATASET_NOT_FOUND);
-	    return DATASET_NOT_FOUND;
-	}
+        return DATASET_NOT_FOUND;
+    }
 
-	bool targetIsDiagnostics = LocalPoolDataSetAttorney::isDiagnostics(*dataSet);
-	if((targetIsDiagnostics and not isDiagnostics) or
-			(not targetIsDiagnostics and isDiagnostics)) {
-		return WRONG_HK_PACKET_TYPE;
-	}
+    bool targetIsDiagnostics = LocalPoolDataSetAttorney::isDiagnostics(*dataSet);
+    if((targetIsDiagnostics and not isDiagnostics) or
+            (not targetIsDiagnostics and isDiagnostics)) {
+        return WRONG_HK_PACKET_TYPE;
+    }
 
-	PeriodicHousekeepingHelper* periodicHelper =
-			LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet);
+    PeriodicHousekeepingHelper* periodicHelper =
+            LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet);
 
-	if(periodicHelper == nullptr) {
-		/* Configuration error, set might not have a corresponding pool manager */
-		return PERIODIC_HELPER_INVALID;
-	}
+    if(periodicHelper == nullptr) {
+        /* Configuration error, set might not have a corresponding pool manager */
+        return PERIODIC_HELPER_INVALID;
+    }
 
-	periodicHelper->changeCollectionInterval(newCollectionInterval);
-	return HasReturnvaluesIF::RETURN_OK;
+    periodicHelper->changeCollectionInterval(newCollectionInterval);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t LocalDataPoolManager::generateSetStructurePacket(sid_t sid,
-		bool isDiagnostics) {
-	/* Get and check dataset first. */
-	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
-	if(dataSet == nullptr) {
-		printWarningOrError(sif::OutputTypes::OUT_WARNING,
-				"performPeriodicHkGeneration", DATASET_NOT_FOUND);
-		return DATASET_NOT_FOUND;
-	}
+        bool isDiagnostics) {
+    /* Get and check dataset first. */
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "performPeriodicHkGeneration", DATASET_NOT_FOUND);
+        return DATASET_NOT_FOUND;
+    }
 
 
-	bool targetIsDiagnostics = LocalPoolDataSetAttorney::isDiagnostics(*dataSet);
-	if((targetIsDiagnostics and not isDiagnostics) or
-			(not targetIsDiagnostics and isDiagnostics)) {
-		return WRONG_HK_PACKET_TYPE;
-	}
+    bool targetIsDiagnostics = LocalPoolDataSetAttorney::isDiagnostics(*dataSet);
+    if((targetIsDiagnostics and not isDiagnostics) or
+            (not targetIsDiagnostics and isDiagnostics)) {
+        return WRONG_HK_PACKET_TYPE;
+    }
 
-	bool valid = dataSet->isValid();
-	bool reportingEnabled = LocalPoolDataSetAttorney::getReportingEnabled(*dataSet);
-	float collectionInterval = LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet)->
-			getCollectionIntervalInSeconds();
+    bool valid = dataSet->isValid();
+    bool reportingEnabled = LocalPoolDataSetAttorney::getReportingEnabled(*dataSet);
+    float collectionInterval = LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet)->
+            getCollectionIntervalInSeconds();
 
-	// Generate set packet which can be serialized.
-	HousekeepingSetPacket setPacket(sid,
-			reportingEnabled, valid, collectionInterval, dataSet);
-	size_t expectedSize = setPacket.getSerializedSize();
-	uint8_t* storePtr = nullptr;
-	store_address_t storeId;
-	ReturnValue_t result = ipcStore->getFreeElement(&storeId,
-			expectedSize,&storePtr);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		printWarningOrError(sif::OutputTypes::OUT_ERROR,
-				"generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED,
-				"Could not get free element from IPC store.");
-		return result;
-	}
+    // Generate set packet which can be serialized.
+    HousekeepingSetPacket setPacket(sid,
+            reportingEnabled, valid, collectionInterval, dataSet);
+    size_t expectedSize = setPacket.getSerializedSize();
+    uint8_t* storePtr = nullptr;
+    store_address_t storeId;
+    ReturnValue_t result = ipcStore->getFreeElement(&storeId,
+            expectedSize,&storePtr);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                "generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED,
+                "Could not get free element from IPC store.");
+        return result;
+    }
 
-	// Serialize set packet into store.
-	size_t size = 0;
-	result = setPacket.serialize(&storePtr, &size, expectedSize,
-			SerializeIF::Endianness::BIG);
-	if(expectedSize != size) {
-		printWarningOrError(sif::OutputTypes::OUT_WARNING,
-				"generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED,
-				"Expected size is not equal to serialized size");
-	}
+    // Serialize set packet into store.
+    size_t size = 0;
+    result = setPacket.serialize(&storePtr, &size, expectedSize,
+            SerializeIF::Endianness::BIG);
+    if(expectedSize != size) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED,
+                "Expected size is not equal to serialized size");
+    }
 
-	// Send structure reporting reply.
-	CommandMessage reply;
-	if(isDiagnostics) {
-		HousekeepingMessage::setDiagnosticsStuctureReportReply(&reply,
-				sid, storeId);
-	}
-	else {
-		HousekeepingMessage::setHkStuctureReportReply(&reply,
-				sid, storeId);
-	}
+    // Send structure reporting reply.
+    CommandMessage reply;
+    if(isDiagnostics) {
+        HousekeepingMessage::setDiagnosticsStuctureReportReply(&reply,
+                sid, storeId);
+    }
+    else {
+        HousekeepingMessage::setHkStuctureReportReply(&reply,
+                sid, storeId);
+    }
 
-	hkQueue->reply(&reply);
-	return result;
+    hkQueue->reply(&reply);
+    return result;
 }
 
 void LocalDataPoolManager::clearReceiversList() {
-	/* Clear the vector completely and releases allocated memory. */
-	HkReceivers().swap(hkReceivers);
-	/* Also clear the reset helper if it exists */
-	if(hkUpdateResetList != nullptr) {
-	    HkUpdateResetList().swap(*hkUpdateResetList);
-	}
+    /* Clear the vector completely and releases allocated memory. */
+    HkReceivers().swap(hkReceivers);
+    /* Also clear the reset helper if it exists */
+    if(hkUpdateResetList != nullptr) {
+        HkUpdateResetList().swap(*hkUpdateResetList);
+    }
 }
 
 MutexIF* LocalDataPoolManager::getLocalPoolMutex() {
-	return this->mutex;
+    return this->mutex;
 }
 
 object_id_t LocalDataPoolManager::getCreatorObjectId() const {
-	return owner->getObjectId();
+    return owner->getObjectId();
 }
 
 void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
-		const char* functionName, ReturnValue_t error, const char* errorPrint) {
+        const char* functionName, ReturnValue_t error, const char* errorPrint) {
 #if FSFW_VERBOSE_LEVEL >= 1
-	if(errorPrint == nullptr) {
-		if(error == DATASET_NOT_FOUND) {
-			errorPrint = "Dataset not found";
-		}
-		else if(error == POOLOBJECT_NOT_FOUND) {
-			errorPrint = "Pool Object not found";
-		}
-		else if(error == HasReturnvaluesIF::RETURN_FAILED) {
-			if(outputType == sif::OutputTypes::OUT_WARNING) {
-				errorPrint = "Generic Warning";
-			}
-			else {
-				errorPrint = "Generic error";
-			}
-		}
-		else if(error == QUEUE_OR_DESTINATION_INVALID) {
-			errorPrint = "Queue or destination not set";
-		}
-		else if(error == localpool::POOL_ENTRY_TYPE_CONFLICT) {
-			errorPrint = "Pool entry type conflict";
-		}
-		else if(error == localpool::POOL_ENTRY_NOT_FOUND) {
-			errorPrint = "Pool entry not found";
-		}
-		else {
-			errorPrint = "Unknown error";
-		}
-	}
-	object_id_t objectId = 0xffffffff;
-	if(owner != nullptr) {
-	    objectId = owner->getObjectId();
-	}
+    if(errorPrint == nullptr) {
+        if(error == DATASET_NOT_FOUND) {
+            errorPrint = "Dataset not found";
+        }
+        else if(error == POOLOBJECT_NOT_FOUND) {
+            errorPrint = "Pool Object not found";
+        }
+        else if(error == HasReturnvaluesIF::RETURN_FAILED) {
+            if(outputType == sif::OutputTypes::OUT_WARNING) {
+                errorPrint = "Generic Warning";
+            }
+            else {
+                errorPrint = "Generic error";
+            }
+        }
+        else if(error == QUEUE_OR_DESTINATION_INVALID) {
+            errorPrint = "Queue or destination not set";
+        }
+        else if(error == localpool::POOL_ENTRY_TYPE_CONFLICT) {
+            errorPrint = "Pool entry type conflict";
+        }
+        else if(error == localpool::POOL_ENTRY_NOT_FOUND) {
+            errorPrint = "Pool entry not found";
+        }
+        else {
+            errorPrint = "Unknown error";
+        }
+    }
+    object_id_t objectId = 0xffffffff;
+    if(owner != nullptr) {
+        objectId = owner->getObjectId();
+    }
 
-	if(outputType == sif::OutputTypes::OUT_WARNING) {
+    if(outputType == sif::OutputTypes::OUT_WARNING) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
-		        std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
-		        std::dec << std::setfill(' ') << std::endl;
+        sif::warning << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+                std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+                std::dec << std::setfill(' ') << std::endl;
 #else
-		sif::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
-				functionName, objectId, errorPrint);
+        sif::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
+                functionName, objectId, errorPrint);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-	}
-	else if(outputType == sif::OutputTypes::OUT_ERROR) {
+    }
+    else if(outputType == sif::OutputTypes::OUT_ERROR) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
-		        std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
-		        std::dec << std::setfill(' ') << std::endl;
+        sif::error << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+                std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+                std::dec << std::setfill(' ') << std::endl;
 #else
-		sif::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
-				functionName, objectId, errorPrint);
+        sif::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
+                functionName, objectId, errorPrint);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-	}
+    }
 #endif /* #if FSFW_VERBOSE_LEVEL >= 1 */
 }
 
 LocalDataPoolManager* LocalDataPoolManager::getPoolManagerHandle() {
-	return this;
+    return this;
 }
diff --git a/serviceinterface/ServiceInterfaceBuffer.cpp b/serviceinterface/ServiceInterfaceBuffer.cpp
index a6a2cb71..b85a43a4 100644
--- a/serviceinterface/ServiceInterfaceBuffer.cpp
+++ b/serviceinterface/ServiceInterfaceBuffer.cpp
@@ -35,7 +35,7 @@ ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string setMessage,
 		colorPrefix = sif::ANSI_COLOR_GREEN;
 	}
 	else if(setMessage.find("WARNING") != std::string::npos) {
-		colorPrefix = sif::ANSI_COLOR_YELLOW;
+		colorPrefix = sif::ANSI_COLOR_MAGENTA;
 	}
 	else if(setMessage.find("ERROR") != std::string::npos) {
 		colorPrefix = sif::ANSI_COLOR_RED;