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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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/157] 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 40b2979ce841261083884234da595cc1347b285c Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 8 Jun 2021 15:03:17 +0200
Subject: [PATCH 123/157] added generic file system message

---
 memory/CMakeLists.txt               |   8 +-
 memory/GenericFileSystemMessage.cpp | 132 ++++++++++++++++++++++++++++
 memory/GenericFileSystemMessage.h   | 113 ++++++++++++++++++++++++
 3 files changed, 249 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..d9355567
--- /dev/null
+++ b/memory/GenericFileSystemMessage.cpp
@@ -0,0 +1,132 @@
+#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
new file mode 100644
index 00000000..1a19a69b
--- /dev/null
+++ b/memory/GenericFileSystemMessage.h
@@ -0,0 +1,113 @@
+#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 fb7b059137ca873b405b95f8d3072f4ae85ce2ff Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Tue, 8 Jun 2021 15:13:49 +0200
Subject: [PATCH 124/157] added clear message

---
 ipc/CommandMessageCleaner.cpp       | 4 ++++
 memory/GenericFileSystemMessage.cpp | 4 ++++
 memory/GenericFileSystemMessage.h   | 2 ++
 3 files changed, 10 insertions(+)

diff --git a/ipc/CommandMessageCleaner.cpp b/ipc/CommandMessageCleaner.cpp
index 6a364069..29998124 100644
--- a/ipc/CommandMessageCleaner.cpp
+++ b/ipc/CommandMessageCleaner.cpp
@@ -1,5 +1,6 @@
 #include "CommandMessageCleaner.h"
 
+#include "../memory/GenericFileSystemMessage.h"
 #include "../devicehandlers/DeviceHandlerMessage.h"
 #include "../health/HealthMessage.h"
 #include "../memory/MemoryMessage.h"
@@ -42,6 +43,9 @@ void CommandMessageCleaner::clearCommandMessage(CommandMessage* message) {
 	case messagetypes::HOUSEKEEPING:
 	    HousekeepingMessage::clear(message);
 	    break;
+	case messagetypes::FILE_SYSTEM_MESSAGE:
+	    GenericFileSystemMessage::clear(message);
+	    break;
 	default:
 		messagetypes::clearMissionMessage(message);
 		break;
diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
index d9355567..e4edc7ac 100644
--- a/memory/GenericFileSystemMessage.cpp
+++ b/memory/GenericFileSystemMessage.cpp
@@ -130,3 +130,7 @@ bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
     }
     return message->getParameter();
 }
+
+ReturnValue_t GenericFileSystemMessage::clear(CommandMessage* message) {
+    return HasReturnvaluesIF::RETURN_OK;
+}
diff --git a/memory/GenericFileSystemMessage.h b/memory/GenericFileSystemMessage.h
index 1a19a69b..6351dab9 100644
--- a/memory/GenericFileSystemMessage.h
+++ b/memory/GenericFileSystemMessage.h
@@ -108,6 +108,8 @@ public:
     static ReturnValue_t getFailureReply(const CommandMessage* message,
             uint32_t* errorParam = nullptr);
 
+    static ReturnValue_t clear(CommandMessage* message);
+
 };
 
 #endif /* MISSION_MEMORY_GENERICFILESYSTEMMESSAGE_H_ */

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 125/157] 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 126/157] 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 127/157] 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;

From 537a30a4de78bd0f86a60a4b73cb861cd38c1a7e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 12:34:06 +0200
Subject: [PATCH 128/157] added pus a tc

---
 FSFW.h                          |  7 ++++++
 tmtcpacket/pus/CMakeLists.txt   |  3 ++-
 tmtcpacket/pus/TcPacketBase.h   | 25 ---------------------
 tmtcpacket/pus/TcPacketPusA.cpp |  3 +++
 tmtcpacket/pus/TcPacketPusA.h   | 40 +++++++++++++++++++++++++++++++++
 tmtcpacket/pus/TcPacketPusC.cpp |  4 ++++
 tmtcpacket/pus/TcPacketPusC.h   |  8 +++++++
 7 files changed, 64 insertions(+), 26 deletions(-)
 create mode 100644 FSFW.h
 create mode 100644 tmtcpacket/pus/TcPacketPusA.cpp
 create mode 100644 tmtcpacket/pus/TcPacketPusA.h
 create mode 100644 tmtcpacket/pus/TcPacketPusC.cpp
 create mode 100644 tmtcpacket/pus/TcPacketPusC.h

diff --git a/FSFW.h b/FSFW.h
new file mode 100644
index 00000000..df06ff3d
--- /dev/null
+++ b/FSFW.h
@@ -0,0 +1,7 @@
+#ifndef FSFW_FSFW_H_
+#define FSFW_FSFW_H_
+
+#include "FSFWConfig.h"
+
+
+#endif /* FSFW_FSFW_H_ */
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index cd4f49f1..2d22cb66 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -1,6 +1,7 @@
 target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
+	PRIVATE
 		TcPacketBase.cpp
+		TcPacketPusA.cpp
 		TcPacketStored.cpp
 		TmPacketBase.cpp
 		TmPacketMinimal.cpp
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h
index 582a2214..451f9c3a 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/TcPacketBase.h
@@ -4,31 +4,6 @@
 #include "../../tmtcpacket/SpacePacketBase.h"
 #include <cstddef>
 
-
-/**
- * This struct defines a byte-wise structured PUS TC Data Field Header.
- * Any optional fields in the header must be added or removed here.
- * Currently, the Source Id field is present with one byte.
- * @ingroup tmtcpackets
- */
-struct PUSTcDataFieldHeader {
-	uint8_t version_type_ack;
-	uint8_t service_type;
-	uint8_t service_subtype;
-	uint8_t source_id;
-};
-
-/**
- * This struct defines the data structure of a PUS Telecommand Packet when
- * accessed via a pointer.
- * @ingroup tmtcpackets
- */
-struct TcPacketPointer {
-	CCSDSPrimaryHeader primary;
-	PUSTcDataFieldHeader dataField;
-	uint8_t appData;
-};
-
 /**
  * This class is the basic data handler for any ECSS PUS Telecommand packet.
  *
diff --git a/tmtcpacket/pus/TcPacketPusA.cpp b/tmtcpacket/pus/TcPacketPusA.cpp
new file mode 100644
index 00000000..c9af38a6
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketPusA.cpp
@@ -0,0 +1,3 @@
+#include "TcPacketPusA.h"
+
+
diff --git a/tmtcpacket/pus/TcPacketPusA.h b/tmtcpacket/pus/TcPacketPusA.h
new file mode 100644
index 00000000..9254ad32
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketPusA.h
@@ -0,0 +1,40 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
+
+#include "ccsds_header.h"
+#include "TcPacketBase.h"
+
+#include <cstdint>
+
+/**
+ * This struct defines a byte-wise structured PUS TC A Data Field Header.
+ * Any optional fields in the header must be added or removed here.
+ * Currently, the Source Id field is present with one byte.
+ * @ingroup tmtcpackets
+ */
+struct PUSTcDataFieldHeader {
+    uint8_t version_type_ack;
+    uint8_t service_type;
+    uint8_t service_subtype;
+    uint8_t source_id;
+};
+
+/**
+ * This struct defines the data structure of a PUS Telecommand A packet when
+ * accessed via a pointer.
+ * @ingroup tmtcpackets
+ */
+struct TcPacketPointer {
+    CCSDSPrimaryHeader primary;
+    PUSTcDataFieldHeader dataField;
+    uint8_t appData;
+};
+
+
+class TcPacketPusA: public TcPacketBase {
+public:
+private:
+};
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_ */
diff --git a/tmtcpacket/pus/TcPacketPusC.cpp b/tmtcpacket/pus/TcPacketPusC.cpp
new file mode 100644
index 00000000..1f2946a8
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketPusC.cpp
@@ -0,0 +1,4 @@
+#include "TcPacketPusC.h"
+
+
+
diff --git a/tmtcpacket/pus/TcPacketPusC.h b/tmtcpacket/pus/TcPacketPusC.h
new file mode 100644
index 00000000..e965e752
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketPusC.h
@@ -0,0 +1,8 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_
+
+
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_ */

From 1b6759020aafce39adfbf4e99870f7e8552378b1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 12:38:24 +0200
Subject: [PATCH 129/157] added getter function

---
 tmtcpacket/pus/TcPacketBase.cpp | 4 ++++
 tmtcpacket/pus/TcPacketBase.h   | 7 +++++++
 2 files changed, 11 insertions(+)

diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp
index ca3e2a99..50130c83 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/TcPacketBase.cpp
@@ -23,6 +23,10 @@ uint8_t TcPacketBase::getSubService() {
 	return tcData->dataField.service_subtype;
 }
 
+uint8_t TcPacketBase::getSourceId() {
+    return tcData->dataField.source_id;
+}
+
 uint8_t TcPacketBase::getAcknowledgeFlags() {
 	return tcData->dataField.version_type_ack & 0b00001111;
 }
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h
index 582a2214..f5073eba 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/TcPacketBase.h
@@ -108,6 +108,13 @@ public:
 	 * @return	The packet's PUS Service Subtype.
 	 */
 	uint8_t getSubService();
+	/**
+	 * The source ID can be used to have an additional identifier, e.g. for different ground
+	 * station.
+	 * @return
+	 */
+	uint8_t getSourceId();
+
 	/**
 	 * This is a getter for a pointer to the packet's Application data.
 	 *

From d0f37b851b4de0fd71521cec5ff19ded413c2d3e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 16:29:13 +0200
Subject: [PATCH 130/157] added pus c support for tc

---
 defaultcfg/fsfwconfig/FSFWConfig.h     |  21 +++--
 tcdistribution/CMakeLists.txt          |  11 +--
 tcdistribution/PUSDistributor.cpp      |   7 +-
 tcdistribution/PUSDistributor.h        |   8 +-
 tcdistribution/TcPacketCheck.cpp       |  58 ++++++------
 tcdistribution/TcPacketCheck.h         |  84 ++++++++---------
 tmtcpacket/CMakeLists.txt              |   7 +-
 tmtcpacket/pus/CMakeLists.txt          |  25 ++---
 tmtcpacket/pus/TcPacketBase.cpp        |  82 ++--------------
 tmtcpacket/pus/TcPacketBase.h          |  53 ++++-------
 tmtcpacket/pus/TcPacketPusA.cpp        |  72 ++++++++++++++
 tmtcpacket/pus/TcPacketPusA.h          |  47 +++++++++-
 tmtcpacket/pus/TcPacketStored.cpp      | 124 -------------------------
 tmtcpacket/pus/TcPacketStored.h        | 122 ++----------------------
 tmtcpacket/pus/TcPacketStoredBase.cpp  |  72 ++++++++++++++
 tmtcpacket/pus/TcPacketStoredBase.h    |  90 ++++++++++++++++++
 tmtcpacket/pus/TcPacketStoredIF.h      |  38 ++++++++
 tmtcpacket/pus/TcPacketStoredPusA.cpp  |  77 +++++++++++++++
 tmtcpacket/pus/TcPacketStoredPusA.h    |  53 +++++++++++
 tmtcpacket/pus/TcPacketStoredPusC.cpp  |  10 ++
 tmtcpacket/pus/TmPacketBase.cpp        |  15 +--
 tmtcservices/CommandingServiceBase.cpp |  44 +++++----
 tmtcservices/CommandingServiceBase.h   |   7 +-
 tmtcservices/PusServiceBase.h          |   6 +-
 tmtcservices/VerificationReporter.cpp  |  22 +++--
 25 files changed, 667 insertions(+), 488 deletions(-)
 delete mode 100644 tmtcpacket/pus/TcPacketStored.cpp
 create mode 100644 tmtcpacket/pus/TcPacketStoredBase.cpp
 create mode 100644 tmtcpacket/pus/TcPacketStoredBase.h
 create mode 100644 tmtcpacket/pus/TcPacketStoredIF.h
 create mode 100644 tmtcpacket/pus/TcPacketStoredPusA.cpp
 create mode 100644 tmtcpacket/pus/TcPacketStoredPusA.h
 create mode 100644 tmtcpacket/pus/TcPacketStoredPusC.cpp

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index 1abdab4c..5518e33c 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -7,29 +7,30 @@
 //! Used to determine whether C++ ostreams are used which can increase
 //! the binary size significantly. If this is disabled,
 //! the C stdio functions can be used alternatively
-#define FSFW_CPP_OSTREAM_ENABLED 	1
+#define FSFW_CPP_OSTREAM_ENABLED 	    1
 
 //! More FSFW related printouts depending on level. Useful for development.
-#define FSFW_VERBOSE_LEVEL			1
+#define FSFW_VERBOSE_LEVEL			    1
 
 //! Can be used to completely disable printouts, even the C stdio ones.
 #if FSFW_CPP_OSTREAM_ENABLED == 0 && FSFW_VERBOSE_LEVEL == 0
-	#define FSFW_DISABLE_PRINTOUT 	0
+	#define FSFW_DISABLE_PRINTOUT 	    0
 #endif
 
-#define FSFW_USE_PUS_C_TELEMETRY    1
+#define FSFW_USE_PUS_C_TELEMETRY        1
+#define FSFW_USE_PUS_C_TELECOMMANDS     1
 
 //! Can be used to disable the ANSI color sequences for C stdio.
-#define FSFW_COLORED_OUTPUT 		1
+#define FSFW_COLORED_OUTPUT 		    1
 
 //! If FSFW_OBJ_EVENT_TRANSLATION is set to one,
 //! additional output which requires the translation files translateObjects
 //! and translateEvents (and their compiled source files)
-#define FSFW_OBJ_EVENT_TRANSLATION	0
+#define FSFW_OBJ_EVENT_TRANSLATION	    0
 
 #if FSFW_OBJ_EVENT_TRANSLATION == 1
 //! Specify whether info events are printed too.
-#define FSFW_DEBUG_INFO				1
+#define FSFW_DEBUG_INFO				    1
 #include "objects/translateObjects.h"
 #include "events/translateEvents.h"
 #else
@@ -37,17 +38,17 @@
 
 //! When using the newlib nano library, C99 support for stdio facilities
 //! will not be provided. This define should be set to 1 if this is the case.
-#define FSFW_NO_C99_IO 	            1
+#define FSFW_NO_C99_IO 	                1
 
 //! Specify whether a special mode store is used for Subsystem components.
-#define FSFW_USE_MODESTORE          0
+#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
+#define FSFW_USE_REALTIME_FOR_LINUX     1
 
 namespace fsfwconfig {
 
diff --git a/tcdistribution/CMakeLists.txt b/tcdistribution/CMakeLists.txt
index 17dc186c..6f237076 100644
--- a/tcdistribution/CMakeLists.txt
+++ b/tcdistribution/CMakeLists.txt
@@ -1,7 +1,6 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		CCSDSDistributor.cpp
-		PUSDistributor.cpp
-		TcDistributor.cpp
-		TcPacketCheck.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    CCSDSDistributor.cpp
+    PUSDistributor.cpp
+    TcDistributor.cpp
+    TcPacketCheck.cpp
 )
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 0fac9ba0..5c2f734b 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -3,7 +3,6 @@
 
 #include "../objectmanager/ObjectManager.h"
 #include "../serviceinterface/ServiceInterface.h"
-#include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
 #define PUS_DISTRIBUTOR_DEBUGGING     0
@@ -119,7 +118,11 @@ uint16_t PUSDistributor::getIdentifier() {
 }
 
 ReturnValue_t PUSDistributor::initialize() {
-    currentPacket = new TcPacketStored();
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    currentPacket = new TcPacketStoredPusC();
+#else
+    currentPacket = new TcPacketStoredPusA();
+#endif
     if(currentPacket == nullptr) {
         // Should not happen, memory allocation failed!
         return ObjectManagerIF::CHILD_INIT_FAILED;
diff --git a/tcdistribution/PUSDistributor.h b/tcdistribution/PUSDistributor.h
index be3804ef..0c288451 100644
--- a/tcdistribution/PUSDistributor.h
+++ b/tcdistribution/PUSDistributor.h
@@ -5,6 +5,7 @@
 #include "TcDistributor.h"
 #include "TcPacketCheck.h"
 
+#include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../tmtcservices/AcceptsTelecommandsIF.h"
 #include "../tmtcservices/VerificationReporter.h"
@@ -51,7 +52,12 @@ protected:
 	/**
 	 * The currently handled packet is stored here.
 	 */
-	TcPacketStored* currentPacket = nullptr;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+	TcPacketStoredPusC* currentPacket = nullptr;
+#else
+	TcPacketStoredPusA* currentPacket = nullptr;
+#endif
+
 	/**
 	 * With this variable, the current check status is stored to generate
 	 * acceptance messages later.
diff --git a/tcdistribution/TcPacketCheck.cpp b/tcdistribution/TcPacketCheck.cpp
index 38ed04aa..e88305cc 100644
--- a/tcdistribution/TcPacketCheck.cpp
+++ b/tcdistribution/TcPacketCheck.cpp
@@ -1,39 +1,45 @@
 #include "TcPacketCheck.h"
 
 #include "../globalfunctions/CRC.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../tmtcpacket/pus/TcPacketBase.h"
+#include "../tmtcpacket/pus/TcPacketStoredBase.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../storagemanager/StorageManagerIF.h"
 #include "../tmtcservices/VerificationCodes.h"
 
-TcPacketCheck::TcPacketCheck( uint16_t setApid ) : apid(setApid) {
+TcPacketCheck::TcPacketCheck(uint16_t setApid): apid(setApid) {
 }
 
-ReturnValue_t TcPacketCheck::checkPacket( TcPacketStored* currentPacket ) {
-	uint16_t calculated_crc = CRC::crc16ccitt( currentPacket->getWholeData(),
-	        currentPacket->getFullSize() );
-	if ( calculated_crc != 0 ) {
-		return INCORRECT_CHECKSUM;
-	}
-	bool condition = (not currentPacket->hasSecondaryHeader()) or
-			(currentPacket->getPacketVersionNumber() != CCSDS_VERSION_NUMBER) or
-			(not currentPacket->isTelecommand());
-	if ( condition ) {
-		return INCORRECT_PRIMARY_HEADER;
-	}
-	if ( currentPacket->getAPID() != this->apid )
-		return ILLEGAL_APID;
+ReturnValue_t TcPacketCheck::checkPacket(TcPacketStoredBase* currentPacket) {
+    TcPacketBase* tcPacketBase = currentPacket->getPacketBase();
+    if(tcPacketBase == nullptr) {
+        return RETURN_FAILED;
+    }
+    uint16_t calculated_crc = CRC::crc16ccitt(tcPacketBase->getWholeData(),
+            tcPacketBase->getFullSize());
+    if (calculated_crc != 0) {
+        return INCORRECT_CHECKSUM;
+    }
+    bool condition = (not tcPacketBase->hasSecondaryHeader()) or
+            (tcPacketBase->getPacketVersionNumber() != CCSDS_VERSION_NUMBER) or
+            (not tcPacketBase->isTelecommand());
+    if (condition) {
+        return INCORRECT_PRIMARY_HEADER;
+    }
+    if (tcPacketBase->getAPID() != this->apid)
+        return ILLEGAL_APID;
 
-	if ( not currentPacket->isSizeCorrect() ) {
-		return INCOMPLETE_PACKET;
-	}
-	condition = (currentPacket->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) ||
-			(currentPacket->getPusVersionNumber() != PUS_VERSION_NUMBER);
-	if ( condition ) {
-		return INCORRECT_SECONDARY_HEADER;
-	}
-	return RETURN_OK;
+    if (not currentPacket->isSizeCorrect()) {
+        return INCOMPLETE_PACKET;
+    }
+    condition = (tcPacketBase->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) ||
+            (tcPacketBase->getPusVersionNumber() != PUS_VERSION_NUMBER);
+    if (condition) {
+        return INCORRECT_SECONDARY_HEADER;
+    }
+    return RETURN_OK;
 }
 
 uint16_t TcPacketCheck::getApid() const {
-	return apid;
+    return apid;
 }
diff --git a/tcdistribution/TcPacketCheck.h b/tcdistribution/TcPacketCheck.h
index 703bb1bb..f7422c19 100644
--- a/tcdistribution/TcPacketCheck.h
+++ b/tcdistribution/TcPacketCheck.h
@@ -2,9 +2,10 @@
 #define FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_
 
 #include "../returnvalues/HasReturnvaluesIF.h"
-#include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
+class TcPacketStoredBase;
+
 /**
  * This class performs a formal packet check for incoming PUS Telecommand Packets.
  * Currently, it only checks if the APID and CRC are correct.
@@ -12,49 +13,48 @@
  */
 class TcPacketCheck : public HasReturnvaluesIF {
 protected:
-	/**
-	 * Describes the version number a packet must have to pass.
-	 */
-	static constexpr uint8_t CCSDS_VERSION_NUMBER = 0;
-	/**
-	 * Describes the secondary header a packet must have to pass.
-	 */
-	static constexpr uint8_t CCSDS_SECONDARY_HEADER_FLAG = 0;
-	/**
-	 * Describes the TC Packet PUS Version Number a packet must have to pass.
-	 */
-	static constexpr uint8_t PUS_VERSION_NUMBER = 1;
-	/**
-	 * The packet id each correct packet should have.
-	 * It is composed of the APID and some static fields.
-	 */
-	uint16_t apid;
+    /**
+     * Describes the version number a packet must have to pass.
+     */
+    static constexpr uint8_t CCSDS_VERSION_NUMBER = 0;
+    /**
+     * Describes the secondary header a packet must have to pass.
+     */
+    static constexpr uint8_t CCSDS_SECONDARY_HEADER_FLAG = 0;
+    /**
+     * Describes the TC Packet PUS Version Number a packet must have to pass.
+     */
+    static constexpr uint8_t PUS_VERSION_NUMBER = 1;
+    /**
+     * The packet id each correct packet should have.
+     * It is composed of the APID and some static fields.
+     */
+    uint16_t apid;
 public:
-	static const uint8_t INTERFACE_ID = CLASS_ID::TC_PACKET_CHECK;
-	static const ReturnValue_t ILLEGAL_APID = MAKE_RETURN_CODE( 0 );
-	static const ReturnValue_t INCOMPLETE_PACKET = MAKE_RETURN_CODE( 1 );
-	static const ReturnValue_t INCORRECT_CHECKSUM = MAKE_RETURN_CODE( 2 );
-	static const ReturnValue_t ILLEGAL_PACKET_TYPE = MAKE_RETURN_CODE( 3 );
-	static const ReturnValue_t ILLEGAL_PACKET_SUBTYPE = MAKE_RETURN_CODE( 4 );
-	static const ReturnValue_t INCORRECT_PRIMARY_HEADER = MAKE_RETURN_CODE( 5 );
-	static const ReturnValue_t INCORRECT_SECONDARY_HEADER = MAKE_RETURN_CODE( 6 );
-	/**
-	 * The constructor only sets the APID attribute.
-	 * @param set_apid The APID to set.
-	 */
-	TcPacketCheck( uint16_t setApid );
-	/**
-	 * This is the actual method to formally check a certain Telecommand Packet.
-	 * The packet's Application Data can not be checked here.
-	 * @param current_packet The packt to check
-	 * @return	- @c RETURN_OK on success.
-	 * 			- @c INCORRECT_CHECKSUM if checksum is invalid.
-	 * 			- @c ILLEGAL_APID if APID does not match.
-	 */
-	ReturnValue_t checkPacket( TcPacketStored* currentPacket );
+    static const uint8_t INTERFACE_ID = CLASS_ID::TC_PACKET_CHECK;
+    static const ReturnValue_t ILLEGAL_APID = MAKE_RETURN_CODE( 0 );
+    static const ReturnValue_t INCOMPLETE_PACKET = MAKE_RETURN_CODE( 1 );
+    static const ReturnValue_t INCORRECT_CHECKSUM = MAKE_RETURN_CODE( 2 );
+    static const ReturnValue_t ILLEGAL_PACKET_TYPE = MAKE_RETURN_CODE( 3 );
+    static const ReturnValue_t ILLEGAL_PACKET_SUBTYPE = MAKE_RETURN_CODE( 4 );
+    static const ReturnValue_t INCORRECT_PRIMARY_HEADER = MAKE_RETURN_CODE( 5 );
+    static const ReturnValue_t INCORRECT_SECONDARY_HEADER = MAKE_RETURN_CODE( 6 );
+    /**
+     * The constructor only sets the APID attribute.
+     * @param set_apid The APID to set.
+     */
+    TcPacketCheck(uint16_t setApid);
+    /**
+     * This is the actual method to formally check a certain Telecommand Packet.
+     * The packet's Application Data can not be checked here.
+     * @param current_packet The packt to check
+     * @return	- @c RETURN_OK on success.
+     * 			- @c INCORRECT_CHECKSUM if checksum is invalid.
+     * 			- @c ILLEGAL_APID if APID does not match.
+     */
+    ReturnValue_t checkPacket(TcPacketStoredBase* currentPacket);
 
-	uint16_t getApid() const;
+    uint16_t getApid() const;
 };
 
-
 #endif /* FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_ */
diff --git a/tmtcpacket/CMakeLists.txt b/tmtcpacket/CMakeLists.txt
index fe3d2a4d..fdc884ec 100644
--- a/tmtcpacket/CMakeLists.txt
+++ b/tmtcpacket/CMakeLists.txt
@@ -1,7 +1,6 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		SpacePacket.cpp
-		SpacePacketBase.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    SpacePacket.cpp
+    SpacePacketBase.cpp
 )
 
 add_subdirectory(packetmatcher)
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index 2d22cb66..9340e3b1 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -1,13 +1,14 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE
-		TcPacketBase.cpp
-		TcPacketPusA.cpp
-		TcPacketStored.cpp
-		TmPacketBase.cpp
-		TmPacketMinimal.cpp
-		TmPacketStoredPusA.cpp
-		TmPacketStoredPusC.cpp
-		TmPacketPusA.cpp
-		TmPacketPusC.cpp
-		TmPacketStoredBase.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE
+    TcPacketBase.cpp
+    TcPacketPusA.cpp
+    TcPacketStoredBase.cpp
+    TcPacketStoredPusA.cpp
+
+    TmPacketBase.cpp
+    TmPacketMinimal.cpp
+    TmPacketStoredPusA.cpp
+    TmPacketStoredPusC.cpp
+    TmPacketPusA.cpp
+    TmPacketPusC.cpp
+    TmPacketStoredBase.cpp
 )
diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp
index ca3e2a99..a5b6f9ce 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/TcPacketBase.cpp
@@ -2,87 +2,19 @@
 
 #include "../../globalfunctions/CRC.h"
 #include "../../globalfunctions/arrayprinter.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 
 #include <cstring>
 
-TcPacketBase::TcPacketBase(const uint8_t* setData) :
-		SpacePacketBase(setData) {
-	tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(setData));
-}
+TcPacketBase::TcPacketBase(const uint8_t* setData): SpacePacketBase(setData) {}
 
-TcPacketBase::~TcPacketBase() {
-	//Nothing to do.
-}
-
-uint8_t TcPacketBase::getService() {
-	return tcData->dataField.service_type;
-}
-
-uint8_t TcPacketBase::getSubService() {
-	return tcData->dataField.service_subtype;
-}
-
-uint8_t TcPacketBase::getAcknowledgeFlags() {
-	return tcData->dataField.version_type_ack & 0b00001111;
-}
-
-const uint8_t* TcPacketBase::getApplicationData() const {
-	return &tcData->appData;
-}
-
-uint16_t TcPacketBase::getApplicationDataSize() {
-	return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
-}
-
-uint16_t TcPacketBase::getErrorControl() {
-	uint16_t size = getApplicationDataSize() + CRC_SIZE;
-	uint8_t* p_to_buffer = &tcData->appData;
-	return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
-}
-
-void TcPacketBase::setErrorControl() {
-	uint32_t full_size = getFullSize();
-	uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
-	uint32_t size = getApplicationDataSize();
-	(&tcData->appData)[size] = (crc & 0XFF00) >> 8;	// CRCH
-	(&tcData->appData)[size + 1] = (crc) & 0X00FF; 		// CRCL
-}
-
-void TcPacketBase::setData(const uint8_t* pData) {
-	SpacePacketBase::setData(pData);
-	tcData = (TcPacketPointer*) pData;
-}
-
-uint8_t TcPacketBase::getSecondaryHeaderFlag() {
-	return (tcData->dataField.version_type_ack & 0b10000000) >> 7;
-}
-
-uint8_t TcPacketBase::getPusVersionNumber() {
-	return (tcData->dataField.version_type_ack & 0b01110000) >> 4;
-}
+TcPacketBase::~TcPacketBase() {}
 
 void TcPacketBase::print() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::debug << "TcPacketBase::print: " << std::endl;
+    sif::info << "TcPacketBase::print:" << std::endl;
+#else
+    sif::printInfo("TcPacketBase::print:\n");
 #endif
-	arrayprinter::print(getWholeData(), getFullSize());
-}
-
-void TcPacketBase::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
-        uint8_t ack, uint8_t service, uint8_t subservice) {
-	initSpacePacketHeader(true, true, apid, sequenceCount);
-	std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
-	setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
-	//Data Field Header:
-	//Set CCSDS_secondary_header_flag to 0 and version number to 001
-	tcData->dataField.version_type_ack = 0b00010000;
-	tcData->dataField.version_type_ack |= (ack & 0x0F);
-	tcData->dataField.service_type = service;
-	tcData->dataField.service_subtype = subservice;
-}
-
-size_t TcPacketBase::calculateFullPacketLength(size_t appDataLen) {
-	return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
-			appDataLen + TcPacketBase::CRC_SIZE;
+    arrayprinter::print(getWholeData(), getFullSize());
 }
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h
index 451f9c3a..80f9e505 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/TcPacketBase.h
@@ -16,9 +16,8 @@
  * @ingroup tmtcpackets
  */
 class TcPacketBase : public SpacePacketBase {
+    friend class TcPacketStoredBase;
 public:
-	static const uint16_t TC_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
-	        sizeof(PUSTcDataFieldHeader) + 2);
 
 	enum AckField {
 		//! No acknowledgements are expected.
@@ -54,7 +53,7 @@ public:
 	 * highest bit of the first byte of the Data Field Header.
 	 * @return	the CCSDS Secondary Header Flag
 	 */
-	uint8_t getSecondaryHeaderFlag();
+	virtual uint8_t getSecondaryHeaderFlag() = 0;
 	/**
 	 * This command returns the TC Packet PUS Version Number.
 	 * The version number of ECSS PUS 2003 is 1.
@@ -62,7 +61,7 @@ public:
 	 * first byte.
 	 * @return
 	 */
-	uint8_t getPusVersionNumber();
+	virtual uint8_t getPusVersionNumber() = 0;
 	/**
 	 * This is a getter for the packet's Ack field, which are the lowest four
 	 * bits of the first byte of the Data Field Header.
@@ -70,19 +69,19 @@ public:
 	 * It is packed in a uint8_t variable.
 	 * @return	The packet's PUS Ack field.
 	 */
-	uint8_t getAcknowledgeFlags();
+	virtual uint8_t getAcknowledgeFlags() = 0;
 	/**
 	 * 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();
+	virtual uint8_t getService() const = 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.
 	 */
-	uint8_t getSubService();
+	virtual uint8_t getSubService() = 0;
 	/**
 	 * This is a getter for a pointer to the packet's Application data.
 	 *
@@ -90,7 +89,7 @@ public:
 	 * the packet's application data.
 	 * @return	A pointer to the PUS Application Data.
 	 */
-	const uint8_t* getApplicationData() const;
+	virtual const uint8_t* getApplicationData() const = 0;
 	/**
 	 * This method calculates the size of the PUS Application data field.
 	 *
@@ -99,7 +98,7 @@ public:
 	 * @return	The size of the PUS Application Data (without Error Control
 	 * 		field)
 	 */
-	uint16_t getApplicationDataSize();
+	virtual uint16_t getApplicationDataSize() = 0;
 	/**
 	 * This getter returns the Error Control Field of the packet.
 	 *
@@ -108,44 +107,26 @@ public:
 	 * supposed to be a 16bit-CRC.
 	 * @return	The PUS Error Control
 	 */
-	uint16_t getErrorControl();
+	virtual uint16_t getErrorControl() = 0;
 	/**
 	 * With this method, the Error Control Field is updated to match the
 	 * current content of the packet.
 	 */
-	void setErrorControl();
+	virtual void setErrorControl() = 0;
 
-	/**
-	 * This is a debugging helper method that prints the whole packet content
-	 * to the screen.
-	 */
-	void print();
 	/**
 	 * Calculate full packet length from application data length.
 	 * @param appDataLen
 	 * @return
 	 */
-	static size_t calculateFullPacketLength(size_t appDataLen);
+	virtual size_t calculateFullPacketLength(size_t appDataLen) = 0;
 
+    /**
+     * This is a debugging helper method that prints the whole packet content
+     * to the screen.
+     */
+    void print();
 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.
-     */
-    TcPacketPointer* tcData;
-
-    /**
-     * Initializes the Tc Packet header.
-     * @param apid APID used.
-     * @param sequenceCount Sequence Count in the primary header.
-     * @param ack Which acknowledeges are expected from the receiver.
-     * @param service   PUS Service
-     * @param subservice PUS Subservice
-     */
-    void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack,
-            uint8_t service, uint8_t subservice);
 
     /**
      * With this method, the packet data pointer can be redirected to another
@@ -155,7 +136,7 @@ protected:
      *
      * @param p_data    A pointer to another PUS Telecommand Packet.
      */
-    void setData( const uint8_t* pData );
+    void setData( const uint8_t* pData ) = 0;
 };
 
 
diff --git a/tmtcpacket/pus/TcPacketPusA.cpp b/tmtcpacket/pus/TcPacketPusA.cpp
index c9af38a6..0ce767b4 100644
--- a/tmtcpacket/pus/TcPacketPusA.cpp
+++ b/tmtcpacket/pus/TcPacketPusA.cpp
@@ -1,3 +1,75 @@
 #include "TcPacketPusA.h"
+#include "../../globalfunctions/CRC.h"
 
+#include <cstring>
 
+TcPacketPusA::TcPacketPusA(const uint8_t *setData): TcPacketBase(setData) {
+    tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(setData));
+}
+
+uint8_t TcPacketPusA::getService() const {
+    return tcData->dataField.service_type;
+}
+
+uint8_t TcPacketPusA::getSubService() {
+    return tcData->dataField.service_subtype;
+}
+
+uint8_t TcPacketPusA::getAcknowledgeFlags() {
+    return tcData->dataField.version_type_ack & 0b00001111;
+}
+
+const uint8_t* TcPacketPusA::getApplicationData() const {
+    return &tcData->appData;
+}
+
+uint16_t TcPacketPusA::getApplicationDataSize() {
+    return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
+}
+
+uint16_t TcPacketPusA::getErrorControl() {
+    uint16_t size = getApplicationDataSize() + CRC_SIZE;
+    uint8_t* p_to_buffer = &tcData->appData;
+    return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
+}
+
+void TcPacketPusA::setErrorControl() {
+    uint32_t full_size = getFullSize();
+    uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
+    uint32_t size = getApplicationDataSize();
+    (&tcData->appData)[size] = (crc & 0XFF00) >> 8; // CRCH
+    (&tcData->appData)[size + 1] = (crc) & 0X00FF;      // CRCL
+}
+
+void TcPacketPusA::setData(const uint8_t* pData) {
+    SpacePacketBase::setData(pData);
+    // This function is const-correct, but it was decided to keep the pointer non-const
+    // for convenience. Therefore, cast aways constness here and then cast to packet type.
+    tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(pData));
+}
+
+uint8_t TcPacketPusA::getSecondaryHeaderFlag() {
+    return (tcData->dataField.version_type_ack & 0b10000000) >> 7;
+}
+
+uint8_t TcPacketPusA::getPusVersionNumber() {
+    return (tcData->dataField.version_type_ack & 0b01110000) >> 4;
+}
+
+void TcPacketPusA::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
+        uint8_t ack, uint8_t service, uint8_t subservice) {
+    initSpacePacketHeader(true, true, apid, sequenceCount);
+    std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
+    setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
+    // Data Field Header:
+    // Set CCSDS_secondary_header_flag to 0 and version number to 001
+    tcData->dataField.version_type_ack = 0b00010000;
+    tcData->dataField.version_type_ack |= (ack & 0x0F);
+    tcData->dataField.service_type = service;
+    tcData->dataField.service_subtype = subservice;
+}
+
+size_t TcPacketPusA::calculateFullPacketLength(size_t appDataLen) {
+    return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
+            appDataLen + TcPacketBase::CRC_SIZE;
+}
diff --git a/tmtcpacket/pus/TcPacketPusA.h b/tmtcpacket/pus/TcPacketPusA.h
index 9254ad32..0e61d829 100644
--- a/tmtcpacket/pus/TcPacketPusA.h
+++ b/tmtcpacket/pus/TcPacketPusA.h
@@ -1,7 +1,7 @@
 #ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
 #define FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
 
-#include "ccsds_header.h"
+#include "../ccsds_header.h"
 #include "TcPacketBase.h"
 
 #include <cstdint>
@@ -33,7 +33,50 @@ struct TcPacketPointer {
 
 class TcPacketPusA: public TcPacketBase {
 public:
-private:
+    static const uint16_t TC_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
+            sizeof(PUSTcDataFieldHeader) + 2);
+
+    /**
+     * Initialize a PUS A telecommand packet which already exists. You can also
+     * create an empty (invalid) object by passing nullptr as the data pointer
+     * @param setData
+     */
+    TcPacketPusA(const uint8_t* setData);
+
+    // Base class overrides
+    virtual uint8_t getSecondaryHeaderFlag() override;
+    virtual uint8_t getPusVersionNumber() override;
+    virtual uint8_t getAcknowledgeFlags() override;
+    virtual uint8_t getService() const override;
+    virtual uint8_t getSubService() override;
+    const uint8_t* getApplicationData() const override;
+    uint16_t getApplicationDataSize() override;
+    uint16_t getErrorControl() override;
+    void setErrorControl() override;
+    size_t calculateFullPacketLength(size_t appDataLen) override;
+
+protected:
+
+    void setData(const uint8_t* pData) override;
+
+    /**
+     * Initializes the Tc Packet header.
+     * @param apid APID used.
+     * @param sequenceCount Sequence Count in the primary header.
+     * @param ack Which acknowledeges are expected from the receiver.
+     * @param service   PUS Service
+     * @param subservice PUS Subservice
+     */
+    void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack,
+            uint8_t service, uint8_t subservice);
+
+    /**
+     * 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.
+     */
+    TcPacketPointer* tcData = nullptr;
 };
 
 
diff --git a/tmtcpacket/pus/TcPacketStored.cpp b/tmtcpacket/pus/TcPacketStored.cpp
deleted file mode 100644
index 36fa8d11..00000000
--- a/tmtcpacket/pus/TcPacketStored.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-#include "TcPacketStored.h"
-
-#include "../../objectmanager/ObjectManager.h"
-#include "../../serviceinterface/ServiceInterface.h"
-
-#include <cstring>
-
-StorageManagerIF* TcPacketStored::store = nullptr;
-
-TcPacketStored::TcPacketStored(store_address_t setAddress) :
-		TcPacketBase(nullptr), storeAddress(setAddress) {
-	setStoreAddress(storeAddress);
-}
-
-TcPacketStored::TcPacketStored(uint16_t apid,  uint8_t service,
-        uint8_t subservice, uint8_t sequenceCount, const uint8_t* data,
-		size_t size, uint8_t ack) :
-		TcPacketBase(nullptr) {
-	this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	if (not this->checkAndSetStore()) {
-		return;
-	}
-	uint8_t* pData = nullptr;
-	ReturnValue_t returnValue = this->store->getFreeElement(&this->storeAddress,
-			(TC_PACKET_MIN_SIZE + size), &pData);
-	if (returnValue != this->store->RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "TcPacketStored: Could not get free element from store!"
-				<< std::endl;
-#endif
-		return;
-	}
-	this->setData(pData);
-	initializeTcPacket(apid, sequenceCount, ack, service, subservice);
-	memcpy(&tcData->appData, data, size);
-	this->setPacketDataLength(
-			size + sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
-	this->setErrorControl();
-}
-
-ReturnValue_t TcPacketStored::getData(const uint8_t ** dataPtr,
-		size_t* dataSize) {
-	auto result = this->store->getData(storeAddress, dataPtr, dataSize);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "TcPacketStored: Could not get data!" << std::endl;
-#endif
-	}
-	return result;
-}
-
-TcPacketStored::TcPacketStored(): TcPacketBase(nullptr) {
-	this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	this->checkAndSetStore();
-
-}
-
-ReturnValue_t TcPacketStored::deletePacket() {
-	ReturnValue_t result = this->store->deleteData(this->storeAddress);
-	this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	this->setData(nullptr);
-	return result;
-}
-
-bool TcPacketStored::checkAndSetStore() {
-	if (this->store == nullptr) {
-		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!"
-					<< std::endl;
-#endif
-			return false;
-		}
-	}
-	return true;
-}
-
-void TcPacketStored::setStoreAddress(store_address_t setAddress) {
-	this->storeAddress = setAddress;
-	const uint8_t* tempData = nullptr;
-	size_t temp_size;
-	ReturnValue_t status = StorageManagerIF::RETURN_FAILED;
-	if (this->checkAndSetStore()) {
-		status = this->store->getData(this->storeAddress, &tempData,
-				&temp_size);
-	}
-	if (status == StorageManagerIF::RETURN_OK) {
-		this->setData(tempData);
-	} else {
-		this->setData(nullptr);
-		this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	}
-}
-
-store_address_t TcPacketStored::getStoreAddress() {
-	return this->storeAddress;
-}
-
-bool TcPacketStored::isSizeCorrect() {
-	const uint8_t* temp_data = nullptr;
-	size_t temp_size;
-	ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data,
-			&temp_size);
-	if (status == StorageManagerIF::RETURN_OK) {
-		if (this->getFullSize() == temp_size) {
-			return true;
-		}
-	}
-	return false;
-}
-
-TcPacketStored::TcPacketStored(const uint8_t* data, uint32_t size) :
-		TcPacketBase(data) {
-	if (getFullSize() != size) {
-		return;
-	}
-	if (this->checkAndSetStore()) {
-		ReturnValue_t status = store->addData(&storeAddress, data, size);
-		if (status != HasReturnvaluesIF::RETURN_OK) {
-			this->setData(nullptr);
-		}
-	}
-}
diff --git a/tmtcpacket/pus/TcPacketStored.h b/tmtcpacket/pus/TcPacketStored.h
index 1666107b..0b6ff0f4 100644
--- a/tmtcpacket/pus/TcPacketStored.h
+++ b/tmtcpacket/pus/TcPacketStored.h
@@ -1,117 +1,13 @@
-#ifndef TMTCPACKET_PUS_TCPACKETSTORED_H_
-#define TMTCPACKET_PUS_TCPACKETSTORED_H_
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_
 
-#include "TcPacketBase.h"
-#include "../../storagemanager/StorageManagerIF.h"
+#include <FSFWConfig.h>
 
-/**
- *	This class generates a ECSS PUS Telecommand 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 TcPacketStored : public TcPacketBase {
-public:
-	/**
-	 * This is a default constructor which does not set the data pointer.
-	 * However, it does try to set the packet store.
-	 */
-	TcPacketStored();
-	/**
-	 * With this constructor, the class instance is linked to an existing
-	 * packet in the packet store.
-	 * The packet content is neither checked nor changed with this call. If
-	 * the packet could not be found, the data pointer is set to NULL.
-	 */
-	TcPacketStored( store_address_t setAddress );
-	/**
-	 * With this constructor, new space is allocated in the packet store and
-	 * a new PUS Telecommand Packet is created there.
-	 * Packet Application Data passed in data is copied into the packet.
-	 * @param apid			Sets the packet's APID field.
-	 * @param service		Sets the packet's Service ID field.
-	 *                      This specifies the destination service.
-	 * @param subservice	Sets the packet's Service Subtype field.
-	 * 		                This specifies the destination sub-service.
-	 * @param sequence_count Sets the packet's Source Sequence Count field.
-	 * @param data		The data to be copied to the Application Data Field.
-	 * @param size		The amount of data to be copied.
-	 * @param ack           Set's the packet's Ack field, which specifies
-	 *                      number of verification packets returned
-     *                      for this command.
-	 */
-	TcPacketStored(uint16_t apid, uint8_t service, uint8_t subservice,
-			uint8_t sequence_count = 0, const uint8_t* data = nullptr,
-			size_t size = 0, uint8_t ack = TcPacketBase::ACK_ALL);
-	/**
-	 * Another constructor to create a TcPacket from a raw packet stream.
-	 * Takes the data and adds it unchecked to the TcStore.
-	 * @param data	Pointer to the complete TC Space Packet.
-	 * @param Size size of the packet.
-	 */
-	TcPacketStored( const uint8_t* data, uint32_t size);
-
-	/**
-	 * Getter function for the raw data.
-	 * @param dataPtr [out] Pointer to the data pointer to set
-	 * @param dataSize [out] Address of size to set.
-	 * @return -@c RETURN_OK if data was retrieved successfully.
-	 */
-	ReturnValue_t getData(const uint8_t ** dataPtr,
-			size_t* dataSize);
-	/**
-	 * 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.
-	 * @return	returncode from deleting the data.
-	 */
-	ReturnValue_t 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 );
-	/**
-	 * This method performs a size check.
-	 * It reads the stored size and compares it with the size entered in the
-	 * packet header. This class is the optimal place for such a check as it
-	 * has access to both the header data and the store.
-	 * @return	true if size is correct, false if packet is not registered in
-	 * 			store or size is incorrect.
-	 */
-	bool isSizeCorrect();
-
-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::TC_STORE.
-     */
-    static StorageManagerIF* store;
-    /**
-     * 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();
-};
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+#include "TcPacketStoredPusC.h"
+#else
+#include "TcPacketStoredPusA.h"
+#endif
 
 
-#endif /* TMTCPACKET_PUS_TCPACKETSTORED_H_ */
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredBase.cpp b/tmtcpacket/pus/TcPacketStoredBase.cpp
new file mode 100644
index 00000000..629562c0
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredBase.cpp
@@ -0,0 +1,72 @@
+#include "TcPacketStoredBase.h"
+
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../objectmanager/frameworkObjects.h"
+
+#include <cstring>
+
+StorageManagerIF* TcPacketStoredBase::store = nullptr;
+
+TcPacketStoredBase::TcPacketStoredBase() {
+    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    this->checkAndSetStore();
+
+}
+
+TcPacketStoredBase::~TcPacketStoredBase() {
+}
+
+ReturnValue_t TcPacketStoredBase::getData(const uint8_t ** dataPtr,
+		size_t* dataSize) {
+	auto result = this->store->getData(storeAddress, dataPtr, dataSize);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "TcPacketStoredBase: Could not get data!" << std::endl;
+#else
+		sif::printWarning("TcPacketStoredBase: Could not get data!\n");
+#endif
+	}
+	return result;
+}
+
+
+
+bool TcPacketStoredBase::checkAndSetStore() {
+	if (this->store == nullptr) {
+		this->store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
+		if (this->store == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::error << "TcPacketStoredBase::TcPacketStoredBase: TC Store not found!"
+					<< std::endl;
+#endif
+			return false;
+		}
+	}
+	return true;
+}
+
+void TcPacketStoredBase::setStoreAddress(store_address_t setAddress) {
+    this->storeAddress = setAddress;
+    const uint8_t* tempData = nullptr;
+    size_t tempSize;
+    ReturnValue_t status = StorageManagerIF::RETURN_FAILED;
+    if (this->checkAndSetStore()) {
+        status = this->store->getData(this->storeAddress, &tempData, &tempSize);
+    }
+    TcPacketBase* tcPacketBase = this->getPacketBase();
+    if(tcPacketBase == nullptr) {
+        return;
+    }
+    if (status == StorageManagerIF::RETURN_OK) {
+        tcPacketBase->setData(tempData);
+    }
+    else {
+        tcPacketBase->setData(nullptr);
+        this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    }
+}
+
+store_address_t TcPacketStoredBase::getStoreAddress() {
+	return this->storeAddress;
+}
diff --git a/tmtcpacket/pus/TcPacketStoredBase.h b/tmtcpacket/pus/TcPacketStoredBase.h
new file mode 100644
index 00000000..811123db
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredBase.h
@@ -0,0 +1,90 @@
+#ifndef TMTCPACKET_PUS_TCPACKETSTORED_H_
+#define TMTCPACKET_PUS_TCPACKETSTORED_H_
+
+#include <fsfw/tmtcpacket/pus/TcPacketStoredIF.h>
+#include "../../storagemanager/StorageManagerIF.h"
+
+/**
+ *	This class generates a ECSS PUS Telecommand 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 TcPacketStoredBase: public TcPacketStoredIF {
+public:
+    /**
+     * This is a default constructor which does not set the data pointer to initialize
+     * with an empty cached store address
+     */
+    TcPacketStoredBase();
+    /**
+     * Constructor to set to an existing store address.
+     * @param setAddress
+     */
+    TcPacketStoredBase(store_address_t setAddress);
+    /**
+     * Another constructor to create a TcPacket from a raw packet stream.
+     * Takes the data and adds it unchecked to the TcStore.
+     * @param data  Pointer to the complete TC Space Packet.
+     * @param Size size of the packet.
+     */
+    TcPacketStoredBase(const uint8_t* data, uint32_t size);
+
+	virtual~ TcPacketStoredBase();
+
+	/**
+	 * Getter function for the raw data.
+	 * @param dataPtr [out] Pointer to the data pointer to set
+	 * @param dataSize [out] Address of size to set.
+	 * @return -@c RETURN_OK if data was retrieved successfully.
+	 */
+	ReturnValue_t getData(const uint8_t ** dataPtr, size_t* dataSize) override;
+
+    void setStoreAddress(store_address_t setAddress) override;
+    store_address_t getStoreAddress() override;
+
+	/**
+	 * With this call, the packet is deleted.
+	 * It removes itself from the store and sets its data pointer to NULL.
+	 * @return	returncode from deleting the data.
+	 */
+	virtual ReturnValue_t deletePacket() = 0;
+
+	/**
+	 * This method performs a size check.
+	 * It reads the stored size and compares it with the size entered in the
+	 * packet header. This class is the optimal place for such a check as it
+	 * has access to both the header data and the store.
+	 * @return	true if size is correct, false if packet is not registered in
+	 * 			store or size is incorrect.
+	 */
+	virtual bool isSizeCorrect() = 0;
+
+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::TC_STORE.
+     */
+    static StorageManagerIF* store;
+    /**
+     * 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();
+};
+
+
+#endif /* TMTCPACKET_PUS_TcPacketStoredBase_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredIF.h b/tmtcpacket/pus/TcPacketStoredIF.h
new file mode 100644
index 00000000..8eb23004
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredIF.h
@@ -0,0 +1,38 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
+
+#include "../../tmtcpacket/pus/TcPacketBase.h"
+#include "../../storagemanager/storeAddress.h"
+#include "../../returnvalues/HasReturnvaluesIF.h"
+
+class TcPacketStoredIF {
+public:
+    virtual~TcPacketStoredIF() {};
+
+    /**
+     * With this call, the stored packet can be set to another packet in a 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.
+     */
+    virtual void setStoreAddress(store_address_t setAddress) = 0;
+
+    virtual store_address_t getStoreAddress() = 0;
+
+    /**
+     * Getter function for the raw data.
+     * @param dataPtr [out]     Pointer to the data pointer to set
+     * @param dataSize [out]    Address of size to set.
+     * @return -@c RETURN_OK if data was retrieved successfully.
+     */
+    virtual ReturnValue_t getData(const uint8_t ** dataPtr, size_t* dataSize) = 0;
+
+    /**
+     * Get packet base pointer which can be used to get access to PUS packet fields
+     * @return
+     */
+    virtual TcPacketBase* getPacketBase() = 0;
+};
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredPusA.cpp b/tmtcpacket/pus/TcPacketStoredPusA.cpp
new file mode 100644
index 00000000..7c706a1a
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredPusA.cpp
@@ -0,0 +1,77 @@
+#include "TcPacketStoredPusA.h"
+
+#include <cstring>
+
+TcPacketStoredPusA::TcPacketStoredPusA(uint16_t apid,  uint8_t service,
+        uint8_t subservice, uint8_t sequenceCount, const uint8_t* data,
+        size_t size, uint8_t ack) :
+        TcPacketPusA(nullptr) {
+    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    if (not this->checkAndSetStore()) {
+        return;
+    }
+    uint8_t* pData = nullptr;
+    ReturnValue_t returnValue = this->store->getFreeElement(&this->storeAddress,
+            (TC_PACKET_MIN_SIZE + size), &pData);
+    if (returnValue != this->store->RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "TcPacketStoredBase: Could not get free element from store!"
+                << std::endl;
+#endif
+        return;
+    }
+    this->setData(pData);
+    initializeTcPacket(apid, sequenceCount, ack, service, subservice);
+    std::memcpy(&tcData->appData, data, size);
+    this->setPacketDataLength(
+            size + sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
+    this->setErrorControl();
+}
+
+TcPacketStoredPusA::TcPacketStoredPusA(): TcPacketStoredBase(), TcPacketPusA(nullptr) {
+}
+
+TcPacketStoredPusA::TcPacketStoredPusA(store_address_t setAddress): TcPacketPusA(nullptr) {
+    TcPacketStoredBase::setStoreAddress(setAddress);
+}
+
+TcPacketStoredPusA::TcPacketStoredPusA(const uint8_t* data, size_t size): TcPacketPusA(data) {
+    if (this->getFullSize() != size) {
+        return;
+    }
+    if (this->checkAndSetStore()) {
+        ReturnValue_t status = store->addData(&storeAddress, data, size);
+        if (status != HasReturnvaluesIF::RETURN_OK) {
+            this->setData(nullptr);
+        }
+        const uint8_t* storePtr = nullptr;
+        // Repoint base data pointer to the data in the store.
+        store->getData(storeAddress, &storePtr, &size);
+        this->setData(storePtr);
+    }
+}
+
+ReturnValue_t TcPacketStoredPusA::deletePacket() {
+    ReturnValue_t result = this->store->deleteData(this->storeAddress);
+    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    this->setData(nullptr);
+    return result;
+}
+
+TcPacketBase* TcPacketStoredPusA::getPacketBase() {
+    return this;
+}
+
+
+bool TcPacketStoredPusA::isSizeCorrect() {
+    const uint8_t* temp_data = nullptr;
+    size_t temp_size;
+    ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data,
+            &temp_size);
+    if (status == StorageManagerIF::RETURN_OK) {
+        if (this->getFullSize() == temp_size) {
+            return true;
+        }
+    }
+    return false;
+}
diff --git a/tmtcpacket/pus/TcPacketStoredPusA.h b/tmtcpacket/pus/TcPacketStoredPusA.h
new file mode 100644
index 00000000..fe1f64f7
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredPusA.h
@@ -0,0 +1,53 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_
+
+#include "TcPacketStoredBase.h"
+#include "TcPacketPusA.h"
+
+class TcPacketStoredPusA:
+        public TcPacketStoredBase,
+        public TcPacketPusA {
+public:
+    /**
+     * With this constructor, new space is allocated in the packet store and
+     * a new PUS Telecommand Packet is created there.
+     * Packet Application Data passed in data is copied into the packet.
+     * @param apid          Sets the packet's APID field.
+     * @param service       Sets the packet's Service ID field.
+     *                      This specifies the destination service.
+     * @param subservice    Sets the packet's Service Subtype field.
+     *                      This specifies the destination sub-service.
+     * @param sequence_count Sets the packet's Source Sequence Count field.
+     * @param data      The data to be copied to the Application Data Field.
+     * @param size      The amount of data to be copied.
+     * @param ack           Set's the packet's Ack field, which specifies
+     *                      number of verification packets returned
+     *                      for this command.
+     */
+    TcPacketStoredPusA(uint16_t apid, uint8_t service, uint8_t subservice,
+            uint8_t sequence_count = 0, const uint8_t* data = nullptr,
+            size_t size = 0, uint8_t ack = TcPacketBase::ACK_ALL);
+    /**
+     * Create stored packet with existing data.
+     * @param data
+     * @param size
+     */
+    TcPacketStoredPusA(const uint8_t* data, size_t size);
+    /**
+     * Create stored packet from existing packet in store
+     * @param setAddress
+     */
+    TcPacketStoredPusA(store_address_t setAddress);
+    TcPacketStoredPusA();
+
+    ReturnValue_t deletePacket() override;
+    TcPacketBase* getPacketBase() override;
+
+private:
+
+    bool isSizeCorrect() override;
+};
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredPusC.cpp b/tmtcpacket/pus/TcPacketStoredPusC.cpp
new file mode 100644
index 00000000..f568d139
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredPusC.cpp
@@ -0,0 +1,10 @@
+/*
+ * TcPacketStoredPusC.cpp
+ *
+ *  Created on: Jun 13, 2021
+ *      Author: rmueller
+ */
+
+
+
+
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp
index acd69b65..b144db1b 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/TmPacketBase.cpp
@@ -44,13 +44,6 @@ ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const {
             &tempSize, getTimestampSize());
 }
 
-void TmPacketBase::print() {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::debug << "TmPacketBase::print: " << std::endl;
-#endif
-    arrayprinter::print(getWholeData(), getFullSize());
-}
-
 bool TmPacketBase::checkAndSetStamper() {
     if (timeStamper == NULL) {
         timeStamper = ObjectManager::instance()->get<TimeStamperIF>(timeStamperId);
@@ -66,3 +59,11 @@ bool TmPacketBase::checkAndSetStamper() {
     return true;
 }
 
+void TmPacketBase::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::info << "TmPacketBase::print:" << std::endl;
+#else
+    sif::printInfo("TmPacketBase::print:\n");
+#endif
+    arrayprinter::print(getWholeData(), getFullSize());
+}
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index 863cba4f..0ede27dc 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -246,7 +246,11 @@ void CommandingServiceBase::handleRequestQueue() {
 	TmTcMessage message;
 	ReturnValue_t result;
 	store_address_t address;
-	TcPacketStored packet;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+	TcPacketStoredPusC packet;
+#else
+	TcPacketStoredPusA packet;
+#endif
 	MessageQueueId_t queue;
 	object_id_t objectId;
 	for (result = requestQueue->receiveMessage(&message); result == RETURN_OK;
@@ -351,14 +355,18 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
 }
 
 
-void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
+void CommandingServiceBase::startExecution(TcPacketStoredBase *storedPacket,
         CommandMapIter iter) {
     ReturnValue_t result = RETURN_OK;
     CommandMessage command;
-    iter->second.subservice = storedPacket->getSubService();
+    TcPacketBase* tcPacketBase = storedPacket->getPacketBase();
+    if(tcPacketBase == nullptr) {
+        return;
+    }
+    iter->second.subservice = tcPacketBase->getSubService();
     result = prepareCommand(&command, iter->second.subservice,
-            storedPacket->getApplicationData(),
-            storedPacket->getApplicationDataSize(), &iter->second.state,
+            tcPacketBase->getApplicationData(),
+            tcPacketBase->getApplicationDataSize(), &iter->second.state,
             iter->second.objectId);
 
     ReturnValue_t sendResult = RETURN_OK;
@@ -371,12 +379,12 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
 		if (sendResult == RETURN_OK) {
 			Clock::getUptime(&iter->second.uptimeOfStart);
 			iter->second.step = 0;
-			iter->second.subservice = storedPacket->getSubService();
+			iter->second.subservice = tcPacketBase->getSubService();
 			iter->second.command = command.getCommand();
-			iter->second.tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
-			iter->second.tcInfo.tcPacketId = storedPacket->getPacketId();
+			iter->second.tcInfo.ackFlags = tcPacketBase->getAcknowledgeFlags();
+			iter->second.tcInfo.tcPacketId = tcPacketBase->getPacketId();
 			iter->second.tcInfo.tcSequenceControl =
-					storedPacket->getPacketSequenceControl();
+			        tcPacketBase->getPacketSequenceControl();
 			acceptPacket(tc_verification::START_SUCCESS, storedPacket);
 		} else {
 			command.clearCommandMessage();
@@ -392,7 +400,7 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
 		}
 		if (sendResult == RETURN_OK) {
 			verificationReporter.sendSuccessReport(tc_verification::START_SUCCESS,
-					storedPacket);
+					storedPacket->getPacketBase());
 			acceptPacket(tc_verification::COMPLETION_SUCCESS, storedPacket);
 			checkAndExecuteFifo(iter);
 		} else {
@@ -409,16 +417,16 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
 }
 
 
-void CommandingServiceBase::rejectPacket(uint8_t report_id,
-		TcPacketStored* packet, ReturnValue_t error_code) {
-	verificationReporter.sendFailureReport(report_id, packet, error_code);
+void CommandingServiceBase::rejectPacket(uint8_t reportId,
+		TcPacketStoredBase* packet, ReturnValue_t errorCode) {
+	verificationReporter.sendFailureReport(reportId, packet->getPacketBase(), errorCode);
 	packet->deletePacket();
 }
 
 
 void CommandingServiceBase::acceptPacket(uint8_t reportId,
-		TcPacketStored* packet) {
-	verificationReporter.sendSuccessReport(reportId, packet);
+		TcPacketStoredBase* packet) {
+	verificationReporter.sendSuccessReport(reportId, packet->getPacketBase());
 	packet->deletePacket();
 }
 
@@ -428,7 +436,11 @@ void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
 	if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
 		commandMap.erase(&iter);
 	} else {
-		TcPacketStored newPacket(address);
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+		TcPacketStoredPusC newPacket(address);
+#else
+		TcPacketStoredPusA newPacket(address);
+#endif
 		startExecution(&newPacket, iter);
 	}
 }
diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h
index e10f2ddd..46191bd5 100644
--- a/tmtcservices/CommandingServiceBase.h
+++ b/tmtcservices/CommandingServiceBase.h
@@ -1,6 +1,7 @@
 #ifndef FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 #define FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 
+#include <fsfw/tmtcpacket/pus/TcPacketStoredBase.h>
 #include "AcceptsTelecommandsIF.h"
 #include "VerificationReporter.h"
 
@@ -351,12 +352,12 @@ private:
 	 */
 	void handleRequestQueue();
 
-	void rejectPacket(uint8_t reportId, TcPacketStored* packet,
+	void rejectPacket(uint8_t reportId, TcPacketStoredBase* packet,
 			ReturnValue_t errorCode);
 
-	void acceptPacket(uint8_t reportId, TcPacketStored* packet);
+	void acceptPacket(uint8_t reportId, TcPacketStoredBase* packet);
 
-	void startExecution(TcPacketStored *storedPacket, CommandMapIter iter);
+	void startExecution(TcPacketStoredBase *storedPacket, CommandMapIter iter);
 
 	void handleCommandMessage(CommandMessage* reply);
 	void handleReplyHandlerResult(ReturnValue_t result, CommandMapIter iter,
diff --git a/tmtcservices/PusServiceBase.h b/tmtcservices/PusServiceBase.h
index 4d3d99bc..f48612b1 100644
--- a/tmtcservices/PusServiceBase.h
+++ b/tmtcservices/PusServiceBase.h
@@ -141,7 +141,11 @@ protected:
 	 * The current Telecommand to be processed.
 	 * It is deleted after handleRequest was executed.
 	 */
-	TcPacketStored currentPacket;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+	TcPacketStoredPusC currentPacket;
+#else
+    TcPacketStoredPusA currentPacket;
+#endif
 
 	static object_id_t packetSource;
 
diff --git a/tmtcservices/VerificationReporter.cpp b/tmtcservices/VerificationReporter.cpp
index 998cbfb6..74e0719c 100644
--- a/tmtcservices/VerificationReporter.cpp
+++ b/tmtcservices/VerificationReporter.cpp
@@ -17,14 +17,17 @@ VerificationReporter::VerificationReporter() :
 VerificationReporter::~VerificationReporter() {}
 
 void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
-		TcPacketBase* current_packet, uint8_t set_step) {
+		TcPacketBase* currentPacket, uint8_t set_step) {
 	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
+	if(currentPacket == nullptr) {
+	    return;
+	}
 	PusVerificationMessage message(set_report_id,
-			current_packet->getAcknowledgeFlags(),
-			current_packet->getPacketId(),
-			current_packet->getPacketSequenceControl(), 0, set_step);
+			currentPacket->getAcknowledgeFlags(),
+			currentPacket->getPacketId(),
+			currentPacket->getPacketSequenceControl(), 0, set_step);
 	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
 			&message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
@@ -56,15 +59,18 @@ void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 }
 
 void VerificationReporter::sendFailureReport(uint8_t report_id,
-		TcPacketBase* current_packet, ReturnValue_t error_code, uint8_t step,
+		TcPacketBase* currentPacket, ReturnValue_t error_code, uint8_t step,
 		uint32_t parameter1, uint32_t parameter2) {
 	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
+	if(currentPacket == nullptr) {
+	    return;
+	}
 	PusVerificationMessage message(report_id,
-			current_packet->getAcknowledgeFlags(),
-			current_packet->getPacketId(),
-			current_packet->getPacketSequenceControl(), error_code, step,
+			currentPacket->getAcknowledgeFlags(),
+			currentPacket->getPacketId(),
+			currentPacket->getPacketSequenceControl(), error_code, step,
 			parameter1, parameter2);
 	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
 	        &message);

From 8dc66784a8aacff57bf6bb45f8061b1730e4a20d Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 16:59:28 +0200
Subject: [PATCH 131/157] tiny changes

---
 tmtcpacket/pus/TcPacketStoredPusC.cpp | 10 +---------
 tmtcpacket/pus/TcPacketStoredPusC.h   |  8 ++++++++
 2 files changed, 9 insertions(+), 9 deletions(-)
 create mode 100644 tmtcpacket/pus/TcPacketStoredPusC.h

diff --git a/tmtcpacket/pus/TcPacketStoredPusC.cpp b/tmtcpacket/pus/TcPacketStoredPusC.cpp
index f568d139..0ff2376a 100644
--- a/tmtcpacket/pus/TcPacketStoredPusC.cpp
+++ b/tmtcpacket/pus/TcPacketStoredPusC.cpp
@@ -1,10 +1,2 @@
-/*
- * TcPacketStoredPusC.cpp
- *
- *  Created on: Jun 13, 2021
- *      Author: rmueller
- */
-
-
-
+#include "TcPacketStoredPusC.h"
 
diff --git a/tmtcpacket/pus/TcPacketStoredPusC.h b/tmtcpacket/pus/TcPacketStoredPusC.h
new file mode 100644
index 00000000..e8ced948
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredPusC.h
@@ -0,0 +1,8 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_
+
+
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_ */

From 40a8c9a495f8f0a51f029dbd673defb6f6822d4f Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 17:58:44 +0200
Subject: [PATCH 132/157] srv 20 fixes

---
 pus/Service20ParameterManagement.cpp  | 3 +++
 pus/servicepackets/Service20Packets.h | 7 +++----
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/pus/Service20ParameterManagement.cpp b/pus/Service20ParameterManagement.cpp
index 8ebc6db0..c4e4b5eb 100644
--- a/pus/Service20ParameterManagement.cpp
+++ b/pus/Service20ParameterManagement.cpp
@@ -133,6 +133,9 @@ ReturnValue_t Service20ParameterManagement::prepareLoadCommand(
     store_address_t storeAddress;
     size_t parameterDataLen = tcDataLen - sizeof(object_id_t) - sizeof(ParameterId_t) -
             sizeof(uint32_t);
+    if(parameterDataLen == 0) {
+        return CommandingServiceBase::INVALID_TC;
+    }
     ReturnValue_t result = IPCStore->getFreeElement(&storeAddress,
             parameterDataLen, &storePointer);
     if(result != HasReturnvaluesIF::RETURN_OK) {
diff --git a/pus/servicepackets/Service20Packets.h b/pus/servicepackets/Service20Packets.h
index 33bd153d..6c7eb6f5 100644
--- a/pus/servicepackets/Service20Packets.h
+++ b/pus/servicepackets/Service20Packets.h
@@ -27,12 +27,11 @@ public:
     ParameterCommand(uint8_t* storePointer, size_t parameterDataLen):
         parameterBuffer(storePointer, parameterDataLen) {
 #if FSFW_VERBOSE_LEVEL >= 1
-        if(parameterDataLen < sizeof(object_id_t) + sizeof(ParameterId_t) + 4) {
+        if(parameterDataLen == 0) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::warning << "ParameterCommand: Parameter data length is less than 12!"
-                    << std::endl;
+            sif::warning << "ParameterCommand: Parameter data length is 0" << std::endl;
 #else
-            sif::printWarning("ParameterCommand: Parameter data length is less than 12!\n");
+            sif::printWarning("ParameterCommand: Parameter data length is 0!\n");
 #endif
         }
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */

From 56d2af9d2578775dfee495aca1016a8610a3125a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 10:19:01 +0200
Subject: [PATCH 133/157] cleaned up a bit

---
 pus/Service17Test.cpp                         |  2 +-
 pus/Service1TelecommandVerification.cpp       |  2 +-
 pus/Service5EventReporting.cpp                |  2 +-
 tcdistribution/PUSDistributor.cpp             |  6 +--
 tcdistribution/PUSDistributor.h               |  8 +---
 tcdistribution/TcPacketCheck.cpp              |  4 +-
 tmstorage/TmStorePackets.h                    |  2 +-
 tmtcpacket/packetmatcher/ApidMatcher.h        |  2 +-
 tmtcpacket/packetmatcher/PacketMatchTree.h    |  2 +-
 tmtcpacket/packetmatcher/ServiceMatcher.h     |  2 +-
 tmtcpacket/packetmatcher/SubserviceMatcher.h  |  2 +-
 tmtcpacket/pus/CMakeLists.txt                 | 16 +------
 tmtcpacket/pus/TcPacketPusC.cpp               |  4 --
 tmtcpacket/pus/TcPacketPusC.h                 |  8 ----
 tmtcpacket/pus/TcPacketStored.h               | 13 -----
 tmtcpacket/pus/TcPacketStoredPusC.cpp         |  2 -
 tmtcpacket/pus/TcPacketStoredPusC.h           |  8 ----
 tmtcpacket/pus/tc.h                           |  7 +++
 tmtcpacket/pus/tc/CMakeLists.txt              |  6 +++
 tmtcpacket/pus/{ => tc}/TcPacketBase.cpp      |  6 +--
 tmtcpacket/pus/{ => tc}/TcPacketBase.h        |  2 +-
 .../{TcPacketPusA.cpp => tc/TcPacketPus.cpp}  | 48 +++++++++----------
 .../pus/{TcPacketPusA.h => tc/TcPacketPus.h}  | 19 +++++---
 .../pus/{ => tc}/TcPacketStoredBase.cpp       |  6 +--
 tmtcpacket/pus/{ => tc}/TcPacketStoredBase.h  |  4 +-
 tmtcpacket/pus/{ => tc}/TcPacketStoredIF.h    |  6 +--
 .../TcPacketStoredPus.cpp}                    | 18 +++----
 .../TcPacketStoredPus.h}                      | 14 +++---
 tmtcpacket/pus/tm.h                           | 16 +++++++
 tmtcpacket/pus/tm/CMakeLists.txt              |  9 ++++
 tmtcpacket/pus/{ => tm}/TmPacketBase.cpp      | 10 ++--
 tmtcpacket/pus/{ => tm}/TmPacketBase.h        |  8 ++--
 tmtcpacket/pus/{ => tm}/TmPacketMinimal.cpp   |  7 +--
 tmtcpacket/pus/{ => tm}/TmPacketMinimal.h     |  4 +-
 tmtcpacket/pus/{ => tm}/TmPacketPusA.cpp      | 10 ++--
 tmtcpacket/pus/{ => tm}/TmPacketPusA.h        |  8 ++--
 tmtcpacket/pus/{ => tm}/TmPacketPusC.cpp      | 10 ++--
 tmtcpacket/pus/{ => tm}/TmPacketPusC.h        |  8 ++--
 tmtcpacket/pus/{ => tm}/TmPacketStored.h      |  0
 .../pus/{ => tm}/TmPacketStoredBase.cpp       |  6 +--
 tmtcpacket/pus/{ => tm}/TmPacketStoredBase.h  | 12 ++---
 .../pus/{ => tm}/TmPacketStoredPusA.cpp       |  4 +-
 tmtcpacket/pus/{ => tm}/TmPacketStoredPusA.h  |  0
 .../pus/{ => tm}/TmPacketStoredPusC.cpp       |  4 +-
 tmtcpacket/pus/{ => tm}/TmPacketStoredPusC.h  |  4 +-
 tmtcservices/CommandingServiceBase.cpp        | 16 ++-----
 tmtcservices/CommandingServiceBase.h          |  2 +-
 tmtcservices/PusServiceBase.h                 |  8 +---
 tmtcservices/PusVerificationReport.h          |  2 +-
 49 files changed, 173 insertions(+), 196 deletions(-)
 delete mode 100644 tmtcpacket/pus/TcPacketPusC.cpp
 delete mode 100644 tmtcpacket/pus/TcPacketPusC.h
 delete mode 100644 tmtcpacket/pus/TcPacketStored.h
 delete mode 100644 tmtcpacket/pus/TcPacketStoredPusC.cpp
 delete mode 100644 tmtcpacket/pus/TcPacketStoredPusC.h
 create mode 100644 tmtcpacket/pus/tc.h
 create mode 100644 tmtcpacket/pus/tc/CMakeLists.txt
 rename tmtcpacket/pus/{ => tc}/TcPacketBase.cpp (72%)
 rename tmtcpacket/pus/{ => tc}/TcPacketBase.h (98%)
 rename tmtcpacket/pus/{TcPacketPusA.cpp => tc/TcPacketPus.cpp} (54%)
 rename tmtcpacket/pus/{TcPacketPusA.h => tc/TcPacketPus.h} (87%)
 rename tmtcpacket/pus/{ => tc}/TcPacketStoredBase.cpp (92%)
 rename tmtcpacket/pus/{ => tc}/TcPacketStoredBase.h (96%)
 rename tmtcpacket/pus/{ => tc}/TcPacketStoredIF.h (88%)
 rename tmtcpacket/pus/{TcPacketStoredPusA.cpp => tc/TcPacketStoredPus.cpp} (78%)
 rename tmtcpacket/pus/{TcPacketStoredPusA.h => tc/TcPacketStoredPus.h} (84%)
 create mode 100644 tmtcpacket/pus/tm.h
 create mode 100644 tmtcpacket/pus/tm/CMakeLists.txt
 rename tmtcpacket/pus/{ => tm}/TmPacketBase.cpp (88%)
 rename tmtcpacket/pus/{ => tm}/TmPacketBase.h (96%)
 rename tmtcpacket/pus/{ => tm}/TmPacketMinimal.cpp (93%)
 rename tmtcpacket/pus/{ => tm}/TmPacketMinimal.h (96%)
 rename tmtcpacket/pus/{ => tm}/TmPacketPusA.cpp (90%)
 rename tmtcpacket/pus/{ => tm}/TmPacketPusA.h (95%)
 rename tmtcpacket/pus/{ => tm}/TmPacketPusC.cpp (90%)
 rename tmtcpacket/pus/{ => tm}/TmPacketPusC.h (95%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStored.h (100%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredBase.cpp (94%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredBase.h (91%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredPusA.cpp (95%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredPusA.h (100%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredPusC.cpp (96%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredPusC.h (96%)

diff --git a/pus/Service17Test.cpp b/pus/Service17Test.cpp
index daed987a..37258cc1 100644
--- a/pus/Service17Test.cpp
+++ b/pus/Service17Test.cpp
@@ -3,7 +3,7 @@
 
 #include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/SystemObject.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
 
 
 Service17Test::Service17Test(object_id_t objectId,
diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index bef7b6b1..8aec6902 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -4,7 +4,7 @@
 #include "../ipc/QueueFactory.h"
 #include "../objectmanager/ObjectManager.h"
 #include "../tmtcservices/PusVerificationReport.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../tmtcservices/AcceptsTelemetryIF.h"
 
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index 272cc203..0c139f3a 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -5,7 +5,7 @@
 #include "../objectmanager/ObjectManager.h"
 #include "../events/EventManagerIF.h"
 #include "../ipc/QueueFactory.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
 
 
 Service5EventReporting::Service5EventReporting(object_id_t objectId,
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 5c2f734b..955a8093 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -118,11 +118,7 @@ uint16_t PUSDistributor::getIdentifier() {
 }
 
 ReturnValue_t PUSDistributor::initialize() {
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-    currentPacket = new TcPacketStoredPusC();
-#else
-    currentPacket = new TcPacketStoredPusA();
-#endif
+    currentPacket = new TcPacketStoredPus();
     if(currentPacket == nullptr) {
         // Should not happen, memory allocation failed!
         return ObjectManagerIF::CHILD_INIT_FAILED;
diff --git a/tcdistribution/PUSDistributor.h b/tcdistribution/PUSDistributor.h
index 0c288451..c6f863f0 100644
--- a/tcdistribution/PUSDistributor.h
+++ b/tcdistribution/PUSDistributor.h
@@ -5,7 +5,7 @@
 #include "TcDistributor.h"
 #include "TcPacketCheck.h"
 
-#include "../tmtcpacket/pus/TcPacketStored.h"
+#include "../tmtcpacket/pus/tc.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../tmtcservices/AcceptsTelecommandsIF.h"
 #include "../tmtcservices/VerificationReporter.h"
@@ -52,11 +52,7 @@ protected:
 	/**
 	 * The currently handled packet is stored here.
 	 */
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-	TcPacketStoredPusC* currentPacket = nullptr;
-#else
-	TcPacketStoredPusA* currentPacket = nullptr;
-#endif
+	TcPacketStoredPus* currentPacket = nullptr;
 
 	/**
 	 * With this variable, the current check status is stored to generate
diff --git a/tcdistribution/TcPacketCheck.cpp b/tcdistribution/TcPacketCheck.cpp
index e88305cc..dce0a458 100644
--- a/tcdistribution/TcPacketCheck.cpp
+++ b/tcdistribution/TcPacketCheck.cpp
@@ -1,8 +1,8 @@
 #include "TcPacketCheck.h"
 
 #include "../globalfunctions/CRC.h"
-#include "../tmtcpacket/pus/TcPacketBase.h"
-#include "../tmtcpacket/pus/TcPacketStoredBase.h"
+#include "../tmtcpacket/pus/tc/TcPacketBase.h"
+#include "../tmtcpacket/pus/tc/TcPacketStoredBase.h"
 #include "../serviceinterface/ServiceInterface.h"
 #include "../storagemanager/StorageManagerIF.h"
 #include "../tmtcservices/VerificationCodes.h"
diff --git a/tmstorage/TmStorePackets.h b/tmstorage/TmStorePackets.h
index 3abd0c1c..53a5d8d6 100644
--- a/tmstorage/TmStorePackets.h
+++ b/tmstorage/TmStorePackets.h
@@ -5,7 +5,7 @@
 #include "../serialize/SerializeElement.h"
 #include "../serialize/SerialLinkedListAdapter.h"
 #include "../serialize/SerialBufferAdapter.h"
-#include "../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../tmtcpacket/pus/tm/TmPacketMinimal.h"
 #include "../timemanager/TimeStamperIF.h"
 #include "../timemanager/CCSDSTime.h"
 #include "../globalfunctions/timevalOperations.h"
diff --git a/tmtcpacket/packetmatcher/ApidMatcher.h b/tmtcpacket/packetmatcher/ApidMatcher.h
index 4f196ac9..64f73dbf 100644
--- a/tmtcpacket/packetmatcher/ApidMatcher.h
+++ b/tmtcpacket/packetmatcher/ApidMatcher.h
@@ -3,7 +3,7 @@
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
-#include "../../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../../tmtcpacket/pus/tm/TmPacketMinimal.h"
 
 class ApidMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 private:
diff --git a/tmtcpacket/packetmatcher/PacketMatchTree.h b/tmtcpacket/packetmatcher/PacketMatchTree.h
index 54fc856c..735f0566 100644
--- a/tmtcpacket/packetmatcher/PacketMatchTree.h
+++ b/tmtcpacket/packetmatcher/PacketMatchTree.h
@@ -4,7 +4,7 @@
 #include "../../container/PlacementFactory.h"
 #include "../../globalfunctions/matching/MatchTree.h"
 #include "../../storagemanager/LocalPool.h"
-#include "../../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../../tmtcpacket/pus/tm/TmPacketMinimal.h"
 
 class PacketMatchTree: public MatchTree<TmPacketMinimal*>, public HasReturnvaluesIF {
 public:
diff --git a/tmtcpacket/packetmatcher/ServiceMatcher.h b/tmtcpacket/packetmatcher/ServiceMatcher.h
index eba23d75..57f491f3 100644
--- a/tmtcpacket/packetmatcher/ServiceMatcher.h
+++ b/tmtcpacket/packetmatcher/ServiceMatcher.h
@@ -3,7 +3,7 @@
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
-#include "../../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../pus/tm/TmPacketMinimal.h"
 
 class ServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 private:
diff --git a/tmtcpacket/packetmatcher/SubserviceMatcher.h b/tmtcpacket/packetmatcher/SubserviceMatcher.h
index a9b6def8..80681b5d 100644
--- a/tmtcpacket/packetmatcher/SubserviceMatcher.h
+++ b/tmtcpacket/packetmatcher/SubserviceMatcher.h
@@ -3,7 +3,7 @@
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
-#include "../../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../pus/tm/TmPacketMinimal.h"
 
 class SubServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 public:
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index 9340e3b1..32c80dd3 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -1,14 +1,2 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE
-    TcPacketBase.cpp
-    TcPacketPusA.cpp
-    TcPacketStoredBase.cpp
-    TcPacketStoredPusA.cpp
-
-    TmPacketBase.cpp
-    TmPacketMinimal.cpp
-    TmPacketStoredPusA.cpp
-    TmPacketStoredPusC.cpp
-    TmPacketPusA.cpp
-    TmPacketPusC.cpp
-    TmPacketStoredBase.cpp
-)
+add_subdirectory(tm)
+add_subdirectory(tc)
diff --git a/tmtcpacket/pus/TcPacketPusC.cpp b/tmtcpacket/pus/TcPacketPusC.cpp
deleted file mode 100644
index 1f2946a8..00000000
--- a/tmtcpacket/pus/TcPacketPusC.cpp
+++ /dev/null
@@ -1,4 +0,0 @@
-#include "TcPacketPusC.h"
-
-
-
diff --git a/tmtcpacket/pus/TcPacketPusC.h b/tmtcpacket/pus/TcPacketPusC.h
deleted file mode 100644
index e965e752..00000000
--- a/tmtcpacket/pus/TcPacketPusC.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_
-#define FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_
-
-
-
-
-
-#endif /* FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_ */
diff --git a/tmtcpacket/pus/TcPacketStored.h b/tmtcpacket/pus/TcPacketStored.h
deleted file mode 100644
index 0b6ff0f4..00000000
--- a/tmtcpacket/pus/TcPacketStored.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_
-#define FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_
-
-#include <FSFWConfig.h>
-
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-#include "TcPacketStoredPusC.h"
-#else
-#include "TcPacketStoredPusA.h"
-#endif
-
-
-#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredPusC.cpp b/tmtcpacket/pus/TcPacketStoredPusC.cpp
deleted file mode 100644
index 0ff2376a..00000000
--- a/tmtcpacket/pus/TcPacketStoredPusC.cpp
+++ /dev/null
@@ -1,2 +0,0 @@
-#include "TcPacketStoredPusC.h"
-
diff --git a/tmtcpacket/pus/TcPacketStoredPusC.h b/tmtcpacket/pus/TcPacketStoredPusC.h
deleted file mode 100644
index e8ced948..00000000
--- a/tmtcpacket/pus/TcPacketStoredPusC.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_
-#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_
-
-
-
-
-
-#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_ */
diff --git a/tmtcpacket/pus/tc.h b/tmtcpacket/pus/tc.h
new file mode 100644
index 00000000..156e49fe
--- /dev/null
+++ b/tmtcpacket/pus/tc.h
@@ -0,0 +1,7 @@
+#ifndef FSFW_TMTCPACKET_PUS_TC_H_
+#define FSFW_TMTCPACKET_PUS_TC_H_
+
+#include "tc/TcPacketStoredPus.h"
+#include "tc/TcPacketPus.h"
+
+#endif /* FSFW_TMTCPACKET_PUS_TC_H_ */
diff --git a/tmtcpacket/pus/tc/CMakeLists.txt b/tmtcpacket/pus/tc/CMakeLists.txt
new file mode 100644
index 00000000..723b7943
--- /dev/null
+++ b/tmtcpacket/pus/tc/CMakeLists.txt
@@ -0,0 +1,6 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE    
+    TcPacketBase.cpp
+    TcPacketPus.cpp
+    TcPacketStoredBase.cpp
+    TcPacketStoredPus.cpp
+)
diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/tc/TcPacketBase.cpp
similarity index 72%
rename from tmtcpacket/pus/TcPacketBase.cpp
rename to tmtcpacket/pus/tc/TcPacketBase.cpp
index a5b6f9ce..dd576fec 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/tc/TcPacketBase.cpp
@@ -1,8 +1,8 @@
 #include "TcPacketBase.h"
 
-#include "../../globalfunctions/CRC.h"
-#include "../../globalfunctions/arrayprinter.h"
-#include "../../serviceinterface/ServiceInterface.h"
+#include "../../../globalfunctions/CRC.h"
+#include "../../../globalfunctions/arrayprinter.h"
+#include "../../../serviceinterface/ServiceInterface.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/tc/TcPacketBase.h
similarity index 98%
rename from tmtcpacket/pus/TcPacketBase.h
rename to tmtcpacket/pus/tc/TcPacketBase.h
index 606975e2..f29f9b35 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/tc/TcPacketBase.h
@@ -1,7 +1,7 @@
 #ifndef TMTCPACKET_PUS_TCPACKETBASE_H_
 #define TMTCPACKET_PUS_TCPACKETBASE_H_
 
-#include "../../tmtcpacket/SpacePacketBase.h"
+#include "../../../tmtcpacket/SpacePacketBase.h"
 #include <cstddef>
 
 /**
diff --git a/tmtcpacket/pus/TcPacketPusA.cpp b/tmtcpacket/pus/tc/TcPacketPus.cpp
similarity index 54%
rename from tmtcpacket/pus/TcPacketPusA.cpp
rename to tmtcpacket/pus/tc/TcPacketPus.cpp
index 0ce767b4..7a184f3c 100644
--- a/tmtcpacket/pus/TcPacketPusA.cpp
+++ b/tmtcpacket/pus/tc/TcPacketPus.cpp
@@ -1,39 +1,39 @@
-#include "TcPacketPusA.h"
-#include "../../globalfunctions/CRC.h"
+#include "TcPacketPus.h"
+#include "../../../globalfunctions/CRC.h"
 
 #include <cstring>
 
-TcPacketPusA::TcPacketPusA(const uint8_t *setData): TcPacketBase(setData) {
+TcPacketPus::TcPacketPus(const uint8_t *setData): TcPacketBase(setData) {
     tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(setData));
 }
 
-uint8_t TcPacketPusA::getService() const {
-    return tcData->dataField.service_type;
+uint8_t TcPacketPus::getService() const {
+    return tcData->dataField.serviceType;
 }
 
-uint8_t TcPacketPusA::getSubService() {
-    return tcData->dataField.service_subtype;
+uint8_t TcPacketPus::getSubService() {
+    return tcData->dataField.serviceSubtype;
 }
 
-uint8_t TcPacketPusA::getAcknowledgeFlags() {
-    return tcData->dataField.version_type_ack & 0b00001111;
+uint8_t TcPacketPus::getAcknowledgeFlags() {
+    return tcData->dataField.versionTypeAck & 0b00001111;
 }
 
-const uint8_t* TcPacketPusA::getApplicationData() const {
+const uint8_t* TcPacketPus::getApplicationData() const {
     return &tcData->appData;
 }
 
-uint16_t TcPacketPusA::getApplicationDataSize() {
+uint16_t TcPacketPus::getApplicationDataSize() {
     return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
 }
 
-uint16_t TcPacketPusA::getErrorControl() {
+uint16_t TcPacketPus::getErrorControl() {
     uint16_t size = getApplicationDataSize() + CRC_SIZE;
     uint8_t* p_to_buffer = &tcData->appData;
     return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
 }
 
-void TcPacketPusA::setErrorControl() {
+void TcPacketPus::setErrorControl() {
     uint32_t full_size = getFullSize();
     uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
     uint32_t size = getApplicationDataSize();
@@ -41,35 +41,35 @@ void TcPacketPusA::setErrorControl() {
     (&tcData->appData)[size + 1] = (crc) & 0X00FF;      // CRCL
 }
 
-void TcPacketPusA::setData(const uint8_t* pData) {
+void TcPacketPus::setData(const uint8_t* pData) {
     SpacePacketBase::setData(pData);
     // This function is const-correct, but it was decided to keep the pointer non-const
     // for convenience. Therefore, cast aways constness here and then cast to packet type.
     tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(pData));
 }
 
-uint8_t TcPacketPusA::getSecondaryHeaderFlag() {
-    return (tcData->dataField.version_type_ack & 0b10000000) >> 7;
+uint8_t TcPacketPus::getSecondaryHeaderFlag() {
+    return (tcData->dataField.versionTypeAck & 0b10000000) >> 7;
 }
 
-uint8_t TcPacketPusA::getPusVersionNumber() {
-    return (tcData->dataField.version_type_ack & 0b01110000) >> 4;
+uint8_t TcPacketPus::getPusVersionNumber() {
+    return (tcData->dataField.versionTypeAck & 0b01110000) >> 4;
 }
 
-void TcPacketPusA::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
+void TcPacketPus::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
         uint8_t ack, uint8_t service, uint8_t subservice) {
     initSpacePacketHeader(true, true, apid, sequenceCount);
     std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
     setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
     // Data Field Header:
     // Set CCSDS_secondary_header_flag to 0 and version number to 001
-    tcData->dataField.version_type_ack = 0b00010000;
-    tcData->dataField.version_type_ack |= (ack & 0x0F);
-    tcData->dataField.service_type = service;
-    tcData->dataField.service_subtype = subservice;
+    tcData->dataField.versionTypeAck = 0b00010000;
+    tcData->dataField.versionTypeAck |= (ack & 0x0F);
+    tcData->dataField.serviceType = service;
+    tcData->dataField.serviceSubtype = subservice;
 }
 
-size_t TcPacketPusA::calculateFullPacketLength(size_t appDataLen) {
+size_t TcPacketPus::calculateFullPacketLength(size_t appDataLen) {
     return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
             appDataLen + TcPacketBase::CRC_SIZE;
 }
diff --git a/tmtcpacket/pus/TcPacketPusA.h b/tmtcpacket/pus/tc/TcPacketPus.h
similarity index 87%
rename from tmtcpacket/pus/TcPacketPusA.h
rename to tmtcpacket/pus/tc/TcPacketPus.h
index 0e61d829..fb8483c5 100644
--- a/tmtcpacket/pus/TcPacketPusA.h
+++ b/tmtcpacket/pus/tc/TcPacketPus.h
@@ -1,7 +1,8 @@
 #ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
 #define FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
 
-#include "../ccsds_header.h"
+#include "../../../FSFW.h"
+#include "../../ccsds_header.h"
 #include "TcPacketBase.h"
 
 #include <cstdint>
@@ -13,10 +14,14 @@
  * @ingroup tmtcpackets
  */
 struct PUSTcDataFieldHeader {
-    uint8_t version_type_ack;
-    uint8_t service_type;
-    uint8_t service_subtype;
-    uint8_t source_id;
+    uint8_t versionTypeAck;
+    uint8_t serviceType;
+    uint8_t serviceSubtype;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    uint16_t sourceId;
+#else
+    uint8_t sourceId;
+#endif
 };
 
 /**
@@ -31,7 +36,7 @@ struct TcPacketPointer {
 };
 
 
-class TcPacketPusA: public TcPacketBase {
+class TcPacketPus: public TcPacketBase {
 public:
     static const uint16_t TC_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
             sizeof(PUSTcDataFieldHeader) + 2);
@@ -41,7 +46,7 @@ public:
      * create an empty (invalid) object by passing nullptr as the data pointer
      * @param setData
      */
-    TcPacketPusA(const uint8_t* setData);
+    TcPacketPus(const uint8_t* setData);
 
     // Base class overrides
     virtual uint8_t getSecondaryHeaderFlag() override;
diff --git a/tmtcpacket/pus/TcPacketStoredBase.cpp b/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
similarity index 92%
rename from tmtcpacket/pus/TcPacketStoredBase.cpp
rename to tmtcpacket/pus/tc/TcPacketStoredBase.cpp
index 629562c0..daae35f5 100644
--- a/tmtcpacket/pus/TcPacketStoredBase.cpp
+++ b/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
@@ -1,8 +1,8 @@
 #include "TcPacketStoredBase.h"
 
-#include "../../objectmanager/ObjectManager.h"
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../objectmanager/frameworkObjects.h"
+#include "../../../objectmanager/ObjectManager.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../objectmanager/frameworkObjects.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TcPacketStoredBase.h b/tmtcpacket/pus/tc/TcPacketStoredBase.h
similarity index 96%
rename from tmtcpacket/pus/TcPacketStoredBase.h
rename to tmtcpacket/pus/tc/TcPacketStoredBase.h
index 811123db..b7d73531 100644
--- a/tmtcpacket/pus/TcPacketStoredBase.h
+++ b/tmtcpacket/pus/tc/TcPacketStoredBase.h
@@ -1,8 +1,8 @@
 #ifndef TMTCPACKET_PUS_TCPACKETSTORED_H_
 #define TMTCPACKET_PUS_TCPACKETSTORED_H_
 
-#include <fsfw/tmtcpacket/pus/TcPacketStoredIF.h>
-#include "../../storagemanager/StorageManagerIF.h"
+#include "TcPacketStoredIF.h"
+#include "../../../storagemanager/StorageManagerIF.h"
 
 /**
  *	This class generates a ECSS PUS Telecommand packet within a given
diff --git a/tmtcpacket/pus/TcPacketStoredIF.h b/tmtcpacket/pus/tc/TcPacketStoredIF.h
similarity index 88%
rename from tmtcpacket/pus/TcPacketStoredIF.h
rename to tmtcpacket/pus/tc/TcPacketStoredIF.h
index 8eb23004..3d356725 100644
--- a/tmtcpacket/pus/TcPacketStoredIF.h
+++ b/tmtcpacket/pus/tc/TcPacketStoredIF.h
@@ -1,9 +1,9 @@
 #ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
 #define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
 
-#include "../../tmtcpacket/pus/TcPacketBase.h"
-#include "../../storagemanager/storeAddress.h"
-#include "../../returnvalues/HasReturnvaluesIF.h"
+#include "TcPacketBase.h"
+#include "../../../storagemanager/storeAddress.h"
+#include "../../../returnvalues/HasReturnvaluesIF.h"
 
 class TcPacketStoredIF {
 public:
diff --git a/tmtcpacket/pus/TcPacketStoredPusA.cpp b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
similarity index 78%
rename from tmtcpacket/pus/TcPacketStoredPusA.cpp
rename to tmtcpacket/pus/tc/TcPacketStoredPus.cpp
index 7c706a1a..f098ce2b 100644
--- a/tmtcpacket/pus/TcPacketStoredPusA.cpp
+++ b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
@@ -1,11 +1,11 @@
-#include "TcPacketStoredPusA.h"
+#include "TcPacketStoredPus.h"
 
 #include <cstring>
 
-TcPacketStoredPusA::TcPacketStoredPusA(uint16_t apid,  uint8_t service,
+TcPacketStoredPus::TcPacketStoredPus(uint16_t apid,  uint8_t service,
         uint8_t subservice, uint8_t sequenceCount, const uint8_t* data,
         size_t size, uint8_t ack) :
-        TcPacketPusA(nullptr) {
+        TcPacketPus(nullptr) {
     this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
     if (not this->checkAndSetStore()) {
         return;
@@ -28,14 +28,14 @@ TcPacketStoredPusA::TcPacketStoredPusA(uint16_t apid,  uint8_t service,
     this->setErrorControl();
 }
 
-TcPacketStoredPusA::TcPacketStoredPusA(): TcPacketStoredBase(), TcPacketPusA(nullptr) {
+TcPacketStoredPus::TcPacketStoredPus(): TcPacketStoredBase(), TcPacketPus(nullptr) {
 }
 
-TcPacketStoredPusA::TcPacketStoredPusA(store_address_t setAddress): TcPacketPusA(nullptr) {
+TcPacketStoredPus::TcPacketStoredPus(store_address_t setAddress): TcPacketPus(nullptr) {
     TcPacketStoredBase::setStoreAddress(setAddress);
 }
 
-TcPacketStoredPusA::TcPacketStoredPusA(const uint8_t* data, size_t size): TcPacketPusA(data) {
+TcPacketStoredPus::TcPacketStoredPus(const uint8_t* data, size_t size): TcPacketPus(data) {
     if (this->getFullSize() != size) {
         return;
     }
@@ -51,19 +51,19 @@ TcPacketStoredPusA::TcPacketStoredPusA(const uint8_t* data, size_t size): TcPack
     }
 }
 
-ReturnValue_t TcPacketStoredPusA::deletePacket() {
+ReturnValue_t TcPacketStoredPus::deletePacket() {
     ReturnValue_t result = this->store->deleteData(this->storeAddress);
     this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
     this->setData(nullptr);
     return result;
 }
 
-TcPacketBase* TcPacketStoredPusA::getPacketBase() {
+TcPacketBase* TcPacketStoredPus::getPacketBase() {
     return this;
 }
 
 
-bool TcPacketStoredPusA::isSizeCorrect() {
+bool TcPacketStoredPus::isSizeCorrect() {
     const uint8_t* temp_data = nullptr;
     size_t temp_size;
     ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data,
diff --git a/tmtcpacket/pus/TcPacketStoredPusA.h b/tmtcpacket/pus/tc/TcPacketStoredPus.h
similarity index 84%
rename from tmtcpacket/pus/TcPacketStoredPusA.h
rename to tmtcpacket/pus/tc/TcPacketStoredPus.h
index fe1f64f7..8b318733 100644
--- a/tmtcpacket/pus/TcPacketStoredPusA.h
+++ b/tmtcpacket/pus/tc/TcPacketStoredPus.h
@@ -2,11 +2,11 @@
 #define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_
 
 #include "TcPacketStoredBase.h"
-#include "TcPacketPusA.h"
+#include "TcPacketPus.h"
 
-class TcPacketStoredPusA:
+class TcPacketStoredPus:
         public TcPacketStoredBase,
-        public TcPacketPusA {
+        public TcPacketPus {
 public:
     /**
      * With this constructor, new space is allocated in the packet store and
@@ -24,7 +24,7 @@ public:
      *                      number of verification packets returned
      *                      for this command.
      */
-    TcPacketStoredPusA(uint16_t apid, uint8_t service, uint8_t subservice,
+    TcPacketStoredPus(uint16_t apid, uint8_t service, uint8_t subservice,
             uint8_t sequence_count = 0, const uint8_t* data = nullptr,
             size_t size = 0, uint8_t ack = TcPacketBase::ACK_ALL);
     /**
@@ -32,13 +32,13 @@ public:
      * @param data
      * @param size
      */
-    TcPacketStoredPusA(const uint8_t* data, size_t size);
+    TcPacketStoredPus(const uint8_t* data, size_t size);
     /**
      * Create stored packet from existing packet in store
      * @param setAddress
      */
-    TcPacketStoredPusA(store_address_t setAddress);
-    TcPacketStoredPusA();
+    TcPacketStoredPus(store_address_t setAddress);
+    TcPacketStoredPus();
 
     ReturnValue_t deletePacket() override;
     TcPacketBase* getPacketBase() override;
diff --git a/tmtcpacket/pus/tm.h b/tmtcpacket/pus/tm.h
new file mode 100644
index 00000000..591ada7c
--- /dev/null
+++ b/tmtcpacket/pus/tm.h
@@ -0,0 +1,16 @@
+#ifndef FSFW_TMTCPACKET_PUS_TM_H_
+#define FSFW_TMTCPACKET_PUS_TM_H_
+
+#include "../../FSFW.h"
+
+#if FSFW_USE_PUS_C_TELEMETRY == 1
+#include "tm/TmPacketPusC.h"
+#include "tm/TmPacketStoredPusC.h"
+#else
+#include "tm/TmPacketPusA.h"
+#include "tm/TmPacketStoredPusA.h"
+#endif
+
+#include "tm/TmPacketMinimal.h"
+
+#endif /* FSFW_TMTCPACKET_PUS_TM_H_ */
diff --git a/tmtcpacket/pus/tm/CMakeLists.txt b/tmtcpacket/pus/tm/CMakeLists.txt
new file mode 100644
index 00000000..ace87820
--- /dev/null
+++ b/tmtcpacket/pus/tm/CMakeLists.txt
@@ -0,0 +1,9 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE
+    TmPacketStoredPusA.cpp
+    TmPacketStoredPusC.cpp
+    TmPacketPusA.cpp
+    TmPacketPusC.cpp
+    TmPacketStoredBase.cpp
+    TmPacketBase.cpp
+    TmPacketMinimal.cpp
+)
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/tm/TmPacketBase.cpp
similarity index 88%
rename from tmtcpacket/pus/TmPacketBase.cpp
rename to tmtcpacket/pus/tm/TmPacketBase.cpp
index b144db1b..91ec7d58 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/tm/TmPacketBase.cpp
@@ -1,10 +1,10 @@
 #include "TmPacketBase.h"
 
-#include "../../globalfunctions/CRC.h"
-#include "../../globalfunctions/arrayprinter.h"
-#include "../../objectmanager/ObjectManager.h"
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../timemanager/CCSDSTime.h"
+#include "../../../globalfunctions/CRC.h"
+#include "../../../globalfunctions/arrayprinter.h"
+#include "../../../objectmanager/ObjectManager.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../timemanager/CCSDSTime.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketBase.h b/tmtcpacket/pus/tm/TmPacketBase.h
similarity index 96%
rename from tmtcpacket/pus/TmPacketBase.h
rename to tmtcpacket/pus/tm/TmPacketBase.h
index 6925e99b..9f534f29 100644
--- a/tmtcpacket/pus/TmPacketBase.h
+++ b/tmtcpacket/pus/tm/TmPacketBase.h
@@ -1,10 +1,10 @@
 #ifndef TMTCPACKET_PUS_TMPACKETBASE_H_
 #define TMTCPACKET_PUS_TMPACKETBASE_H_
 
-#include "../SpacePacketBase.h"
-#include "../../timemanager/TimeStamperIF.h"
-#include "../../timemanager/Clock.h"
-#include "../../objectmanager/SystemObjectIF.h"
+#include "../../SpacePacketBase.h"
+#include "../../../timemanager/TimeStamperIF.h"
+#include "../../../timemanager/Clock.h"
+#include "../../../objectmanager/SystemObjectIF.h"
 
 namespace Factory{
 void setStaticFrameworkObjectIds();
diff --git a/tmtcpacket/pus/TmPacketMinimal.cpp b/tmtcpacket/pus/tm/TmPacketMinimal.cpp
similarity index 93%
rename from tmtcpacket/pus/TmPacketMinimal.cpp
rename to tmtcpacket/pus/tm/TmPacketMinimal.cpp
index 18e9dda1..3f785cde 100644
--- a/tmtcpacket/pus/TmPacketMinimal.cpp
+++ b/tmtcpacket/pus/tm/TmPacketMinimal.cpp
@@ -1,7 +1,8 @@
 #include "TmPacketMinimal.h"
-#include <stddef.h>
-#include <time.h>
-#include "PacketTimestampInterpreterIF.h"
+#include "../PacketTimestampInterpreterIF.h"
+
+#include <cstddef>
+#include <ctime>
 
 TmPacketMinimal::TmPacketMinimal(const uint8_t* set_data) : SpacePacketBase( set_data ) {
 	this->tm_data = (TmPacketMinimalPointer*)set_data;
diff --git a/tmtcpacket/pus/TmPacketMinimal.h b/tmtcpacket/pus/tm/TmPacketMinimal.h
similarity index 96%
rename from tmtcpacket/pus/TmPacketMinimal.h
rename to tmtcpacket/pus/tm/TmPacketMinimal.h
index 728acb15..08daa584 100644
--- a/tmtcpacket/pus/TmPacketMinimal.h
+++ b/tmtcpacket/pus/tm/TmPacketMinimal.h
@@ -2,8 +2,8 @@
 #define FRAMEWORK_TMTCPACKET_PUS_TMPACKETMINIMAL_H_
 
 
-#include "../../tmtcpacket/SpacePacketBase.h"
-#include "../../returnvalues/HasReturnvaluesIF.h"
+#include "../../SpacePacketBase.h"
+#include "../../../returnvalues/HasReturnvaluesIF.h"
 
 struct timeval;
 class PacketTimestampInterpreterIF;
diff --git a/tmtcpacket/pus/TmPacketPusA.cpp b/tmtcpacket/pus/tm/TmPacketPusA.cpp
similarity index 90%
rename from tmtcpacket/pus/TmPacketPusA.cpp
rename to tmtcpacket/pus/tm/TmPacketPusA.cpp
index d96f6aa7..bdc0a815 100644
--- a/tmtcpacket/pus/TmPacketPusA.cpp
+++ b/tmtcpacket/pus/tm/TmPacketPusA.cpp
@@ -1,11 +1,11 @@
 #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 "../../../globalfunctions/CRC.h"
+#include "../../../globalfunctions/arrayprinter.h"
+#include "../../../objectmanager/ObjectManagerIF.h"
+#include "../../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../../timemanager/CCSDSTime.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketPusA.h b/tmtcpacket/pus/tm/TmPacketPusA.h
similarity index 95%
rename from tmtcpacket/pus/TmPacketPusA.h
rename to tmtcpacket/pus/tm/TmPacketPusA.h
index dd9a5d09..486b68f3 100644
--- a/tmtcpacket/pus/TmPacketPusA.h
+++ b/tmtcpacket/pus/tm/TmPacketPusA.h
@@ -2,10 +2,10 @@
 #define FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_
 
 #include "TmPacketBase.h"
-#include "../SpacePacketBase.h"
-#include "../../timemanager/TimeStamperIF.h"
-#include "../../timemanager/Clock.h"
-#include "../../objectmanager/SystemObjectIF.h"
+#include "../../SpacePacketBase.h"
+#include "../../../timemanager/TimeStamperIF.h"
+#include "../../../timemanager/Clock.h"
+#include "../../../objectmanager/SystemObjectIF.h"
 
 namespace Factory{
 void setStaticFrameworkObjectIds();
diff --git a/tmtcpacket/pus/TmPacketPusC.cpp b/tmtcpacket/pus/tm/TmPacketPusC.cpp
similarity index 90%
rename from tmtcpacket/pus/TmPacketPusC.cpp
rename to tmtcpacket/pus/tm/TmPacketPusC.cpp
index ca2bccdb..5090aaeb 100644
--- a/tmtcpacket/pus/TmPacketPusC.cpp
+++ b/tmtcpacket/pus/tm/TmPacketPusC.cpp
@@ -1,11 +1,11 @@
 #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 "../../../globalfunctions/CRC.h"
+#include "../../../globalfunctions/arrayprinter.h"
+#include "../../../objectmanager/ObjectManagerIF.h"
+#include "../../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../../timemanager/CCSDSTime.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketPusC.h b/tmtcpacket/pus/tm/TmPacketPusC.h
similarity index 95%
rename from tmtcpacket/pus/TmPacketPusC.h
rename to tmtcpacket/pus/tm/TmPacketPusC.h
index fdc27548..97e5a9a5 100644
--- a/tmtcpacket/pus/TmPacketPusC.h
+++ b/tmtcpacket/pus/tm/TmPacketPusC.h
@@ -2,10 +2,10 @@
 #define FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
 
 #include "TmPacketBase.h"
-#include "../SpacePacketBase.h"
-#include "../../timemanager/TimeStamperIF.h"
-#include "../../timemanager/Clock.h"
-#include "../../objectmanager/SystemObjectIF.h"
+#include "../../SpacePacketBase.h"
+#include "../../../timemanager/TimeStamperIF.h"
+#include "../../../timemanager/Clock.h"
+#include "../../../objectmanager/SystemObjectIF.h"
 
 namespace Factory{
 void setStaticFrameworkObjectIds();
diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/tm/TmPacketStored.h
similarity index 100%
rename from tmtcpacket/pus/TmPacketStored.h
rename to tmtcpacket/pus/tm/TmPacketStored.h
diff --git a/tmtcpacket/pus/TmPacketStoredBase.cpp b/tmtcpacket/pus/tm/TmPacketStoredBase.cpp
similarity index 94%
rename from tmtcpacket/pus/TmPacketStoredBase.cpp
rename to tmtcpacket/pus/tm/TmPacketStoredBase.cpp
index eeaa938d..ba8b15d1 100644
--- a/tmtcpacket/pus/TmPacketStoredBase.cpp
+++ b/tmtcpacket/pus/tm/TmPacketStoredBase.cpp
@@ -1,8 +1,8 @@
 #include "TmPacketStoredBase.h"
 
-#include "../../objectmanager/ObjectManager.h"
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../tmtcservices/TmTcMessage.h"
+#include "../../../objectmanager/ObjectManager.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../tmtcservices/TmTcMessage.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketStoredBase.h b/tmtcpacket/pus/tm/TmPacketStoredBase.h
similarity index 91%
rename from tmtcpacket/pus/TmPacketStoredBase.h
rename to tmtcpacket/pus/tm/TmPacketStoredBase.h
index dd7e31eb..1bc092dd 100644
--- a/tmtcpacket/pus/TmPacketStoredBase.h
+++ b/tmtcpacket/pus/tm/TmPacketStoredBase.h
@@ -1,15 +1,15 @@
 #ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
 #define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
 
+#include "../../../FSFW.h"
 #include "TmPacketBase.h"
 #include "TmPacketStoredBase.h"
-#include <FSFWConfig.h>
+#include "TmPacketPusA.h"
 
-#include "../../tmtcpacket/pus/TmPacketPusA.h"
-#include "../../serialize/SerializeIF.h"
-#include "../../storagemanager/StorageManagerIF.h"
-#include "../../internalError/InternalErrorReporterIF.h"
-#include "../../ipc/MessageQueueSenderIF.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
diff --git a/tmtcpacket/pus/TmPacketStoredPusA.cpp b/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp
similarity index 95%
rename from tmtcpacket/pus/TmPacketStoredPusA.cpp
rename to tmtcpacket/pus/tm/TmPacketStoredPusA.cpp
index 68102b62..02fb77ef 100644
--- a/tmtcpacket/pus/TmPacketStoredPusA.cpp
+++ b/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp
@@ -1,7 +1,7 @@
 #include "TmPacketStoredPusA.h"
 
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../tmtcservices/TmTcMessage.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../tmtcservices/TmTcMessage.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketStoredPusA.h b/tmtcpacket/pus/tm/TmPacketStoredPusA.h
similarity index 100%
rename from tmtcpacket/pus/TmPacketStoredPusA.h
rename to tmtcpacket/pus/tm/TmPacketStoredPusA.h
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.cpp b/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
similarity index 96%
rename from tmtcpacket/pus/TmPacketStoredPusC.cpp
rename to tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
index 7f774411..6f8f7fa2 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.cpp
+++ b/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
@@ -1,7 +1,7 @@
 #include "TmPacketStoredPusC.h"
 
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../tmtcservices/TmTcMessage.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../tmtcservices/TmTcMessage.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.h b/tmtcpacket/pus/tm/TmPacketStoredPusC.h
similarity index 96%
rename from tmtcpacket/pus/TmPacketStoredPusC.h
rename to tmtcpacket/pus/tm/TmPacketStoredPusC.h
index 883dc0ff..35c66083 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.h
+++ b/tmtcpacket/pus/tm/TmPacketStoredPusC.h
@@ -1,8 +1,8 @@
 #ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
 #define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
 
-#include <fsfw/tmtcpacket/pus/TmPacketPusC.h>
-#include <fsfw/tmtcpacket/pus/TmPacketStoredBase.h>
+#include "TmPacketPusC.h"
+#include "TmPacketStoredBase.h"
 
 /**
  *  This class generates a ECSS PUS C Telemetry packet within a given
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index 0ede27dc..b416ffbe 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -6,8 +6,8 @@
 #include "../tcdistribution/PUSDistributorIF.h"
 #include "../objectmanager/ObjectManager.h"
 #include "../ipc/QueueFactory.h"
-#include "../tmtcpacket/pus/TcPacketStored.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/tc.h"
+#include "../tmtcpacket/pus/tm.h"
 #include "../serviceinterface/ServiceInterface.h"
 
 object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
@@ -246,11 +246,7 @@ void CommandingServiceBase::handleRequestQueue() {
 	TmTcMessage message;
 	ReturnValue_t result;
 	store_address_t address;
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-	TcPacketStoredPusC packet;
-#else
-	TcPacketStoredPusA packet;
-#endif
+	TcPacketStoredPus packet;
 	MessageQueueId_t queue;
 	object_id_t objectId;
 	for (result = requestQueue->receiveMessage(&message); result == RETURN_OK;
@@ -436,11 +432,7 @@ void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
 	if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
 		commandMap.erase(&iter);
 	} else {
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-		TcPacketStoredPusC newPacket(address);
-#else
-		TcPacketStoredPusA newPacket(address);
-#endif
+		TcPacketStoredPus newPacket(address);
 		startExecution(&newPacket, iter);
 	}
 }
diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h
index 46191bd5..4ee4a21a 100644
--- a/tmtcservices/CommandingServiceBase.h
+++ b/tmtcservices/CommandingServiceBase.h
@@ -1,7 +1,6 @@
 #ifndef FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 #define FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 
-#include <fsfw/tmtcpacket/pus/TcPacketStoredBase.h>
 #include "AcceptsTelecommandsIF.h"
 #include "VerificationReporter.h"
 
@@ -17,6 +16,7 @@
 #include <FSFWConfig.h>
 
 class TcPacketStored;
+class TcPacketStoredBase;
 
 namespace Factory{
 void setStaticFrameworkObjectIds();
diff --git a/tmtcservices/PusServiceBase.h b/tmtcservices/PusServiceBase.h
index f48612b1..6cde6fb4 100644
--- a/tmtcservices/PusServiceBase.h
+++ b/tmtcservices/PusServiceBase.h
@@ -9,7 +9,7 @@
 #include "../objectmanager/SystemObject.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../tasks/ExecutableObjectIF.h"
-#include "../tmtcpacket/pus/TcPacketStored.h"
+#include "../tmtcpacket/pus/tc.h"
 #include "../ipc/MessageQueueIF.h"
 
 namespace Factory{
@@ -141,11 +141,7 @@ protected:
 	 * The current Telecommand to be processed.
 	 * It is deleted after handleRequest was executed.
 	 */
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-	TcPacketStoredPusC currentPacket;
-#else
-    TcPacketStoredPusA currentPacket;
-#endif
+	TcPacketStoredPus currentPacket;
 
 	static object_id_t packetSource;
 
diff --git a/tmtcservices/PusVerificationReport.h b/tmtcservices/PusVerificationReport.h
index 9dce95ac..0e1732ef 100644
--- a/tmtcservices/PusVerificationReport.h
+++ b/tmtcservices/PusVerificationReport.h
@@ -4,7 +4,7 @@
 #include "VerificationCodes.h"
 
 #include "../ipc/MessageQueueMessage.h"
-#include "../tmtcpacket/pus/TcPacketBase.h"
+#include "../tmtcpacket/pus/tc/TcPacketBase.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 
 class PusVerificationMessage: public MessageQueueMessage {

From b1e3a1b2b586809e45b0a741f9e62fc1d2125093 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 11:16:56 +0200
Subject: [PATCH 134/157] const correctness

---
 tmtcpacket/SpacePacketBase.cpp         |  2 +-
 tmtcpacket/SpacePacketBase.h           |  6 ++--
 tmtcpacket/pus/tc/TcPacketBase.h       | 16 +++++-----
 tmtcpacket/pus/tc/TcPacketPus.cpp      | 44 +++++++++++++++-----------
 tmtcpacket/pus/tc/TcPacketPus.h        | 20 ++++++------
 tmtcservices/CommandingServiceBase.cpp |  1 +
 6 files changed, 51 insertions(+), 38 deletions(-)

diff --git a/tmtcpacket/SpacePacketBase.cpp b/tmtcpacket/SpacePacketBase.cpp
index e13af8d0..14198027 100644
--- a/tmtcpacket/SpacePacketBase.cpp
+++ b/tmtcpacket/SpacePacketBase.cpp
@@ -72,7 +72,7 @@ void SpacePacketBase::setPacketSequenceCount( uint16_t new_count) {
 	this->data->header.sequence_control_l = ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x00FF );
 }
 
-uint16_t SpacePacketBase::getPacketDataLength( void ) {
+uint16_t SpacePacketBase::getPacketDataLength() const {
 	return ( (this->data->header.packet_length_h) << 8 )
 		+ this->data->header.packet_length_l;
 }
diff --git a/tmtcpacket/SpacePacketBase.h b/tmtcpacket/SpacePacketBase.h
index 19cbd074..13cb3130 100644
--- a/tmtcpacket/SpacePacketBase.h
+++ b/tmtcpacket/SpacePacketBase.h
@@ -138,9 +138,11 @@ public:
 	 * Returns the packet data length, which is the fifth and sixth byte of the
 	 * CCSDS Primary Header. The packet data length is the size of every kind
 	 * of data \b after the CCSDS Primary Header \b -1.
-	 * @return The CCSDS packet data length.
+	 * @return
+	 * The CCSDS packet data length. uint16_t is sufficient,
+	 * because this is limit in CCSDS standard
 	 */
-	uint16_t getPacketDataLength( void ); //uint16_t is sufficient, because this is limit in CCSDS standard
+	uint16_t getPacketDataLength(void) const;
 	/**
 	 * Sets the packet data length, which is the fifth and sixth byte of the
 	 * CCSDS Primary Header.
diff --git a/tmtcpacket/pus/tc/TcPacketBase.h b/tmtcpacket/pus/tc/TcPacketBase.h
index f29f9b35..68f08896 100644
--- a/tmtcpacket/pus/tc/TcPacketBase.h
+++ b/tmtcpacket/pus/tc/TcPacketBase.h
@@ -53,7 +53,7 @@ public:
 	 * highest bit of the first byte of the Data Field Header.
 	 * @return	the CCSDS Secondary Header Flag
 	 */
-	virtual uint8_t getSecondaryHeaderFlag() = 0;
+	virtual uint8_t getSecondaryHeaderFlag() const = 0;
 	/**
 	 * This command returns the TC Packet PUS Version Number.
 	 * The version number of ECSS PUS 2003 is 1.
@@ -61,7 +61,7 @@ public:
 	 * first byte.
 	 * @return
 	 */
-	virtual uint8_t getPusVersionNumber() = 0;
+	virtual uint8_t getPusVersionNumber() const = 0;
 	/**
 	 * This is a getter for the packet's Ack field, which are the lowest four
 	 * bits of the first byte of the Data Field Header.
@@ -69,7 +69,7 @@ public:
 	 * It is packed in a uint8_t variable.
 	 * @return	The packet's PUS Ack field.
 	 */
-	virtual uint8_t getAcknowledgeFlags() = 0;
+	virtual uint8_t getAcknowledgeFlags() const = 0;
 	/**
 	 * This is a getter for the packet's PUS Service ID, which is the second
 	 * byte of the Data Field Header.
@@ -81,13 +81,13 @@ public:
 	 * third byte of the Data Field Header.
 	 * @return	The packet's PUS Service Subtype.
 	 */
-	virtual uint8_t getSubService() = 0;
+	virtual uint8_t getSubService() const = 0;
 	/**
 	 * The source ID can be used to have an additional identifier, e.g. for different ground
 	 * station.
 	 * @return
 	 */
-	uint8_t getSourceId();
+	virtual uint16_t getSourceId() const = 0;
 
 	/**
 	 * This is a getter for a pointer to the packet's Application data.
@@ -105,7 +105,7 @@ public:
 	 * @return	The size of the PUS Application Data (without Error Control
 	 * 		field)
 	 */
-	virtual uint16_t getApplicationDataSize() = 0;
+	virtual uint16_t getApplicationDataSize() const = 0;
 	/**
 	 * This getter returns the Error Control Field of the packet.
 	 *
@@ -114,7 +114,7 @@ public:
 	 * supposed to be a 16bit-CRC.
 	 * @return	The PUS Error Control
 	 */
-	virtual uint16_t getErrorControl() = 0;
+	virtual uint16_t getErrorControl() const = 0;
 	/**
 	 * With this method, the Error Control Field is updated to match the
 	 * current content of the packet.
@@ -126,7 +126,7 @@ public:
 	 * @param appDataLen
 	 * @return
 	 */
-	virtual size_t calculateFullPacketLength(size_t appDataLen) = 0;
+	virtual size_t calculateFullPacketLength(size_t appDataLen) const = 0;
 
     /**
      * This is a debugging helper method that prints the whole packet content
diff --git a/tmtcpacket/pus/tc/TcPacketPus.cpp b/tmtcpacket/pus/tc/TcPacketPus.cpp
index 7a184f3c..1df61c07 100644
--- a/tmtcpacket/pus/tc/TcPacketPus.cpp
+++ b/tmtcpacket/pus/tc/TcPacketPus.cpp
@@ -7,15 +7,28 @@ TcPacketPus::TcPacketPus(const uint8_t *setData): TcPacketBase(setData) {
     tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(setData));
 }
 
+void TcPacketPus::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
+        uint8_t ack, uint8_t service, uint8_t subservice) {
+    initSpacePacketHeader(true, true, apid, sequenceCount);
+    std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
+    setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
+    // Data Field Header:
+    // Set CCSDS_secondary_header_flag to 0 and version number to 001
+    tcData->dataField.versionTypeAck = 0b00010000;
+    tcData->dataField.versionTypeAck |= (ack & 0x0F);
+    tcData->dataField.serviceType = service;
+    tcData->dataField.serviceSubtype = subservice;
+}
+
 uint8_t TcPacketPus::getService() const {
     return tcData->dataField.serviceType;
 }
 
-uint8_t TcPacketPus::getSubService() {
+uint8_t TcPacketPus::getSubService() const {
     return tcData->dataField.serviceSubtype;
 }
 
-uint8_t TcPacketPus::getAcknowledgeFlags() {
+uint8_t TcPacketPus::getAcknowledgeFlags() const {
     return tcData->dataField.versionTypeAck & 0b00001111;
 }
 
@@ -23,11 +36,11 @@ const uint8_t* TcPacketPus::getApplicationData() const {
     return &tcData->appData;
 }
 
-uint16_t TcPacketPus::getApplicationDataSize() {
+uint16_t TcPacketPus::getApplicationDataSize() const {
     return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
 }
 
-uint16_t TcPacketPus::getErrorControl() {
+uint16_t TcPacketPus::getErrorControl() const {
     uint16_t size = getApplicationDataSize() + CRC_SIZE;
     uint8_t* p_to_buffer = &tcData->appData;
     return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
@@ -48,28 +61,23 @@ void TcPacketPus::setData(const uint8_t* pData) {
     tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(pData));
 }
 
-uint8_t TcPacketPus::getSecondaryHeaderFlag() {
+uint8_t TcPacketPus::getSecondaryHeaderFlag() const {
     return (tcData->dataField.versionTypeAck & 0b10000000) >> 7;
 }
 
-uint8_t TcPacketPus::getPusVersionNumber() {
+uint8_t TcPacketPus::getPusVersionNumber() const {
     return (tcData->dataField.versionTypeAck & 0b01110000) >> 4;
 }
 
-void TcPacketPus::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
-        uint8_t ack, uint8_t service, uint8_t subservice) {
-    initSpacePacketHeader(true, true, apid, sequenceCount);
-    std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
-    setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
-    // Data Field Header:
-    // Set CCSDS_secondary_header_flag to 0 and version number to 001
-    tcData->dataField.versionTypeAck = 0b00010000;
-    tcData->dataField.versionTypeAck |= (ack & 0x0F);
-    tcData->dataField.serviceType = service;
-    tcData->dataField.serviceSubtype = subservice;
+uint16_t TcPacketPus::getSourceId() const {
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    return (tcData->dataField.sourceIdH << 8) | tcData->dataField.sourceIdL;
+#else
+    return tcData->dataField.sourceId;
+#endif
 }
 
-size_t TcPacketPus::calculateFullPacketLength(size_t appDataLen) {
+size_t TcPacketPus::calculateFullPacketLength(size_t appDataLen) const {
     return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
             appDataLen + TcPacketBase::CRC_SIZE;
 }
diff --git a/tmtcpacket/pus/tc/TcPacketPus.h b/tmtcpacket/pus/tc/TcPacketPus.h
index fb8483c5..b50b868d 100644
--- a/tmtcpacket/pus/tc/TcPacketPus.h
+++ b/tmtcpacket/pus/tc/TcPacketPus.h
@@ -18,7 +18,8 @@ struct PUSTcDataFieldHeader {
     uint8_t serviceType;
     uint8_t serviceSubtype;
 #if FSFW_USE_PUS_C_TELECOMMANDS == 1
-    uint16_t sourceId;
+    uint8_t sourceIdH;
+    uint8_t sourceIdL;
 #else
     uint8_t sourceId;
 #endif
@@ -49,16 +50,17 @@ public:
     TcPacketPus(const uint8_t* setData);
 
     // Base class overrides
-    virtual uint8_t getSecondaryHeaderFlag() override;
-    virtual uint8_t getPusVersionNumber() override;
-    virtual uint8_t getAcknowledgeFlags() override;
-    virtual uint8_t getService() const override;
-    virtual uint8_t getSubService() override;
+    uint8_t getSecondaryHeaderFlag() const override;
+    uint8_t getPusVersionNumber() const override;
+    uint8_t getAcknowledgeFlags() const override;
+    uint8_t getService() const override;
+    uint8_t getSubService() const override;
+    uint16_t getSourceId() const override;
     const uint8_t* getApplicationData() const override;
-    uint16_t getApplicationDataSize() override;
-    uint16_t getErrorControl() override;
+    uint16_t getApplicationDataSize() const override;
+    uint16_t getErrorControl() const override;
     void setErrorControl() override;
-    size_t calculateFullPacketLength(size_t appDataLen) override;
+    size_t calculateFullPacketLength(size_t appDataLen) const override;
 
 protected:
 
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index b416ffbe..307a2a98 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -259,6 +259,7 @@ void CommandingServiceBase::handleRequestQueue() {
 			rejectPacket(tc_verification::START_FAILURE, &packet, INVALID_SUBSERVICE);
 			continue;
 		}
+
 		result = getMessageQueueAndObject(packet.getSubService(),
 				packet.getApplicationData(), packet.getApplicationDataSize(),
 				&queue, &objectId);

From 7cf4aa0d5a8cc707721785e9a55f1fd0e2eaf652 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 11:44:39 +0200
Subject: [PATCH 135/157] fixes for pus tc c

---
 tcdistribution/TcPacketCheck.cpp  | 1 +
 tcdistribution/TcPacketCheck.h    | 6 ++++++
 tmtcpacket/pus/tc/TcPacketPus.cpp | 9 +++++++++
 3 files changed, 16 insertions(+)

diff --git a/tcdistribution/TcPacketCheck.cpp b/tcdistribution/TcPacketCheck.cpp
index dce0a458..b3a025a4 100644
--- a/tcdistribution/TcPacketCheck.cpp
+++ b/tcdistribution/TcPacketCheck.cpp
@@ -32,6 +32,7 @@ ReturnValue_t TcPacketCheck::checkPacket(TcPacketStoredBase* currentPacket) {
     if (not currentPacket->isSizeCorrect()) {
         return INCOMPLETE_PACKET;
     }
+
     condition = (tcPacketBase->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) ||
             (tcPacketBase->getPusVersionNumber() != PUS_VERSION_NUMBER);
     if (condition) {
diff --git a/tcdistribution/TcPacketCheck.h b/tcdistribution/TcPacketCheck.h
index f7422c19..7106b7e4 100644
--- a/tcdistribution/TcPacketCheck.h
+++ b/tcdistribution/TcPacketCheck.h
@@ -1,6 +1,7 @@
 #ifndef FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_
 #define FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_
 
+#include "../FSFW.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
@@ -24,7 +25,12 @@ protected:
     /**
      * Describes the TC Packet PUS Version Number a packet must have to pass.
      */
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    static constexpr uint8_t PUS_VERSION_NUMBER = 2;
+#else
     static constexpr uint8_t PUS_VERSION_NUMBER = 1;
+#endif
+
     /**
      * The packet id each correct packet should have.
      * It is composed of the APID and some static fields.
diff --git a/tmtcpacket/pus/tc/TcPacketPus.cpp b/tmtcpacket/pus/tc/TcPacketPus.cpp
index 1df61c07..334da4e7 100644
--- a/tmtcpacket/pus/tc/TcPacketPus.cpp
+++ b/tmtcpacket/pus/tc/TcPacketPus.cpp
@@ -62,11 +62,20 @@ void TcPacketPus::setData(const uint8_t* pData) {
 }
 
 uint8_t TcPacketPus::getSecondaryHeaderFlag() const {
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    // Does not exist for PUS C
+    return 0;
+#else
     return (tcData->dataField.versionTypeAck & 0b10000000) >> 7;
+#endif
 }
 
 uint8_t TcPacketPus::getPusVersionNumber() const {
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    return (tcData->dataField.versionTypeAck & 0b11110000) >> 4;
+#else
     return (tcData->dataField.versionTypeAck & 0b01110000) >> 4;
+#endif
 }
 
 uint16_t TcPacketPus::getSourceId() const {

From 99b007f37e13d10fd3d994e6fccd6d06bcbaea8a Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 14:37:38 +0200
Subject: [PATCH 136/157] added deletions

---
 memory/GenericFileSystemMessage.cpp | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
index e4edc7ac..005f4ede 100644
--- a/memory/GenericFileSystemMessage.cpp
+++ b/memory/GenericFileSystemMessage.cpp
@@ -1,5 +1,7 @@
 #include "GenericFileSystemMessage.h"
 
+#include "../objectmanager/ObjectManager.h"
+#include "../storagemanager/StorageManagerIF.h"
 
 void GenericFileSystemMessage::setCreateFileCommand(CommandMessage* message,
         store_address_t storeId) {
@@ -132,5 +134,28 @@ bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
 }
 
 ReturnValue_t GenericFileSystemMessage::clear(CommandMessage* message) {
+	switch(message->getCommand()) {
+	case(CMD_CREATE_FILE):
+	case(CMD_DELETE_FILE):
+	case(CMD_CREATE_DIRECTORY):
+	case(CMD_REPORT_FILE_ATTRIBUTES):
+	case(REPLY_REPORT_FILE_ATTRIBUTES):
+	case(CMD_LOCK_FILE):
+	case(CMD_UNLOCK_FILE):
+	case(CMD_COPY_FILE):
+	case(REPLY_READ_FROM_FILE):
+	case(CMD_READ_FROM_FILE):
+	case(CMD_APPEND_TO_FILE):
+	case(CMD_FINISH_APPEND_TO_FILE):
+	case(REPLY_READ_FINISHED_STOP):
+	case(REPLY_FINISH_APPEND): {
+		store_address_t storeId = GenericFileSystemMessage::getStoreId(message);
+		auto ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+		if(ipcStore == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		return ipcStore->deleteData(storeId);
+	}
+	}
     return HasReturnvaluesIF::RETURN_OK;
 }

From a7068acca76d6e65a7f9f18bb566b2d8a330dcf3 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 14:38:09 +0200
Subject: [PATCH 137/157] updated generic file system message

---
 memory/GenericFileSystemMessage.cpp | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
index e4edc7ac..005f4ede 100644
--- a/memory/GenericFileSystemMessage.cpp
+++ b/memory/GenericFileSystemMessage.cpp
@@ -1,5 +1,7 @@
 #include "GenericFileSystemMessage.h"
 
+#include "../objectmanager/ObjectManager.h"
+#include "../storagemanager/StorageManagerIF.h"
 
 void GenericFileSystemMessage::setCreateFileCommand(CommandMessage* message,
         store_address_t storeId) {
@@ -132,5 +134,28 @@ bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
 }
 
 ReturnValue_t GenericFileSystemMessage::clear(CommandMessage* message) {
+	switch(message->getCommand()) {
+	case(CMD_CREATE_FILE):
+	case(CMD_DELETE_FILE):
+	case(CMD_CREATE_DIRECTORY):
+	case(CMD_REPORT_FILE_ATTRIBUTES):
+	case(REPLY_REPORT_FILE_ATTRIBUTES):
+	case(CMD_LOCK_FILE):
+	case(CMD_UNLOCK_FILE):
+	case(CMD_COPY_FILE):
+	case(REPLY_READ_FROM_FILE):
+	case(CMD_READ_FROM_FILE):
+	case(CMD_APPEND_TO_FILE):
+	case(CMD_FINISH_APPEND_TO_FILE):
+	case(REPLY_READ_FINISHED_STOP):
+	case(REPLY_FINISH_APPEND): {
+		store_address_t storeId = GenericFileSystemMessage::getStoreId(message);
+		auto ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+		if(ipcStore == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		return ipcStore->deleteData(storeId);
+	}
+	}
     return HasReturnvaluesIF::RETURN_OK;
 }

From eead2a8a49733cd7f0c79d273432d7d978506c24 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Mon, 14 Jun 2021 14:40:40 +0200
Subject: [PATCH 138/157] Moved leap second management

---
 osal/FreeRTOS/Clock.cpp | 68 -------------------------------
 osal/host/Clock.cpp     | 68 -------------------------------
 osal/linux/Clock.cpp    | 68 -------------------------------
 osal/rtems/Clock.cpp    | 62 ----------------------------
 timemanager/Clock.h     | 90 ++++++++++++++++++++++++++++++++---------
 5 files changed, 71 insertions(+), 285 deletions(-)

diff --git a/osal/FreeRTOS/Clock.cpp b/osal/FreeRTOS/Clock.cpp
index c15971fe..66207d75 100644
--- a/osal/FreeRTOS/Clock.cpp
+++ b/osal/FreeRTOS/Clock.cpp
@@ -134,71 +134,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
             / 3600.;
     return HasReturnvaluesIF::RETURN_OK;
 }
-
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
-    //SHOULDDO: works not for dates in the past (might have less leap seconds)
-    if (timeMutex == nullptr) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-
-    uint16_t leapSeconds;
-    ReturnValue_t result = getLeapSeconds(&leapSeconds);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        return result;
-    }
-    timeval leapSeconds_timeval = { 0, 0 };
-    leapSeconds_timeval.tv_sec = leapSeconds;
-
-    //initial offset between UTC and TAI
-    timeval UTCtoTAI1972 = { 10, 0 };
-
-    timeval TAItoTT = { 32, 184000 };
-
-    *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-    if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-    ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        return result;
-    }
-
-    leapSeconds = leapSeconds_;
-
-    result = timeMutex->unlockMutex();
-    return result;
-}
-
-ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
-    if (timeMutex == NULL) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-    ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        return result;
-    }
-
-    *leapSeconds_ = leapSeconds;
-
-    result = timeMutex->unlockMutex();
-    return result;
-}
-
-ReturnValue_t Clock::checkOrCreateClockMutex() {
-    if (timeMutex == NULL) {
-        MutexFactory* mutexFactory = MutexFactory::instance();
-        if (mutexFactory == NULL) {
-            return HasReturnvaluesIF::RETURN_FAILED;
-        }
-        timeMutex = mutexFactory->createMutex();
-        if (timeMutex == NULL) {
-            return HasReturnvaluesIF::RETURN_FAILED;
-        }
-    }
-    return HasReturnvaluesIF::RETURN_OK;
-}
diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp
index c097f619..219d3e96 100644
--- a/osal/host/Clock.cpp
+++ b/osal/host/Clock.cpp
@@ -170,71 +170,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
 			/ 3600.;
 	return HasReturnvaluesIF::RETURN_OK;
 }
-
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
-	//SHOULDDO: works not for dates in the past (might have less leap seconds)
-	if (timeMutex == NULL) {
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	uint16_t leapSeconds;
-	ReturnValue_t result = getLeapSeconds(&leapSeconds);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-	timeval leapSeconds_timeval = { 0, 0 };
-	leapSeconds_timeval.tv_sec = leapSeconds;
-
-	//initial offset between UTC and TAI
-	timeval UTCtoTAI1972 = { 10, 0 };
-
-	timeval TAItoTT = { 32, 184000 };
-
-	*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-	if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	ReturnValue_t result = timeMutex->lockMutex();
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-
-	leapSeconds = leapSeconds_;
-
-	result = timeMutex->unlockMutex();
-	return result;
-}
-
-ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
-	if(timeMutex == nullptr){
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	ReturnValue_t result = timeMutex->lockMutex();
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-
-	*leapSeconds_ = leapSeconds;
-
-	result = timeMutex->unlockMutex();
-	return result;
-}
-
-ReturnValue_t Clock::checkOrCreateClockMutex(){
-	if(timeMutex == nullptr){
-		MutexFactory* mutexFactory = MutexFactory::instance();
-		if (mutexFactory == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		timeMutex = mutexFactory->createMutex();
-		if (timeMutex == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-	}
-	return HasReturnvaluesIF::RETURN_OK;
-}
diff --git a/osal/linux/Clock.cpp b/osal/linux/Clock.cpp
index 35cbfae0..960d9c0d 100644
--- a/osal/linux/Clock.cpp
+++ b/osal/linux/Clock.cpp
@@ -153,71 +153,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
 			/ 3600.;
 	return HasReturnvaluesIF::RETURN_OK;
 }
-
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
-	//SHOULDDO: works not for dates in the past (might have less leap seconds)
-	if (timeMutex == NULL) {
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	uint16_t leapSeconds;
-	ReturnValue_t result = getLeapSeconds(&leapSeconds);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-	timeval leapSeconds_timeval = { 0, 0 };
-	leapSeconds_timeval.tv_sec = leapSeconds;
-
-	//initial offset between UTC and TAI
-	timeval UTCtoTAI1972 = { 10, 0 };
-
-	timeval TAItoTT = { 32, 184000 };
-
-	*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-	if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-
-	leapSeconds = leapSeconds_;
-
-	result = timeMutex->unlockMutex();
-	return result;
-}
-
-ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
-	if(timeMutex==NULL){
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
-
-	*leapSeconds_ = leapSeconds;
-
-	result = timeMutex->unlockMutex();
-	return result;
-}
-
-ReturnValue_t Clock::checkOrCreateClockMutex(){
-	if(timeMutex == nullptr){
-		MutexFactory* mutexFactory = MutexFactory::instance();
-		if (mutexFactory == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		timeMutex = mutexFactory->createMutex();
-		if (timeMutex == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-	}
-	return HasReturnvaluesIF::RETURN_OK;
-}
diff --git a/osal/rtems/Clock.cpp b/osal/rtems/Clock.cpp
index b80786f7..ae720c36 100644
--- a/osal/rtems/Clock.cpp
+++ b/osal/rtems/Clock.cpp
@@ -154,65 +154,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
             / 3600.;
     return HasReturnvaluesIF::RETURN_OK;
 }
-
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
-    //SHOULDDO: works not for dates in the past (might have less leap seconds)
-    if (timeMutex == nullptr) {
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-
-    uint16_t leapSeconds;
-    ReturnValue_t result = getLeapSeconds(&leapSeconds);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        return result;
-    }
-    timeval leapSeconds_timeval = { 0, 0 };
-    leapSeconds_timeval.tv_sec = leapSeconds;
-
-    //initial offset between UTC and TAI
-    timeval UTCtoTAI1972 = { 10, 0 };
-
-    timeval TAItoTT = { 32, 184000 };
-
-    *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-    if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-    MutexGuard helper(timeMutex);
-
-
-    leapSeconds = leapSeconds_;
-
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
-    if(timeMutex==nullptr){
-        return HasReturnvaluesIF::RETURN_FAILED;
-    }
-    MutexGuard helper(timeMutex);
-
-    *leapSeconds_ = leapSeconds;
-
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t Clock::checkOrCreateClockMutex(){
-    if(timeMutex==nullptr){
-        MutexFactory* mutexFactory = MutexFactory::instance();
-        if (mutexFactory == nullptr) {
-            return HasReturnvaluesIF::RETURN_FAILED;
-        }
-        timeMutex = mutexFactory->createMutex();
-        if (timeMutex == nullptr) {
-            return HasReturnvaluesIF::RETURN_FAILED;
-        }
-    }
-    return HasReturnvaluesIF::RETURN_OK;
-}
diff --git a/timemanager/Clock.h b/timemanager/Clock.h
index 6400d284..2ac48384 100644
--- a/timemanager/Clock.h
+++ b/timemanager/Clock.h
@@ -41,14 +41,14 @@ public:
 	 * @return	-@c RETURN_OK on success. Otherwise, the OS failure code
 	 * 				is returned.
 	 */
-	static ReturnValue_t setClock(const TimeOfDay_t* time);
+	static ReturnValue_t setClock(const TimeOfDay_t *time);
 	/**
 	 * This system call sets the system time.
 	 * To set the time, it uses a timeval struct.
 	 * @param time The struct with the time settings to set.
 	 * @return	-@c RETURN_OK on success. Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t setClock(const timeval* time);
+	static ReturnValue_t setClock(const timeval *time);
 	/**
 	 * This system call returns the current system clock in timeval format.
 	 * The timval format has the fields @c tv_sec with seconds and @c tv_usec with
@@ -56,7 +56,7 @@ public:
 	 * @param time	A pointer to a timeval struct where the current time is stored.
 	 * @return @c RETURN_OK on success. Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t getClock_timeval(timeval* time);
+	static ReturnValue_t getClock_timeval(timeval *time);
 
 	/**
 	 * Get the time since boot in a timeval struct
@@ -66,7 +66,7 @@ public:
 	 *
 	 * @deprecated, I do not think this should be able to fail, use timeval getUptime()
 	 */
-	static ReturnValue_t getUptime(timeval* uptime);
+	static ReturnValue_t getUptime(timeval *uptime);
 
 	static timeval getUptime();
 
@@ -79,7 +79,7 @@ public:
 	 * @param ms uptime in ms
 	 * @return RETURN_OK on success. Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t getUptime(uint32_t* uptimeMs);
+	static ReturnValue_t getUptime(uint32_t *uptimeMs);
 
 	/**
 	 * Returns the time in microseconds since an OS-defined epoch.
@@ -89,7 +89,7 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t getClock_usecs(uint64_t* time);
+	static ReturnValue_t getClock_usecs(uint64_t *time);
 	/**
 	 * Returns the time in a TimeOfDay_t struct.
 	 * @param time A pointer to a TimeOfDay_t struct.
@@ -97,7 +97,7 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t getDateAndTime(TimeOfDay_t* time);
+	static ReturnValue_t getDateAndTime(TimeOfDay_t *time);
 
 	/**
 	 * Converts a time of day struct to POSIX seconds.
@@ -107,8 +107,8 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t convertTimeOfDayToTimeval(const TimeOfDay_t* from,
-			timeval* to);
+	static ReturnValue_t convertTimeOfDayToTimeval(const TimeOfDay_t *from,
+			timeval *to);
 
 	/**
 	 * Converts a time represented as seconds and subseconds since unix
@@ -118,12 +118,14 @@ public:
 	 * @param[out] JD2000 days since J2000
 	 * @return @c RETURN_OK
 	 */
-	static ReturnValue_t convertTimevalToJD2000(timeval time, double* JD2000);
+	static ReturnValue_t convertTimevalToJD2000(timeval time, double *JD2000);
 
 	/**
 	 * Calculates and adds the offset between UTC and TT
 	 *
 	 * Depends on the leap seconds to be set correctly.
+	 * Therefore, it does not work for historic
+	 * dates as only the current leap seconds are known.
 	 *
 	 * @param utc timeval, corresponding to UTC time
 	 * @param[out] tt timeval, corresponding to Terrestial Time
@@ -131,7 +133,28 @@ public:
 	 *  - @c RETURN_OK on success
 	 *  - @c RETURN_FAILED if leapSeconds are not set
 	 */
-	static ReturnValue_t convertUTCToTT(timeval utc, timeval* tt);
+	static ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
+		if (timeMutex == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+
+		uint16_t leapSeconds;
+		ReturnValue_t result = getLeapSeconds(&leapSeconds);
+		if (result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+		timeval leapSeconds_timeval = { 0, 0 };
+		leapSeconds_timeval.tv_sec = leapSeconds;
+
+		//initial offset between UTC and TAI
+		timeval UTCtoTAI1972 = { 10, 0 };
+
+		timeval TAItoTT = { 32, 184000 };
+
+		*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
+
+		return HasReturnvaluesIF::RETURN_OK;
+	}
 
 	/**
 	 * Set the Leap Seconds since 1972
@@ -139,34 +162,63 @@ public:
 	 * @param leapSeconds_
 	 * @return
 	 *  - @c RETURN_OK on success.
-	 *  - Otherwise, the OS failure code is returned.
 	 */
-	static ReturnValue_t setLeapSeconds(const uint16_t leapSeconds_);
+	static ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
+		if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		MutexGuard helper(timeMutex);
+
+		leapSeconds = leapSeconds_;
+
+		return HasReturnvaluesIF::RETURN_OK;
+	}
 
 	/**
 	 * Get the Leap Seconds since 1972
 	 *
-	 * Must be set before!
+	 * Setter must be called before
 	 *
 	 * @param[out] leapSeconds_
 	 * @return
 	 *  - @c RETURN_OK on success.
-	 *  - Otherwise, the OS failure code is returned.
+	 *  - @c RETURN_FAILED on error
 	 */
 	static ReturnValue_t getLeapSeconds(uint16_t *leapSeconds_);
+	ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
+		if (timeMutex == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		MutexGuard helper(timeMutex);
 
+		*leapSeconds_ = leapSeconds;
+
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+
+private:
 	/**
 	 * Function to check and create the Mutex for the clock
 	 * @return
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise @c RETURN_FAILED if not able to create one
 	 */
-	static ReturnValue_t checkOrCreateClockMutex();
+	static ReturnValue_t Clock::checkOrCreateClockMutex() {
+		if (timeMutex == nullptr) {
+			MutexFactory *mutexFactory = MutexFactory::instance();
+			if (mutexFactory == nullptr) {
+				return HasReturnvaluesIF::RETURN_FAILED;
+			}
+			timeMutex = mutexFactory->createMutex();
+			if (timeMutex == nullptr) {
+				return HasReturnvaluesIF::RETURN_FAILED;
+			}
+		}
+		return HasReturnvaluesIF::RETURN_OK;
+	}
 
-private:
-	static MutexIF* timeMutex;
+	static MutexIF *timeMutex;
 	static uint16_t leapSeconds;
 };
 
-
 #endif /* FSFW_TIMEMANAGER_CLOCK_H_ */

From af99303eac5f6f98383c01538a9c9d18a1413537 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 15:05:40 +0200
Subject: [PATCH 139/157] indentation

---
 memory/GenericFileSystemMessage.cpp | 46 ++++++++++++++---------------
 1 file changed, 23 insertions(+), 23 deletions(-)

diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
index 005f4ede..b0e1a9ec 100644
--- a/memory/GenericFileSystemMessage.cpp
+++ b/memory/GenericFileSystemMessage.cpp
@@ -134,28 +134,28 @@ bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
 }
 
 ReturnValue_t GenericFileSystemMessage::clear(CommandMessage* message) {
-	switch(message->getCommand()) {
-	case(CMD_CREATE_FILE):
-	case(CMD_DELETE_FILE):
-	case(CMD_CREATE_DIRECTORY):
-	case(CMD_REPORT_FILE_ATTRIBUTES):
-	case(REPLY_REPORT_FILE_ATTRIBUTES):
-	case(CMD_LOCK_FILE):
-	case(CMD_UNLOCK_FILE):
-	case(CMD_COPY_FILE):
-	case(REPLY_READ_FROM_FILE):
-	case(CMD_READ_FROM_FILE):
-	case(CMD_APPEND_TO_FILE):
-	case(CMD_FINISH_APPEND_TO_FILE):
-	case(REPLY_READ_FINISHED_STOP):
-	case(REPLY_FINISH_APPEND): {
-		store_address_t storeId = GenericFileSystemMessage::getStoreId(message);
-		auto ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
-		if(ipcStore == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		return ipcStore->deleteData(storeId);
-	}
-	}
+    switch(message->getCommand()) {
+    case(CMD_CREATE_FILE):
+    case(CMD_DELETE_FILE):
+    case(CMD_CREATE_DIRECTORY):
+    case(CMD_REPORT_FILE_ATTRIBUTES):
+    case(REPLY_REPORT_FILE_ATTRIBUTES):
+    case(CMD_LOCK_FILE):
+    case(CMD_UNLOCK_FILE):
+    case(CMD_COPY_FILE):
+    case(REPLY_READ_FROM_FILE):
+    case(CMD_READ_FROM_FILE):
+    case(CMD_APPEND_TO_FILE):
+    case(CMD_FINISH_APPEND_TO_FILE):
+    case(REPLY_READ_FINISHED_STOP):
+    case(REPLY_FINISH_APPEND): {
+        store_address_t storeId = GenericFileSystemMessage::getStoreId(message);
+        auto ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+        if(ipcStore == nullptr) {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        return ipcStore->deleteData(storeId);
+    }
+    }
     return HasReturnvaluesIF::RETURN_OK;
 }

From 5f27fe63922a1a9458714de75f8cb7bec93abd46 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 15:06:26 +0200
Subject: [PATCH 140/157] indentation

---
 memory/GenericFileSystemMessage.cpp | 46 ++++++++++++++---------------
 1 file changed, 23 insertions(+), 23 deletions(-)

diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
index 005f4ede..b0e1a9ec 100644
--- a/memory/GenericFileSystemMessage.cpp
+++ b/memory/GenericFileSystemMessage.cpp
@@ -134,28 +134,28 @@ bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
 }
 
 ReturnValue_t GenericFileSystemMessage::clear(CommandMessage* message) {
-	switch(message->getCommand()) {
-	case(CMD_CREATE_FILE):
-	case(CMD_DELETE_FILE):
-	case(CMD_CREATE_DIRECTORY):
-	case(CMD_REPORT_FILE_ATTRIBUTES):
-	case(REPLY_REPORT_FILE_ATTRIBUTES):
-	case(CMD_LOCK_FILE):
-	case(CMD_UNLOCK_FILE):
-	case(CMD_COPY_FILE):
-	case(REPLY_READ_FROM_FILE):
-	case(CMD_READ_FROM_FILE):
-	case(CMD_APPEND_TO_FILE):
-	case(CMD_FINISH_APPEND_TO_FILE):
-	case(REPLY_READ_FINISHED_STOP):
-	case(REPLY_FINISH_APPEND): {
-		store_address_t storeId = GenericFileSystemMessage::getStoreId(message);
-		auto ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
-		if(ipcStore == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		return ipcStore->deleteData(storeId);
-	}
-	}
+    switch(message->getCommand()) {
+    case(CMD_CREATE_FILE):
+    case(CMD_DELETE_FILE):
+    case(CMD_CREATE_DIRECTORY):
+    case(CMD_REPORT_FILE_ATTRIBUTES):
+    case(REPLY_REPORT_FILE_ATTRIBUTES):
+    case(CMD_LOCK_FILE):
+    case(CMD_UNLOCK_FILE):
+    case(CMD_COPY_FILE):
+    case(REPLY_READ_FROM_FILE):
+    case(CMD_READ_FROM_FILE):
+    case(CMD_APPEND_TO_FILE):
+    case(CMD_FINISH_APPEND_TO_FILE):
+    case(REPLY_READ_FINISHED_STOP):
+    case(REPLY_FINISH_APPEND): {
+        store_address_t storeId = GenericFileSystemMessage::getStoreId(message);
+        auto ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+        if(ipcStore == nullptr) {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        return ipcStore->deleteData(storeId);
+    }
+    }
     return HasReturnvaluesIF::RETURN_OK;
 }

From aa33ff2f486f74797ed20e6b631907af7d250c2c Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 15:14:57 +0200
Subject: [PATCH 141/157] max tm packet size now configurable

---
 defaultcfg/fsfwconfig/FSFWConfig.h           |   2 +
 tmtcpacket/packetmatcher/ApidMatcher.h       |  54 ++--
 tmtcpacket/packetmatcher/PacketMatchTree.cpp | 319 +++++++++----------
 tmtcpacket/packetmatcher/PacketMatchTree.h   |  46 +--
 tmtcpacket/packetmatcher/ServiceMatcher.h    |  50 +--
 tmtcpacket/packetmatcher/SubserviceMatcher.h |  54 ++--
 tmtcpacket/pus/tc/TcPacketBase.h             | 206 ++++++------
 tmtcpacket/pus/tc/TcPacketStoredBase.cpp     |  34 +-
 tmtcpacket/pus/tc/TcPacketStoredBase.h       |  46 +--
 tmtcpacket/pus/tm/TmPacketBase.cpp           |   4 +-
 tmtcpacket/pus/tm/TmPacketPusA.h             |   5 +-
 tmtcpacket/pus/tm/TmPacketPusC.h             |   3 +-
 12 files changed, 409 insertions(+), 414 deletions(-)

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index 5518e33c..adf9912f 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -68,6 +68,8 @@ static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
 
 static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
 
+static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048;
+
 }
 
 #endif /* CONFIG_FSFWCONFIG_H_ */
diff --git a/tmtcpacket/packetmatcher/ApidMatcher.h b/tmtcpacket/packetmatcher/ApidMatcher.h
index 64f73dbf..33db7151 100644
--- a/tmtcpacket/packetmatcher/ApidMatcher.h
+++ b/tmtcpacket/packetmatcher/ApidMatcher.h
@@ -1,5 +1,5 @@
-#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
-#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
@@ -7,32 +7,32 @@
 
 class ApidMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 private:
-	uint16_t apid;
+    uint16_t apid;
 public:
-	ApidMatcher(uint16_t setApid) :
-			apid(setApid) {
-	}
-	ApidMatcher(TmPacketMinimal* test) :
-			apid(test->getAPID()) {
-	}
-	bool match(TmPacketMinimal* packet) {
-		if (packet->getAPID() == apid) {
-			return true;
-		} else {
-			return false;
-		}
-	}
-	ReturnValue_t serialize(uint8_t** buffer, size_t* size,
-			size_t maxSize, Endianness streamEndianness) const {
-		return SerializeAdapter::serialize(&apid, buffer, size, maxSize, streamEndianness);
-	}
-	size_t getSerializedSize() const {
-		return SerializeAdapter::getSerializedSize(&apid);
-	}
-	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
-			Endianness streamEndianness) {
-		return SerializeAdapter::deSerialize(&apid, buffer, size, streamEndianness);
-	}
+    ApidMatcher(uint16_t setApid) :
+        apid(setApid) {
+    }
+    ApidMatcher(TmPacketMinimal* test) :
+        apid(test->getAPID()) {
+    }
+    bool match(TmPacketMinimal* packet) {
+        if (packet->getAPID() == apid) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+    ReturnValue_t serialize(uint8_t** buffer, size_t* size,
+            size_t maxSize, Endianness streamEndianness) const {
+        return SerializeAdapter::serialize(&apid, buffer, size, maxSize, streamEndianness);
+    }
+    size_t getSerializedSize() const {
+        return SerializeAdapter::getSerializedSize(&apid);
+    }
+    ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+            Endianness streamEndianness) {
+        return SerializeAdapter::deSerialize(&apid, buffer, size, streamEndianness);
+    }
 };
 
 
diff --git a/tmtcpacket/packetmatcher/PacketMatchTree.cpp b/tmtcpacket/packetmatcher/PacketMatchTree.cpp
index ac72b3e7..05e176ef 100644
--- a/tmtcpacket/packetmatcher/PacketMatchTree.cpp
+++ b/tmtcpacket/packetmatcher/PacketMatchTree.cpp
@@ -5,197 +5,194 @@
 
 // This should be configurable..
 const LocalPool::LocalPoolConfig PacketMatchTree::poolConfig = {
-		{10, sizeof(ServiceMatcher)},
-		{20, sizeof(SubServiceMatcher)},
-		{2, sizeof(ApidMatcher)},
-		{40, sizeof(PacketMatchTree::Node)}
+        {10, sizeof(ServiceMatcher)},
+        {20, sizeof(SubServiceMatcher)},
+        {2, sizeof(ApidMatcher)},
+        {40, sizeof(PacketMatchTree::Node)}
 };
 
-PacketMatchTree::PacketMatchTree(Node* root) :
-		MatchTree<TmPacketMinimal*>(root, 2),
-		factoryBackend(0, poolConfig, false, true),
-		factory(&factoryBackend) {
+PacketMatchTree::PacketMatchTree(Node* root): MatchTree<TmPacketMinimal*>(root, 2),
+        factoryBackend(0, poolConfig, false, true),
+        factory(&factoryBackend) {
 }
 
-PacketMatchTree::PacketMatchTree(iterator root) :
-		MatchTree<TmPacketMinimal*>(root.element, 2),
-		factoryBackend(0, poolConfig, false, true),
-		factory(&factoryBackend) {
+PacketMatchTree::PacketMatchTree(iterator root): MatchTree<TmPacketMinimal*>(root.element, 2),
+        factoryBackend(0, poolConfig, false, true),
+        factory(&factoryBackend) {
 }
 
-PacketMatchTree::PacketMatchTree() :
-		MatchTree<TmPacketMinimal*>((Node*) NULL, 2),
-		factoryBackend(0, poolConfig, false, true),
-		factory(&factoryBackend) {
+PacketMatchTree::PacketMatchTree(): MatchTree<TmPacketMinimal*>((Node*) NULL, 2),
+        factoryBackend(0, poolConfig, false, true),
+        factory(&factoryBackend) {
 }
 
 PacketMatchTree::~PacketMatchTree() {
 }
 
 ReturnValue_t PacketMatchTree::addMatch(uint16_t apid, uint8_t type,
-		uint8_t subtype) {
-	//We assume adding APID is always requested.
-	TmPacketMinimal::TmPacketMinimalPointer data;
-	data.data_field.service_type = type;
-	data.data_field.service_subtype = subtype;
-	TmPacketMinimal testPacket((uint8_t*) &data);
-	testPacket.setAPID(apid);
-	iterator lastTest;
-	iterator rollback;
-	ReturnValue_t result = findOrInsertMatch<TmPacketMinimal*, ApidMatcher>(
-			this->begin(), &testPacket, &lastTest);
-	if (result == NEW_NODE_CREATED) {
-		rollback = lastTest;
-	} else if (result != RETURN_OK) {
-		return result;
-	}
-	if (type == 0) {
-		//Check if lastTest has no children, otherwise, delete them,
-		//as a more general check is requested.
-		if (lastTest.left() != this->end()) {
-			removeElementAndAllChildren(lastTest.left());
-		}
-		return RETURN_OK;
-	}
-	//Type insertion required.
-	result = findOrInsertMatch<TmPacketMinimal*, ServiceMatcher>(
-			lastTest.left(), &testPacket, &lastTest);
-	if (result == NEW_NODE_CREATED) {
-		if  (rollback == this->end()) {
-			rollback = lastTest;
-		}
-	} else if (result != RETURN_OK) {
-		if (rollback != this->end()) {
-			removeElementAndAllChildren(rollback);
-		}
-		return result;
-	}
-	if (subtype == 0) {
-		if (lastTest.left() != this->end()) {
-			//See above
-			removeElementAndAllChildren(lastTest.left());
-		}
-		return RETURN_OK;
-	}
-	//Subtype insertion required.
-	result = findOrInsertMatch<TmPacketMinimal*, SubServiceMatcher>(
-			lastTest.left(), &testPacket, &lastTest);
-	if (result == NEW_NODE_CREATED) {
-		return RETURN_OK;
-	} else if (result != RETURN_OK) {
-		if (rollback != this->end()) {
-			removeElementAndAllChildren(rollback);
-		}
-		return result;
-	}
-	return RETURN_OK;
+        uint8_t subtype) {
+    //We assume adding APID is always requested.
+    TmPacketMinimal::TmPacketMinimalPointer data;
+    data.data_field.service_type = type;
+    data.data_field.service_subtype = subtype;
+    TmPacketMinimal testPacket((uint8_t*) &data);
+    testPacket.setAPID(apid);
+    iterator lastTest;
+    iterator rollback;
+    ReturnValue_t result = findOrInsertMatch<TmPacketMinimal*, ApidMatcher>(
+            this->begin(), &testPacket, &lastTest);
+    if (result == NEW_NODE_CREATED) {
+        rollback = lastTest;
+    } else if (result != RETURN_OK) {
+        return result;
+    }
+    if (type == 0) {
+        //Check if lastTest has no children, otherwise, delete them,
+        //as a more general check is requested.
+        if (lastTest.left() != this->end()) {
+            removeElementAndAllChildren(lastTest.left());
+        }
+        return RETURN_OK;
+    }
+    //Type insertion required.
+    result = findOrInsertMatch<TmPacketMinimal*, ServiceMatcher>(
+            lastTest.left(), &testPacket, &lastTest);
+    if (result == NEW_NODE_CREATED) {
+        if  (rollback == this->end()) {
+            rollback = lastTest;
+        }
+    } else if (result != RETURN_OK) {
+        if (rollback != this->end()) {
+            removeElementAndAllChildren(rollback);
+        }
+        return result;
+    }
+    if (subtype == 0) {
+        if (lastTest.left() != this->end()) {
+            //See above
+            removeElementAndAllChildren(lastTest.left());
+        }
+        return RETURN_OK;
+    }
+    //Subtype insertion required.
+    result = findOrInsertMatch<TmPacketMinimal*, SubServiceMatcher>(
+            lastTest.left(), &testPacket, &lastTest);
+    if (result == NEW_NODE_CREATED) {
+        return RETURN_OK;
+    } else if (result != RETURN_OK) {
+        if (rollback != this->end()) {
+            removeElementAndAllChildren(rollback);
+        }
+        return result;
+    }
+    return RETURN_OK;
 }
 
 template<typename VALUE_T, typename INSERTION_T>
 ReturnValue_t PacketMatchTree::findOrInsertMatch(iterator startAt, VALUE_T test,
-		iterator* lastTest) {
-	bool attachToBranch = AND;
-	iterator iter = startAt;
-	while (iter != this->end()) {
-		bool isMatch = iter->match(test);
-		attachToBranch = OR;
-		*lastTest = iter;
-		if (isMatch) {
-			return RETURN_OK;
-		} else {
-			//Go down OR branch.
-			iter = iter.right();
-		}
-	}
-	//Only reached if nothing was found.
-	SerializeableMatcherIF<VALUE_T>* newContent = factory.generate<INSERTION_T>(
-			test);
-	if (newContent == NULL) {
-		return FULL;
-	}
-	Node* newNode = factory.generate<Node>(newContent);
-	if (newNode == NULL) {
-		//Need to make sure partially generated content is deleted, otherwise, that's a leak.
-		factory.destroy<INSERTION_T>(static_cast<INSERTION_T*>(newContent));
-		return FULL;
-	}
-	*lastTest = insert(attachToBranch, *lastTest, newNode);
-	if (*lastTest == end()) {
-		//This actaully never fails, so creating a dedicated returncode seems an overshoot.
-		return RETURN_FAILED;
-	}
-	return NEW_NODE_CREATED;
+        iterator* lastTest) {
+    bool attachToBranch = AND;
+    iterator iter = startAt;
+    while (iter != this->end()) {
+        bool isMatch = iter->match(test);
+        attachToBranch = OR;
+        *lastTest = iter;
+        if (isMatch) {
+            return RETURN_OK;
+        } else {
+            //Go down OR branch.
+            iter = iter.right();
+        }
+    }
+    //Only reached if nothing was found.
+    SerializeableMatcherIF<VALUE_T>* newContent = factory.generate<INSERTION_T>(
+            test);
+    if (newContent == NULL) {
+        return FULL;
+    }
+    Node* newNode = factory.generate<Node>(newContent);
+    if (newNode == NULL) {
+        //Need to make sure partially generated content is deleted, otherwise, that's a leak.
+        factory.destroy<INSERTION_T>(static_cast<INSERTION_T*>(newContent));
+        return FULL;
+    }
+    *lastTest = insert(attachToBranch, *lastTest, newNode);
+    if (*lastTest == end()) {
+        //This actaully never fails, so creating a dedicated returncode seems an overshoot.
+        return RETURN_FAILED;
+    }
+    return NEW_NODE_CREATED;
 }
 
 ReturnValue_t PacketMatchTree::removeMatch(uint16_t apid, uint8_t type,
-		uint8_t subtype) {
-	TmPacketMinimal::TmPacketMinimalPointer data;
-	data.data_field.service_type = type;
-	data.data_field.service_subtype = subtype;
-	TmPacketMinimal testPacket((uint8_t*) &data);
-	testPacket.setAPID(apid);
-	iterator foundElement = findMatch(begin(), &testPacket);
-	if (foundElement == this->end()) {
-		return NO_MATCH;
-	}
-	if (type == 0) {
-		if (foundElement.left() == end()) {
-			return removeElementAndReconnectChildren(foundElement);
-		} else {
-			return TOO_GENERAL_REQUEST;
-		}
-	}
-	//Go down AND branch. Will abort if empty.
-	foundElement = findMatch(foundElement.left(), &testPacket);
-	if (foundElement == this->end()) {
-		return NO_MATCH;
-	}
-	if (subtype == 0) {
-		if (foundElement.left() == end()) {
-			return removeElementAndReconnectChildren(foundElement);
-		} else {
-			return TOO_GENERAL_REQUEST;
-		}
-	}
-	//Again, go down AND branch.
-	foundElement = findMatch(foundElement.left(), &testPacket);
-	if (foundElement == end()) {
-		return NO_MATCH;
-	}
-	return removeElementAndReconnectChildren(foundElement);
+        uint8_t subtype) {
+    TmPacketMinimal::TmPacketMinimalPointer data;
+    data.data_field.service_type = type;
+    data.data_field.service_subtype = subtype;
+    TmPacketMinimal testPacket((uint8_t*) &data);
+    testPacket.setAPID(apid);
+    iterator foundElement = findMatch(begin(), &testPacket);
+    if (foundElement == this->end()) {
+        return NO_MATCH;
+    }
+    if (type == 0) {
+        if (foundElement.left() == end()) {
+            return removeElementAndReconnectChildren(foundElement);
+        } else {
+            return TOO_GENERAL_REQUEST;
+        }
+    }
+    //Go down AND branch. Will abort if empty.
+    foundElement = findMatch(foundElement.left(), &testPacket);
+    if (foundElement == this->end()) {
+        return NO_MATCH;
+    }
+    if (subtype == 0) {
+        if (foundElement.left() == end()) {
+            return removeElementAndReconnectChildren(foundElement);
+        } else {
+            return TOO_GENERAL_REQUEST;
+        }
+    }
+    //Again, go down AND branch.
+    foundElement = findMatch(foundElement.left(), &testPacket);
+    if (foundElement == end()) {
+        return NO_MATCH;
+    }
+    return removeElementAndReconnectChildren(foundElement);
 }
 
 PacketMatchTree::iterator PacketMatchTree::findMatch(iterator startAt,
-		TmPacketMinimal* test) {
-	iterator iter = startAt;
-	while (iter != end()) {
-		bool isMatch = iter->match(test);
-		if (isMatch) {
-			break;
-		} else {
-			iter = iter.right(); //next OR element
-		}
-	}
-	return iter;
+        TmPacketMinimal* test) {
+    iterator iter = startAt;
+    while (iter != end()) {
+        bool isMatch = iter->match(test);
+        if (isMatch) {
+            break;
+        } else {
+            iter = iter.right(); //next OR element
+        }
+    }
+    return iter;
 }
 
 ReturnValue_t PacketMatchTree::initialize() {
-	return factoryBackend.initialize();
+    return factoryBackend.initialize();
 }
 
 
 ReturnValue_t PacketMatchTree::changeMatch(bool addToMatch, uint16_t apid,
-		uint8_t type, uint8_t subtype) {
-	if (addToMatch) {
-		return addMatch(apid, type, subtype);
-	} else {
-		return removeMatch(apid, type, subtype);
-	}
+        uint8_t type, uint8_t subtype) {
+    if (addToMatch) {
+        return addMatch(apid, type, subtype);
+    } else {
+        return removeMatch(apid, type, subtype);
+    }
 }
 
 ReturnValue_t PacketMatchTree::cleanUpElement(iterator position) {
-	factory.destroy(position.element->value);
-	//Go on anyway, there's nothing we can do.
-	//SHOULDDO: Throw event, or write debug message?
-	return factory.destroy(position.element);
+    factory.destroy(position.element->value);
+    //Go on anyway, there's nothing we can do.
+    //SHOULDDO: Throw event, or write debug message?
+    return factory.destroy(position.element);
 }
diff --git a/tmtcpacket/packetmatcher/PacketMatchTree.h b/tmtcpacket/packetmatcher/PacketMatchTree.h
index 735f0566..a40b5099 100644
--- a/tmtcpacket/packetmatcher/PacketMatchTree.h
+++ b/tmtcpacket/packetmatcher/PacketMatchTree.h
@@ -1,5 +1,5 @@
-#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
-#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
 
 #include "../../container/PlacementFactory.h"
 #include "../../globalfunctions/matching/MatchTree.h"
@@ -8,29 +8,29 @@
 
 class PacketMatchTree: public MatchTree<TmPacketMinimal*>, public HasReturnvaluesIF {
 public:
-	PacketMatchTree(Node* root);
-	PacketMatchTree(iterator root);
-	PacketMatchTree();
-	virtual ~PacketMatchTree();
-	ReturnValue_t changeMatch(bool addToMatch, uint16_t apid, uint8_t type = 0,
-			uint8_t subtype = 0);
-	ReturnValue_t addMatch(uint16_t apid, uint8_t type = 0,
-			uint8_t subtype = 0);
-	ReturnValue_t removeMatch(uint16_t apid, uint8_t type = 0,
-			uint8_t subtype = 0);
-	ReturnValue_t initialize();
+    PacketMatchTree(Node* root);
+    PacketMatchTree(iterator root);
+    PacketMatchTree();
+    virtual ~PacketMatchTree();
+    ReturnValue_t changeMatch(bool addToMatch, uint16_t apid, uint8_t type = 0,
+            uint8_t subtype = 0);
+    ReturnValue_t addMatch(uint16_t apid, uint8_t type = 0,
+            uint8_t subtype = 0);
+    ReturnValue_t removeMatch(uint16_t apid, uint8_t type = 0,
+            uint8_t subtype = 0);
+    ReturnValue_t initialize();
 protected:
-	ReturnValue_t cleanUpElement(iterator position);
+    ReturnValue_t cleanUpElement(iterator position);
 private:
-	static const uint8_t N_POOLS = 4;
-	LocalPool factoryBackend;
-	PlacementFactory factory;
-	static const LocalPool::LocalPoolConfig poolConfig;
-	static const uint16_t POOL_SIZES[N_POOLS];
-	static const uint16_t N_ELEMENTS[N_POOLS];
-	template<typename VALUE_T, typename INSERTION_T>
-	ReturnValue_t findOrInsertMatch(iterator startAt, VALUE_T test, iterator* lastTest);
-	iterator findMatch(iterator startAt, TmPacketMinimal* test);
+    static const uint8_t N_POOLS = 4;
+    LocalPool factoryBackend;
+    PlacementFactory factory;
+    static const LocalPool::LocalPoolConfig poolConfig;
+    static const uint16_t POOL_SIZES[N_POOLS];
+    static const uint16_t N_ELEMENTS[N_POOLS];
+    template<typename VALUE_T, typename INSERTION_T>
+    ReturnValue_t findOrInsertMatch(iterator startAt, VALUE_T test, iterator* lastTest);
+    iterator findMatch(iterator startAt, TmPacketMinimal* test);
 };
 
 #endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_ */
diff --git a/tmtcpacket/packetmatcher/ServiceMatcher.h b/tmtcpacket/packetmatcher/ServiceMatcher.h
index 57f491f3..67d09d2b 100644
--- a/tmtcpacket/packetmatcher/ServiceMatcher.h
+++ b/tmtcpacket/packetmatcher/ServiceMatcher.h
@@ -7,32 +7,32 @@
 
 class ServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 private:
-	uint8_t service;
+    uint8_t service;
 public:
-	ServiceMatcher(uint8_t setService) :
-			service(setService) {
-	}
-	ServiceMatcher(TmPacketMinimal* test) :
-			service(test->getService()) {
-	}
-	bool match(TmPacketMinimal* packet) {
-		if (packet->getService() == service) {
-			return true;
-		} else {
-			return false;
-		}
-	}
-	ReturnValue_t serialize(uint8_t** buffer, size_t* size,
-			size_t maxSize, Endianness streamEndianness) const {
-		return SerializeAdapter::serialize(&service, buffer, size, maxSize, streamEndianness);
-	}
-	size_t getSerializedSize() const {
-		return SerializeAdapter::getSerializedSize(&service);
-	}
-	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
-			Endianness streamEndianness) {
-		return SerializeAdapter::deSerialize(&service, buffer, size, streamEndianness);
-	}
+    ServiceMatcher(uint8_t setService) :
+        service(setService) {
+    }
+    ServiceMatcher(TmPacketMinimal* test) :
+        service(test->getService()) {
+    }
+    bool match(TmPacketMinimal* packet) {
+        if (packet->getService() == service) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+    ReturnValue_t serialize(uint8_t** buffer, size_t* size,
+            size_t maxSize, Endianness streamEndianness) const {
+        return SerializeAdapter::serialize(&service, buffer, size, maxSize, streamEndianness);
+    }
+    size_t getSerializedSize() const {
+        return SerializeAdapter::getSerializedSize(&service);
+    }
+    ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+            Endianness streamEndianness) {
+        return SerializeAdapter::deSerialize(&service, buffer, size, streamEndianness);
+    }
 };
 
 
diff --git a/tmtcpacket/packetmatcher/SubserviceMatcher.h b/tmtcpacket/packetmatcher/SubserviceMatcher.h
index 80681b5d..e570b452 100644
--- a/tmtcpacket/packetmatcher/SubserviceMatcher.h
+++ b/tmtcpacket/packetmatcher/SubserviceMatcher.h
@@ -1,5 +1,5 @@
-#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
-#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
@@ -7,32 +7,32 @@
 
 class SubServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 public:
-	SubServiceMatcher(uint8_t subService) :
-			subService(subService) {
-	}
-	SubServiceMatcher(TmPacketMinimal* test) :
-			subService(test->getSubService()) {
-	}
-	bool match(TmPacketMinimal* packet) {
-		if (packet->getSubService() == subService) {
-			return true;
-		} else {
-			return false;
-		}
-	}
-	ReturnValue_t serialize(uint8_t** buffer, size_t* size,
-			size_t maxSize, Endianness streamEndianness) const {
-		return SerializeAdapter::serialize(&subService, buffer, size, maxSize, streamEndianness);
-	}
-	size_t getSerializedSize() const {
-		return SerializeAdapter::getSerializedSize(&subService);
-	}
-	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
-			Endianness streamEndianness) {
-		return SerializeAdapter::deSerialize(&subService, buffer, size, streamEndianness);
-	}
+    SubServiceMatcher(uint8_t subService) :
+        subService(subService) {
+    }
+    SubServiceMatcher(TmPacketMinimal* test) :
+        subService(test->getSubService()) {
+    }
+    bool match(TmPacketMinimal* packet) {
+        if (packet->getSubService() == subService) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+    ReturnValue_t serialize(uint8_t** buffer, size_t* size,
+            size_t maxSize, Endianness streamEndianness) const {
+        return SerializeAdapter::serialize(&subService, buffer, size, maxSize, streamEndianness);
+    }
+    size_t getSerializedSize() const {
+        return SerializeAdapter::getSerializedSize(&subService);
+    }
+    ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+            Endianness streamEndianness) {
+        return SerializeAdapter::deSerialize(&subService, buffer, size, streamEndianness);
+    }
 private:
-	uint8_t subService;
+    uint8_t subService;
 };
 
 
diff --git a/tmtcpacket/pus/tc/TcPacketBase.h b/tmtcpacket/pus/tc/TcPacketBase.h
index 68f08896..e2872246 100644
--- a/tmtcpacket/pus/tc/TcPacketBase.h
+++ b/tmtcpacket/pus/tc/TcPacketBase.h
@@ -19,114 +19,114 @@ class TcPacketBase : public SpacePacketBase {
     friend class TcPacketStoredBase;
 public:
 
-	enum AckField {
-		//! No acknowledgements are expected.
-		ACK_NONE = 0b0000,
-		//! Acknowledgements on acceptance are expected.
-		ACK_ACCEPTANCE = 0b0001,
-		//! Acknowledgements on start are expected.
-		ACK_START = 0b0010,
-		//! Acknowledgements on step are expected.
-		ACK_STEP = 0b0100,
-		//! Acknowledfgement on completion are expected.
-		ACK_COMPLETION = 0b1000
-	};
+    enum AckField {
+        //! No acknowledgements are expected.
+        ACK_NONE = 0b0000,
+        //! Acknowledgements on acceptance are expected.
+        ACK_ACCEPTANCE = 0b0001,
+        //! Acknowledgements on start are expected.
+        ACK_START = 0b0010,
+        //! Acknowledgements on step are expected.
+        ACK_STEP = 0b0100,
+        //! Acknowledfgement on completion are expected.
+        ACK_COMPLETION = 0b1000
+    };
 
-	static constexpr uint8_t ACK_ALL = ACK_ACCEPTANCE | ACK_START | ACK_STEP |
-			ACK_COMPLETION;
+    static constexpr uint8_t ACK_ALL = ACK_ACCEPTANCE | ACK_START | ACK_STEP |
+            ACK_COMPLETION;
 
-	/**
-	 * 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 setData	The position where the packet data lies.
-	 */
-	TcPacketBase( const uint8_t* setData );
-	/**
-	 * This is the empty default destructor.
-	 */
-	virtual ~TcPacketBase();
+    /**
+     * 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 setData	The position where the packet data lies.
+     */
+    TcPacketBase( const uint8_t* setData );
+    /**
+     * This is the empty default destructor.
+     */
+    virtual ~TcPacketBase();
 
-	/**
-	 * This command returns the CCSDS Secondary Header Flag.
-	 * It shall always be zero for PUS Packets. This is the
-	 * highest bit of the first byte of the Data Field Header.
-	 * @return	the CCSDS Secondary Header Flag
-	 */
-	virtual uint8_t getSecondaryHeaderFlag() const = 0;
-	/**
-	 * This command returns the TC Packet PUS Version Number.
-	 * The version number of ECSS PUS 2003 is 1.
-	 * It consists of the second to fourth highest bits of the
-	 * first byte.
-	 * @return
-	 */
-	virtual uint8_t getPusVersionNumber() const = 0;
-	/**
-	 * This is a getter for the packet's Ack field, which are the lowest four
-	 * bits of the first byte of the Data Field Header.
-	 *
-	 * It is packed in a uint8_t variable.
-	 * @return	The packet's PUS Ack field.
-	 */
-	virtual uint8_t getAcknowledgeFlags() const = 0;
-	/**
-	 * 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() const = 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() const = 0;
-	/**
-	 * The source ID can be used to have an additional identifier, e.g. for different ground
-	 * station.
-	 * @return
-	 */
-	virtual uint16_t getSourceId() const = 0;
+    /**
+     * This command returns the CCSDS Secondary Header Flag.
+     * It shall always be zero for PUS Packets. This is the
+     * highest bit of the first byte of the Data Field Header.
+     * @return	the CCSDS Secondary Header Flag
+     */
+    virtual uint8_t getSecondaryHeaderFlag() const = 0;
+    /**
+     * This command returns the TC Packet PUS Version Number.
+     * The version number of ECSS PUS 2003 is 1.
+     * It consists of the second to fourth highest bits of the
+     * first byte.
+     * @return
+     */
+    virtual uint8_t getPusVersionNumber() const = 0;
+    /**
+     * This is a getter for the packet's Ack field, which are the lowest four
+     * bits of the first byte of the Data Field Header.
+     *
+     * It is packed in a uint8_t variable.
+     * @return	The packet's PUS Ack field.
+     */
+    virtual uint8_t getAcknowledgeFlags() const = 0;
+    /**
+     * 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() const = 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() const = 0;
+    /**
+     * The source ID can be used to have an additional identifier, e.g. for different ground
+     * station.
+     * @return
+     */
+    virtual uint16_t getSourceId() const = 0;
 
-	/**
-	 * This is a getter for a pointer to the packet's Application data.
-	 *
-	 * These are the bytes that follow after the Data Field Header. They form
-	 * the packet's application data.
-	 * @return	A pointer to the PUS Application Data.
-	 */
-	virtual const uint8_t* getApplicationData() const = 0;
-	/**
-	 * This method calculates the size of the PUS Application 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 Application Data (without Error Control
-	 * 		field)
-	 */
-	virtual uint16_t getApplicationDataSize() const = 0;
-	/**
-	 * This getter returns the Error Control Field of the packet.
-	 *
-	 * The field is placed after any possible Application Data. If no
-	 * Application Data is present there's still an Error Control field. It is
-	 * supposed to be a 16bit-CRC.
-	 * @return	The PUS Error Control
-	 */
-	virtual uint16_t getErrorControl() const = 0;
-	/**
-	 * With this method, the Error Control Field is updated to match the
-	 * current content of the packet.
-	 */
-	virtual void setErrorControl() = 0;
+    /**
+     * This is a getter for a pointer to the packet's Application data.
+     *
+     * These are the bytes that follow after the Data Field Header. They form
+     * the packet's application data.
+     * @return	A pointer to the PUS Application Data.
+     */
+    virtual const uint8_t* getApplicationData() const = 0;
+    /**
+     * This method calculates the size of the PUS Application 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 Application Data (without Error Control
+     * 		field)
+     */
+    virtual uint16_t getApplicationDataSize() const = 0;
+    /**
+     * This getter returns the Error Control Field of the packet.
+     *
+     * The field is placed after any possible Application Data. If no
+     * Application Data is present there's still an Error Control field. It is
+     * supposed to be a 16bit-CRC.
+     * @return	The PUS Error Control
+     */
+    virtual uint16_t getErrorControl() const = 0;
+    /**
+     * With this method, the Error Control Field is updated to match the
+     * current content of the packet.
+     */
+    virtual void setErrorControl() = 0;
 
-	/**
-	 * Calculate full packet length from application data length.
-	 * @param appDataLen
-	 * @return
-	 */
-	virtual size_t calculateFullPacketLength(size_t appDataLen) const = 0;
+    /**
+     * Calculate full packet length from application data length.
+     * @param appDataLen
+     * @return
+     */
+    virtual size_t calculateFullPacketLength(size_t appDataLen) const = 0;
 
     /**
      * This is a debugging helper method that prints the whole packet content
diff --git a/tmtcpacket/pus/tc/TcPacketStoredBase.cpp b/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
index daae35f5..cf980e68 100644
--- a/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
+++ b/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
@@ -18,32 +18,32 @@ TcPacketStoredBase::~TcPacketStoredBase() {
 }
 
 ReturnValue_t TcPacketStoredBase::getData(const uint8_t ** dataPtr,
-		size_t* dataSize) {
-	auto result = this->store->getData(storeAddress, dataPtr, dataSize);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
+        size_t* dataSize) {
+    auto result = this->store->getData(storeAddress, dataPtr, dataSize);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "TcPacketStoredBase: Could not get data!" << std::endl;
+        sif::warning << "TcPacketStoredBase: Could not get data!" << std::endl;
 #else
-		sif::printWarning("TcPacketStoredBase: Could not get data!\n");
+        sif::printWarning("TcPacketStoredBase: Could not get data!\n");
 #endif
-	}
-	return result;
+    }
+    return result;
 }
 
 
 
 bool TcPacketStoredBase::checkAndSetStore() {
-	if (this->store == nullptr) {
-		this->store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
-		if (this->store == nullptr) {
+    if (this->store == nullptr) {
+        this->store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
+        if (this->store == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TcPacketStoredBase::TcPacketStoredBase: TC Store not found!"
-					<< std::endl;
+            sif::error << "TcPacketStoredBase::TcPacketStoredBase: TC Store not found!"
+                    << std::endl;
 #endif
-			return false;
-		}
-	}
-	return true;
+            return false;
+        }
+    }
+    return true;
 }
 
 void TcPacketStoredBase::setStoreAddress(store_address_t setAddress) {
@@ -68,5 +68,5 @@ void TcPacketStoredBase::setStoreAddress(store_address_t setAddress) {
 }
 
 store_address_t TcPacketStoredBase::getStoreAddress() {
-	return this->storeAddress;
+    return this->storeAddress;
 }
diff --git a/tmtcpacket/pus/tc/TcPacketStoredBase.h b/tmtcpacket/pus/tc/TcPacketStoredBase.h
index b7d73531..1e1681f6 100644
--- a/tmtcpacket/pus/tc/TcPacketStoredBase.h
+++ b/tmtcpacket/pus/tc/TcPacketStoredBase.h
@@ -34,35 +34,35 @@ public:
      */
     TcPacketStoredBase(const uint8_t* data, uint32_t size);
 
-	virtual~ TcPacketStoredBase();
+    virtual~ TcPacketStoredBase();
 
-	/**
-	 * Getter function for the raw data.
-	 * @param dataPtr [out] Pointer to the data pointer to set
-	 * @param dataSize [out] Address of size to set.
-	 * @return -@c RETURN_OK if data was retrieved successfully.
-	 */
-	ReturnValue_t getData(const uint8_t ** dataPtr, size_t* dataSize) override;
+    /**
+     * Getter function for the raw data.
+     * @param dataPtr [out] Pointer to the data pointer to set
+     * @param dataSize [out] Address of size to set.
+     * @return -@c RETURN_OK if data was retrieved successfully.
+     */
+    ReturnValue_t getData(const uint8_t ** dataPtr, size_t* dataSize) override;
 
     void setStoreAddress(store_address_t setAddress) override;
     store_address_t getStoreAddress() override;
 
-	/**
-	 * With this call, the packet is deleted.
-	 * It removes itself from the store and sets its data pointer to NULL.
-	 * @return	returncode from deleting the data.
-	 */
-	virtual ReturnValue_t deletePacket() = 0;
+    /**
+     * With this call, the packet is deleted.
+     * It removes itself from the store and sets its data pointer to NULL.
+     * @return	returncode from deleting the data.
+     */
+    virtual ReturnValue_t deletePacket() = 0;
 
-	/**
-	 * This method performs a size check.
-	 * It reads the stored size and compares it with the size entered in the
-	 * packet header. This class is the optimal place for such a check as it
-	 * has access to both the header data and the store.
-	 * @return	true if size is correct, false if packet is not registered in
-	 * 			store or size is incorrect.
-	 */
-	virtual bool isSizeCorrect() = 0;
+    /**
+     * This method performs a size check.
+     * It reads the stored size and compares it with the size entered in the
+     * packet header. This class is the optimal place for such a check as it
+     * has access to both the header data and the store.
+     * @return	true if size is correct, false if packet is not registered in
+     * 			store or size is incorrect.
+     */
+    virtual bool isSizeCorrect() = 0;
 
 protected:
     /**
diff --git a/tmtcpacket/pus/tm/TmPacketBase.cpp b/tmtcpacket/pus/tm/TmPacketBase.cpp
index 91ec7d58..3dd1749f 100644
--- a/tmtcpacket/pus/tm/TmPacketBase.cpp
+++ b/tmtcpacket/pus/tm/TmPacketBase.cpp
@@ -11,9 +11,7 @@
 TimeStamperIF* TmPacketBase::timeStamper = nullptr;
 object_id_t TmPacketBase::timeStamperId = objects::NO_OBJECT;
 
-TmPacketBase::TmPacketBase(uint8_t* setData):
-        SpacePacketBase(setData) {
-}
+TmPacketBase::TmPacketBase(uint8_t* setData): SpacePacketBase(setData) {}
 
 TmPacketBase::~TmPacketBase() {
     //Nothing to do.
diff --git a/tmtcpacket/pus/tm/TmPacketPusA.h b/tmtcpacket/pus/tm/TmPacketPusA.h
index 486b68f3..49f26b34 100644
--- a/tmtcpacket/pus/tm/TmPacketPusA.h
+++ b/tmtcpacket/pus/tm/TmPacketPusA.h
@@ -23,7 +23,7 @@ struct PUSTmDataFieldHeaderPusA {
     uint8_t service_type;
     uint8_t service_subtype;
     uint8_t subcounter;
-//  uint8_t destination;
+    // uint8_t destination;
     uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
 };
 
@@ -51,8 +51,7 @@ public:
     static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
             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;
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = fsfwconfig::FSFW_MAX_TM_PACKET_SIZE;
 
     /**
      * This is the default constructor.
diff --git a/tmtcpacket/pus/tm/TmPacketPusC.h b/tmtcpacket/pus/tm/TmPacketPusC.h
index 97e5a9a5..2a6d3bf6 100644
--- a/tmtcpacket/pus/tm/TmPacketPusC.h
+++ b/tmtcpacket/pus/tm/TmPacketPusC.h
@@ -53,8 +53,7 @@ public:
     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;
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = fsfwconfig::FSFW_MAX_TM_PACKET_SIZE;
 
     /**
      * This is the default constructor.

From 105a498b93d563126cf25cb373fa337915c09436 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 15:24:26 +0200
Subject: [PATCH 142/157] added way to set source ID

---
 tmtcpacket/pus/tc/TcPacketPus.cpp | 8 +++++++-
 tmtcpacket/pus/tc/TcPacketPus.h   | 2 +-
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/tmtcpacket/pus/tc/TcPacketPus.cpp b/tmtcpacket/pus/tc/TcPacketPus.cpp
index 334da4e7..d2b19206 100644
--- a/tmtcpacket/pus/tc/TcPacketPus.cpp
+++ b/tmtcpacket/pus/tc/TcPacketPus.cpp
@@ -8,7 +8,7 @@ TcPacketPus::TcPacketPus(const uint8_t *setData): TcPacketBase(setData) {
 }
 
 void TcPacketPus::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
-        uint8_t ack, uint8_t service, uint8_t subservice) {
+        uint8_t ack, uint8_t service, uint8_t subservice, uint16_t sourceId) {
     initSpacePacketHeader(true, true, apid, sequenceCount);
     std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
     setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
@@ -18,6 +18,12 @@ void TcPacketPus::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
     tcData->dataField.versionTypeAck |= (ack & 0x0F);
     tcData->dataField.serviceType = service;
     tcData->dataField.serviceSubtype = subservice;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    tcData->dataField.sourceIdH = (sourceId >> 8) | 0xff;
+    tcData->dataField.sourceIdL = sourceId & 0xff;
+#else
+    tcData->dataField.sourceId = sourceId;
+#endif
 }
 
 uint8_t TcPacketPus::getService() const {
diff --git a/tmtcpacket/pus/tc/TcPacketPus.h b/tmtcpacket/pus/tc/TcPacketPus.h
index b50b868d..7a28a957 100644
--- a/tmtcpacket/pus/tc/TcPacketPus.h
+++ b/tmtcpacket/pus/tc/TcPacketPus.h
@@ -75,7 +75,7 @@ protected:
      * @param subservice PUS Subservice
      */
     void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack,
-            uint8_t service, uint8_t subservice);
+            uint8_t service, uint8_t subservice, uint16_t sourceId = 0);
 
     /**
      * A pointer to a structure which defines the data structure of

From 4b248740f326b3329c417f3050e0f1cb9c472fd5 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 13:32:11 +0200
Subject: [PATCH 143/157] added HAL gpio class ID

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

diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h
index 60cb33ac..af32f9a7 100644
--- a/returnvalues/FwClassIds.h
+++ b/returnvalues/FwClassIds.h
@@ -75,6 +75,7 @@ enum: uint8_t {
     HAL_SPI, //HSPI
     HAL_UART, //HURT
     HAL_I2C, //HI2C
+    HAL_GPIO, //HGIO
     FW_CLASS_ID_COUNT // [EXPORT] : [END]
 
 };

From c371cf485130178fa9ebd8ba1d7f8cfd8debd1ea Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 14:21:46 +0200
Subject: [PATCH 144/157] changelog update

---
 CHANGELOG | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/CHANGELOG b/CHANGELOG
index add8e1a5..09b8db6a 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -1,3 +1,16 @@
+## Changes from ASTP 1.0.0 to 1.1.0
+
+### PUS
+
+- Added PUS C support
+
+### Configuration
+
+- Additional configuration option fsfwconfig::FSFW_MAX_TM_PACKET_SIZE which 
+  need to be specified in FSFWConfig.h
+
+
+
 ## Changes from ASTP 0.0.1 to 1.0.0
 
 ### Host OSAL

From aa1bfcbb967465658ff50a0d73bb9500e792b78e Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 14:52:56 +0200
Subject: [PATCH 145/157] DHB update

---
 devicehandlers/DeviceHandlerBase.cpp | 27 +++++++++++++--------------
 devicehandlers/DeviceHandlerBase.h   |  2 +-
 2 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 43660616..2a453a37 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -226,16 +226,15 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 }
 
 void DeviceHandlerBase::decrementDeviceReplyMap() {
-    for (std::map<DeviceCommandId_t, DeviceReplyInfo>::iterator iter =
-            deviceReplyMap.begin(); iter != deviceReplyMap.end(); iter++) {
-        if (iter->second.delayCycles != 0) {
-            iter->second.delayCycles--;
-            if (iter->second.delayCycles == 0) {
-                if (iter->second.periodic) {
-                    iter->second.delayCycles = iter->second.maxDelayCycles;
+    for (auto pair: deviceReplyMap) {
+        if (pair.second.delayCycles != 0) {
+            pair.second.delayCycles--;
+            if (pair.second.delayCycles == 0) {
+                if (pair.second.periodic) {
+                    pair.second.delayCycles = pair.second.maxDelayCycles;
                 }
-                replyToReply(iter, TIMEOUT);
-                missedReply(iter->first);
+                replyToReply(pair.first, pair.second, TIMEOUT);
+                missedReply(pair.first);
             }
         }
     }
@@ -584,16 +583,16 @@ void DeviceHandlerBase::replyToCommand(ReturnValue_t status,
     }
 }
 
-void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter,
+void DeviceHandlerBase::replyToReply(DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
         ReturnValue_t status) {
     // No need to check if iter exists, as this is checked by callers.
     // If someone else uses the method, add check.
-    if (iter->second.command == deviceCommandMap.end()) {
+    if (replyInfo.command == deviceCommandMap.end()) {
         //Is most likely periodic reply. Silent return.
         return;
     }
     // Check if more replies are expected. If so, do nothing.
-    DeviceCommandInfo* info = &(iter->second.command->second);
+    DeviceCommandInfo* info = &replyInfo.command->second;
     if (--info->expectedReplies == 0) {
         // Check if it was transition or internal command.
         // Don't send any replies in that case.
@@ -602,7 +601,7 @@ void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter,
             if(status == HasReturnvaluesIF::RETURN_OK) {
                 success = true;
             }
-            actionHelper.finish(success, info->sendReplyTo, iter->first, status);
+            actionHelper.finish(success, info->sendReplyTo, command, status);
         }
         info->isExecuting = false;
     }
@@ -801,7 +800,7 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData,
             replyRawReplyIfnotWiretapped(receivedData, foundLen);
             triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId);
         }
-        replyToReply(iter, result);
+        replyToReply(iter->first, iter->second, result);
     }
     else {
         /* Other completion failure messages are created by timeout.
diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index 496c08ff..5141800f 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -1195,7 +1195,7 @@ private:
      * @foundLen the length of the packet
      */
     void handleReply(const uint8_t *data, DeviceCommandId_t id, uint32_t foundLen);
-    void replyToReply(DeviceReplyMap::iterator iter, ReturnValue_t status);
+    void replyToReply(DeviceCommandId_t command, DeviceReplyInfo& replyInfo, ReturnValue_t status);
 
     /**
      * Build and send a command to the device.

From 1c7c532ef6c237938aac18069e8595c2a8162fc4 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 15:01:02 +0200
Subject: [PATCH 146/157] DHB update

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

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 2a453a37..dfa9f947 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -226,15 +226,15 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 }
 
 void DeviceHandlerBase::decrementDeviceReplyMap() {
-    for (auto pair: deviceReplyMap) {
-        if (pair.second.delayCycles != 0) {
-            pair.second.delayCycles--;
-            if (pair.second.delayCycles == 0) {
-                if (pair.second.periodic) {
-                    pair.second.delayCycles = pair.second.maxDelayCycles;
+    for (auto replyPair: deviceReplyMap) {
+        if (replyPair.second.delayCycles != 0) {
+            replyPair.second.delayCycles--;
+            if (replyPair.second.delayCycles == 0) {
+                if (replyPair.second.periodic) {
+                    replyPair.second.delayCycles = replyPair.second.maxDelayCycles;
                 }
-                replyToReply(pair.first, pair.second, TIMEOUT);
-                missedReply(pair.first);
+                replyToReply(replyPair.first, replyPair.second, TIMEOUT);
+                missedReply(replyPair.first);
             }
         }
     }

From cc5c8ca698615bdb426d41d6eef11afff6450ac0 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 15:04:40 +0200
Subject: [PATCH 147/157] type

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

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index dfa9f947..e0062a81 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -226,7 +226,7 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 }
 
 void DeviceHandlerBase::decrementDeviceReplyMap() {
-    for (auto replyPair: deviceReplyMap) {
+    for (std::pair<const DeviceCommandId_t, DeviceReplyInfo>& replyPair: deviceReplyMap) {
         if (replyPair.second.delayCycles != 0) {
             replyPair.second.delayCycles--;
             if (replyPair.second.delayCycles == 0) {

From 03e1a3e945c860f4a69d2e74aebff31228e8e5b4 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 15:08:18 +0200
Subject: [PATCH 148/157] const correctness

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

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index e0062a81..2a3f9d66 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -583,7 +583,7 @@ void DeviceHandlerBase::replyToCommand(ReturnValue_t status,
     }
 }
 
-void DeviceHandlerBase::replyToReply(DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
+void DeviceHandlerBase::replyToReply(const DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
         ReturnValue_t status) {
     // No need to check if iter exists, as this is checked by callers.
     // If someone else uses the method, add check.
diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index 5141800f..c278985e 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -1195,7 +1195,8 @@ private:
      * @foundLen the length of the packet
      */
     void handleReply(const uint8_t *data, DeviceCommandId_t id, uint32_t foundLen);
-    void replyToReply(DeviceCommandId_t command, DeviceReplyInfo& replyInfo, ReturnValue_t status);
+    void replyToReply(const DeviceCommandId_t command, DeviceReplyInfo& replyInfo,
+            ReturnValue_t status);
 
     /**
      * Build and send a command to the device.

From 0cde65f5a1ccc7f18074cb8ede32f702b0256802 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 15:33:52 +0200
Subject: [PATCH 149/157] Added some precautions in DHB::replyToReply

---
 devicehandlers/DeviceHandlerBase.cpp | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 2a3f9d66..5665b101 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -591,9 +591,20 @@ void DeviceHandlerBase::replyToReply(const DeviceCommandId_t command, DeviceRepl
         //Is most likely periodic reply. Silent return.
         return;
     }
-    // Check if more replies are expected. If so, do nothing.
     DeviceCommandInfo* info = &replyInfo.command->second;
-    if (--info->expectedReplies == 0) {
+    if (info == nullptr){
+    	printWarningOrError(sif::OutputTypes::OUT_ERROR,
+    	                        "replyToReply", HasReturnvaluesIF::RETURN_FAILED,
+    	                        "Command pointer not found");
+    	return;
+    }
+
+    if (info->expectedReplies > 0){
+    	// Check before to avoid underflow
+    	info->expectedReplies--;
+    }
+    // Check if more replies are expected. If so, do nothing.
+    if (info->expectedReplies == 0) {
         // Check if it was transition or internal command.
         // Don't send any replies in that case.
         if (info->sendReplyTo != NO_COMMANDER) {

From ecb03b8a6db9e8e7fcf673f424104055af5247d6 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 15:59:20 +0200
Subject: [PATCH 150/157] Moved Code to own cpp

---
 timemanager/CMakeLists.txt  |  1 +
 timemanager/Clock.h         | 60 +++-----------------------------
 timemanager/ClockCommon.cpp | 68 +++++++++++++++++++++++++++++++++++++
 3 files changed, 73 insertions(+), 56 deletions(-)
 create mode 100644 timemanager/ClockCommon.cpp

diff --git a/timemanager/CMakeLists.txt b/timemanager/CMakeLists.txt
index 3367775f..70dd41fa 100644
--- a/timemanager/CMakeLists.txt
+++ b/timemanager/CMakeLists.txt
@@ -5,4 +5,5 @@ target_sources(${LIB_FSFW_NAME}
 		Stopwatch.cpp
 		TimeMessage.cpp
 		TimeStamper.cpp
+		ClockCommon.cpp
 )
diff --git a/timemanager/Clock.h b/timemanager/Clock.h
index 2ac48384..5750e3d8 100644
--- a/timemanager/Clock.h
+++ b/timemanager/Clock.h
@@ -133,28 +133,7 @@ public:
 	 *  - @c RETURN_OK on success
 	 *  - @c RETURN_FAILED if leapSeconds are not set
 	 */
-	static ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
-		if (timeMutex == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-
-		uint16_t leapSeconds;
-		ReturnValue_t result = getLeapSeconds(&leapSeconds);
-		if (result != HasReturnvaluesIF::RETURN_OK) {
-			return result;
-		}
-		timeval leapSeconds_timeval = { 0, 0 };
-		leapSeconds_timeval.tv_sec = leapSeconds;
-
-		//initial offset between UTC and TAI
-		timeval UTCtoTAI1972 = { 10, 0 };
-
-		timeval TAItoTT = { 32, 184000 };
-
-		*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
-
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+	static ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt);
 
 	/**
 	 * Set the Leap Seconds since 1972
@@ -163,16 +142,7 @@ public:
 	 * @return
 	 *  - @c RETURN_OK on success.
 	 */
-	static ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
-		if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		MutexGuard helper(timeMutex);
-
-		leapSeconds = leapSeconds_;
-
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+	static ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_);
 
 	/**
 	 * Get the Leap Seconds since 1972
@@ -184,17 +154,7 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - @c RETURN_FAILED on error
 	 */
-	static ReturnValue_t getLeapSeconds(uint16_t *leapSeconds_);
-	ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
-		if (timeMutex == nullptr) {
-			return HasReturnvaluesIF::RETURN_FAILED;
-		}
-		MutexGuard helper(timeMutex);
-
-		*leapSeconds_ = leapSeconds;
-
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+	static ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_);
 
 private:
 	/**
@@ -203,19 +163,7 @@ private:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise @c RETURN_FAILED if not able to create one
 	 */
-	static ReturnValue_t Clock::checkOrCreateClockMutex() {
-		if (timeMutex == nullptr) {
-			MutexFactory *mutexFactory = MutexFactory::instance();
-			if (mutexFactory == nullptr) {
-				return HasReturnvaluesIF::RETURN_FAILED;
-			}
-			timeMutex = mutexFactory->createMutex();
-			if (timeMutex == nullptr) {
-				return HasReturnvaluesIF::RETURN_FAILED;
-			}
-		}
-		return HasReturnvaluesIF::RETURN_OK;
-	}
+	static ReturnValue_t Clock::checkOrCreateClockMutex();
 
 	static MutexIF *timeMutex;
 	static uint16_t leapSeconds;
diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp
new file mode 100644
index 00000000..62be4375
--- /dev/null
+++ b/timemanager/ClockCommon.cpp
@@ -0,0 +1,68 @@
+/*
+ * ClockCommon.cpp
+ *
+ *  Created on: Jun 15, 2021
+ *      Author: steffen
+ */
+
+#include "Clock.h"
+#include "../ipc/MutexGuard.h"
+
+ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
+	if (timeMutex == nullptr) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	uint16_t leapSeconds;
+	ReturnValue_t result = getLeapSeconds(&leapSeconds);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	timeval leapSeconds_timeval = { 0, 0 };
+	leapSeconds_timeval.tv_sec = leapSeconds;
+
+	//initial offset between UTC and TAI
+	timeval UTCtoTAI1972 = { 10, 0 };
+
+	timeval TAItoTT = { 32, 184000 };
+
+	*tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
+	if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	MutexGuard helper(timeMutex);
+
+	leapSeconds = leapSeconds_;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
+	if (timeMutex == nullptr) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	MutexGuard helper(timeMutex);
+
+	*leapSeconds_ = leapSeconds;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+static ReturnValue_t Clock::checkOrCreateClockMutex() {
+	if (timeMutex == nullptr) {
+		MutexFactory *mutexFactory = MutexFactory::instance();
+		if (mutexFactory == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		timeMutex = mutexFactory->createMutex();
+		if (timeMutex == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}

From 717971f69c67ece165f19ea4e2c11b2825e9c942 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:01:28 +0200
Subject: [PATCH 151/157] Fixed wrong namespace qualifier

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

diff --git a/timemanager/Clock.h b/timemanager/Clock.h
index 5750e3d8..6a76c86d 100644
--- a/timemanager/Clock.h
+++ b/timemanager/Clock.h
@@ -133,7 +133,7 @@ public:
 	 *  - @c RETURN_OK on success
 	 *  - @c RETURN_FAILED if leapSeconds are not set
 	 */
-	static ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt);
+	static ReturnValue_t convertUTCToTT(timeval utc, timeval *tt);
 
 	/**
 	 * Set the Leap Seconds since 1972
@@ -142,7 +142,7 @@ public:
 	 * @return
 	 *  - @c RETURN_OK on success.
 	 */
-	static ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_);
+	static ReturnValue_t setLeapSeconds(const uint16_t leapSeconds_);
 
 	/**
 	 * Get the Leap Seconds since 1972
@@ -154,7 +154,7 @@ public:
 	 *  - @c RETURN_OK on success.
 	 *  - @c RETURN_FAILED on error
 	 */
-	static ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_);
+	static ReturnValue_t getLeapSeconds(uint16_t *leapSeconds_);
 
 private:
 	/**
@@ -163,7 +163,7 @@ private:
 	 *  - @c RETURN_OK on success.
 	 *  - Otherwise @c RETURN_FAILED if not able to create one
 	 */
-	static ReturnValue_t Clock::checkOrCreateClockMutex();
+	static ReturnValue_t checkOrCreateClockMutex();
 
 	static MutexIF *timeMutex;
 	static uint16_t leapSeconds;

From dbfcf8b271d8e37d5645288d00e8ec26e8d3eb81 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:03:11 +0200
Subject: [PATCH 152/157] Removed old check

---
 timemanager/ClockCommon.cpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp
index 62be4375..efec0937 100644
--- a/timemanager/ClockCommon.cpp
+++ b/timemanager/ClockCommon.cpp
@@ -9,10 +9,6 @@
 #include "../ipc/MutexGuard.h"
 
 ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
-	if (timeMutex == nullptr) {
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
 	uint16_t leapSeconds;
 	ReturnValue_t result = getLeapSeconds(&leapSeconds);
 	if (result != HasReturnvaluesIF::RETURN_OK) {

From ee89a2f00d8bf7c36f63ecea1a8a1a3305adea3d Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:05:12 +0200
Subject: [PATCH 153/157] Removed author tag

---
 timemanager/ClockCommon.cpp | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp
index efec0937..8b75f3e5 100644
--- a/timemanager/ClockCommon.cpp
+++ b/timemanager/ClockCommon.cpp
@@ -1,10 +1,3 @@
-/*
- * ClockCommon.cpp
- *
- *  Created on: Jun 15, 2021
- *      Author: steffen
- */
-
 #include "Clock.h"
 #include "../ipc/MutexGuard.h"
 

From 2ccc0dbb0075d3c90068a565ef55436affb3de71 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:12:25 +0200
Subject: [PATCH 154/157] Removed wrong static

---
 timemanager/ClockCommon.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp
index 8b75f3e5..e56d4953 100644
--- a/timemanager/ClockCommon.cpp
+++ b/timemanager/ClockCommon.cpp
@@ -42,7 +42,7 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
-static ReturnValue_t Clock::checkOrCreateClockMutex() {
+ReturnValue_t Clock::checkOrCreateClockMutex() {
 	if (timeMutex == nullptr) {
 		MutexFactory *mutexFactory = MutexFactory::instance();
 		if (mutexFactory == nullptr) {

From bd7c13ff7e187bb5590f597d87cd2a76c2650497 Mon Sep 17 00:00:00 2001
From: Steffen Gaisser <gaisser@irs.uni-stuttgart.de>
Date: Tue, 15 Jun 2021 16:47:24 +0200
Subject: [PATCH 155/157] NO_COMMANDER was not using NO_QUEUE in DHB

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

diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index c278985e..53bd1e65 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -667,7 +667,7 @@ protected:
     static const ReturnValue_t CHILD_TIMEOUT = MAKE_RETURN_CODE(0xE0);
     static const ReturnValue_t SWITCH_FAILED = MAKE_RETURN_CODE(0xE1);
 
-    static const MessageQueueId_t NO_COMMANDER = 0;
+    static const MessageQueueId_t NO_COMMANDER = MessageQueueIF::NO_QUEUE;
 
     //! Pointer to the raw packet that will be sent.
     uint8_t *rawPacket = nullptr;

From 47af5260a2341249bc00cdba095f96efa5a509dc Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Thu, 17 Jun 2021 12:04:39 +0200
Subject: [PATCH 156/157] added missing include

---
 tmtcpacket/pus/tc/TcPacketStoredPus.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/tmtcpacket/pus/tc/TcPacketStoredPus.cpp b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
index f098ce2b..426aafdb 100644
--- a/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
+++ b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
@@ -1,5 +1,7 @@
 #include "TcPacketStoredPus.h"
 
+#include "../../../serviceinterface/ServiceInterface.h"
+
 #include <cstring>
 
 TcPacketStoredPus::TcPacketStoredPus(uint16_t apid,  uint8_t service,

From b52644dd2af39901f442ac30833f27cbddc4b9df Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 21 Jun 2021 14:50:19 +0200
Subject: [PATCH 157/157] tiny form fix

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

diff --git a/osal/linux/PeriodicPosixTask.cpp b/osal/linux/PeriodicPosixTask.cpp
index 1cba22ee..c0152bec 100644
--- a/osal/linux/PeriodicPosixTask.cpp
+++ b/osal/linux/PeriodicPosixTask.cpp
@@ -32,7 +32,7 @@ ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) {
 		sif::error << "PeriodicTask::addComponent: Invalid object. Make sure"
 				<< " it implements ExecutableObjectIF!" << std::endl;
 #else
-		sif::printError("PeriodicTask::addComponent: Invalid object. Make sure it"
+		sif::printError("PeriodicTask::addComponent: Invalid object. Make sure it "
 		        "implements ExecutableObjectIF!\n");
 #endif
 		return HasReturnvaluesIF::RETURN_FAILED;