diff --git a/globalfunctions/printer.cpp b/globalfunctions/printer.cpp
index bf4c5848..a68a91ee 100644
--- a/globalfunctions/printer.cpp
+++ b/globalfunctions/printer.cpp
@@ -1,36 +1,61 @@
 #include <framework/globalfunctions/printer.h>
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
+#include <bitset>
 
-void printer::print(uint8_t *data, size_t size, OutputType type) {
-	sif::info << "Printing data with size " << size << ": [";
-	if(type == OutputType::HEX) {
-		printer::printHex(data, size);
+void printer::print(const uint8_t *data, size_t size, OutputType type,
+		bool printInfo, size_t maxCharPerLine) {
+	if(printInfo) {
+		sif::info << "Printing data with size " << size << ": ";
 	}
-	else {
-		printer::printDec(data, size);
+	sif::info << "[";
+	if(type == OutputType::HEX) {
+		printer::printHex(data, size, maxCharPerLine);
+	}
+	else if (type == OutputType::DEC) {
+		printer::printDec(data, size, maxCharPerLine);
+	}
+	else if(type == OutputType::BIN) {
+	    printer::printBin(data, size);
 	}
 }
 
-void printer::printHex(uint8_t *data, size_t size) {
+void printer::printHex(const uint8_t *data, size_t size,
+		size_t maxCharPerLine) {
 	sif::info << std::hex;
 	for(size_t i = 0; i < size; i++) {
 		sif::info << "0x" << static_cast<int>(data[i]);
 		if(i < size - 1){
 			sif::info << " , ";
+			if(i > 0 and i % maxCharPerLine == 0) {
+				sif::info << std::endl;
+			}
 		}
+
 	}
 	sif::info << std::dec;
 	sif::info << "]" << std::endl;
 }
 
-void printer::printDec(uint8_t *data, size_t size) {
+void printer::printDec(const uint8_t *data, size_t size,
+		size_t maxCharPerLine) {
 	sif::info << std::dec;
 	for(size_t i = 0; i < size; i++) {
-		sif::info << "0x" << static_cast<int>(data[i]);
+		sif::info << static_cast<int>(data[i]);
 		if(i < size - 1){
 			sif::info << " , ";
+			if(i > 0 and i % maxCharPerLine == 0) {
+				sif::info << std::endl;
+			}
 		}
 	}
 	sif::info << "]" << std::endl;
 }
 
+void printer::printBin(const uint8_t *data, size_t size) {
+    sif::info << "\n" << std::flush;
+    for(size_t i = 0; i < size; i++) {
+        sif::info << "Byte " << i + 1 << ": 0b"<<
+                std::bitset<8>(data[i]) << ",\n" << std::flush;
+    }
+    sif::info << "]" << std::endl;
+}
diff --git a/globalfunctions/printer.h b/globalfunctions/printer.h
index 3c9d6aa0..33a382ec 100644
--- a/globalfunctions/printer.h
+++ b/globalfunctions/printer.h
@@ -7,12 +7,15 @@ namespace printer {
 
 enum class OutputType {
 	DEC,
-	HEX
+	HEX,
+	BIN
 };
 
-void print(uint8_t* data, size_t size, OutputType type = OutputType::HEX);
-void printHex(uint8_t* data, size_t size);
-void printDec(uint8_t* data, size_t size);
+void print(const uint8_t* data, size_t size, OutputType type = OutputType::HEX,
+		bool printInfo = true, size_t maxCharPerLine = 12);
+void printHex(const uint8_t* data, size_t size, size_t maxCharPerLine = 12);
+void printDec(const uint8_t* data, size_t size, size_t maxCharPerLine = 12);
+void printBin(const uint8_t* data, size_t size);
 }
 
 #endif /* FRAMEWORK_GLOBALFUNCTIONS_PRINTER_H_ */
diff --git a/osal/FreeRTOS/BinarySemaphore.cpp b/osal/FreeRTOS/BinarySemaphore.cpp
index 6cacef65..026704d6 100644
--- a/osal/FreeRTOS/BinarySemaphore.cpp
+++ b/osal/FreeRTOS/BinarySemaphore.cpp
@@ -1,8 +1,3 @@
-/**
- * @file BinarySemaphore.cpp
- *
- * @date 25.02.2020
- */
 #include <framework/osal/FreeRTOS/BinarySemaphore.h>
 #include <framework/osal/FreeRTOS/TaskManagement.h>
 
@@ -10,9 +5,8 @@
 
 BinarySemaphore::BinarySemaphore() {
 	handle = xSemaphoreCreateBinary();
-	if(handle == nullptr) {
-
-		sif::error << "Binary semaphore creation failure" << std::endl;
+	if(handle == nullptr) {
+		sif::error << "Semaphore: Binary semaph creation failure" << std::endl;
 	}
 	xSemaphoreGive(handle);
 }
@@ -21,27 +15,6 @@ BinarySemaphore::~BinarySemaphore() {
 	vSemaphoreDelete(handle);
 }
 
-// This copy ctor is important as it prevents the assignment to a ressource
-// (other.handle) variable which is later deleted!
-BinarySemaphore::BinarySemaphore(const BinarySemaphore& other) {
-    handle = xSemaphoreCreateBinary();
-    if(handle == nullptr) {
-        sif::error << "Binary semaphore creation failure" << std::endl;
-    }
-    xSemaphoreGive(handle);
-}
-
-BinarySemaphore& BinarySemaphore::operator =(const BinarySemaphore& s) {
-    if(this != &s) {
-        handle = xSemaphoreCreateBinary();
-        if(handle == nullptr) {
-            sif::error << "Binary semaphore creation failure" << std::endl;
-        }
-        xSemaphoreGive(handle);
-    }
-    return *this;
-}
-
 BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) {
     handle = xSemaphoreCreateBinary();
     if(handle == nullptr) {
@@ -132,7 +105,18 @@ void BinarySemaphore::resetSemaphore() {
 		xSemaphoreGive(handle);
 	}
 }
+
+ReturnValue_t BinarySemaphore::acquire(uint32_t timeoutMs) {
+	return takeBinarySemaphore(timeoutMs);
+}
 
+ReturnValue_t BinarySemaphore::release() {
+	return giveBinarySemaphore();
+}
+
+uint8_t BinarySemaphore::getSemaphoreCounter() {
+	return uxSemaphoreGetCount(handle);
+}
 
 // Be careful with the stack size here. This is called from an ISR!
 ReturnValue_t BinarySemaphore::giveBinarySemaphoreFromISR(SemaphoreHandle_t semaphore,
@@ -151,4 +135,4 @@ ReturnValue_t BinarySemaphore::giveBinarySemaphoreFromISR(SemaphoreHandle_t sema
 	} else {
 		return SEMAPHORE_NOT_OWNED;
 	}
-}
+}
diff --git a/osal/FreeRTOS/BinarySemaphore.h b/osal/FreeRTOS/BinarySemaphore.h
index 0ab9cdea..5f69dc90 100644
--- a/osal/FreeRTOS/BinarySemaphore.h
+++ b/osal/FreeRTOS/BinarySemaphore.h
@@ -1,30 +1,30 @@
-/**
- * @file BinarySempahore.h
- *
- * @date 25.02.2020
- */
 #ifndef FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_
 #define FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_
 
-#include <framework/returnvalues/HasReturnvaluesIF.h>
-
+#include <framework/returnvalues/HasReturnvaluesIF.h>
+#include <framework/tasks/SemaphoreIF.h>
 extern "C" {
-#include "FreeRTOS.h"
-#include "semphr.h"
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
 }
 
-
+// TODO: Counting semaphores and implement the new (better)
+//		 task notifications. However, those use task notifications require
+//		 the task handle. Maybe it would be better to make a separate class
+// 		 and switch between the classes with #ifdefs.
+//		 Task Notifications require FreeRTOS V8.2 something..
 /**
- * @brief OS Tool to achieve synchronization of between tasks or between task and ISR
+ * @brief 	OS Tool to achieve synchronization of between tasks or between
+ * 			task and ISR. The default semaphore implementation creates a
+ * 			binary semaphore, which can only be taken once.
  * @details
  * Documentation: https://www.freertos.org/Embedded-RTOS-Binary-Semaphores.html
  *
- * SHOULDDO: check freeRTOS version and use new task notifications,
- * if non-ancient freeRTOS version is used.
- *
+ * @author 	R. Mueller
  * @ingroup osal
  */
-class BinarySemaphore: public HasReturnvaluesIF {
+class BinarySemaphore: public SemaphoreIF,
+		public HasReturnvaluesIF {
 public:
 	static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF;
 
@@ -36,40 +36,25 @@ public:
 	//! Can be passed as tick type and ms value.
 	static constexpr TickType_t BLOCK_TIMEOUT_TICKS = portMAX_DELAY;
 	static constexpr uint32_t BLOCK_TIMEOUT = portMAX_DELAY;
-
-	//! Semaphore timeout
-	static constexpr ReturnValue_t SEMAPHORE_TIMEOUT = MAKE_RETURN_CODE(1);
-	/** The current semaphore can not be given, because it is not owned */
-	static constexpr ReturnValue_t SEMAPHORE_NOT_OWNED = MAKE_RETURN_CODE(2);
-	static constexpr ReturnValue_t SEMAPHORE_NULLPOINTER = MAKE_RETURN_CODE(3);
-
+
+	//! @brief Default ctor
 	BinarySemaphore();
-
-	/**
-	 * @brief Copy ctor
-	 */
-	BinarySemaphore(const BinarySemaphore&);
-
-	/**
-	 * @brief Copy assignment
-	 */
-	BinarySemaphore& operator=(const BinarySemaphore&);
-
-	/**
-	 * @brief Move constructor
-	 */
+	//! @brief Copy ctor, deleted explicitely.
+	BinarySemaphore(const BinarySemaphore&) = delete;
+	//! @brief Copy assignment, deleted explicitely.
+	BinarySemaphore& operator=(const BinarySemaphore&) = delete;
+	//! @brief Move ctor
 	BinarySemaphore (BinarySemaphore &&);
-
-	/**
-	 * Move assignment
-	 */
+	//! @brief Move assignment
 	BinarySemaphore & operator=(BinarySemaphore &&);
-
-	/**
-	 * Delete the binary semaphore to prevent a memory leak
-	 */
+	//! @brief Destructor
 	virtual ~BinarySemaphore();
 
+	ReturnValue_t acquire(uint32_t timeoutMs =
+	        BinarySemaphore::NO_BLOCK_TIMEOUT) override;
+	ReturnValue_t release() override;
+	uint8_t getSemaphoreCounter() override;
+
 	/**
 	 * Take the binary semaphore.
 	 * If the semaphore has already been taken, the task will be blocked
@@ -127,7 +112,8 @@ public:
 	 */
 	static ReturnValue_t giveBinarySemaphoreFromISR(SemaphoreHandle_t semaphore,
 				BaseType_t * higherPriorityTaskWoken);
-private:
+
+protected:
 	SemaphoreHandle_t handle;
 };
 
diff --git a/osal/FreeRTOS/CountingSemaphore.cpp b/osal/FreeRTOS/CountingSemaphore.cpp
new file mode 100644
index 00000000..2b81848a
--- /dev/null
+++ b/osal/FreeRTOS/CountingSemaphore.cpp
@@ -0,0 +1,28 @@
+#include <framework/osal/FreeRTOS/CountingSemaphore.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+
+// Make sure #define configUSE_COUNTING_SEMAPHORES 1 is set in
+// free FreeRTOSConfig.h file.
+CountingSemaphore::CountingSemaphore(uint8_t count, uint8_t initCount):
+		count(count), initCount(initCount) {
+	handle = xSemaphoreCreateCounting(count, initCount);
+	if(handle == nullptr) {
+		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+	}
+}
+
+CountingSemaphore::CountingSemaphore(CountingSemaphore&& other) {
+	handle = xSemaphoreCreateCounting(other.count, other.initCount);
+	if(handle == nullptr) {
+		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+	}
+}
+
+CountingSemaphore& CountingSemaphore::operator =(
+		CountingSemaphore&& other) {
+	handle = xSemaphoreCreateCounting(other.count, other.initCount);
+	if(handle == nullptr) {
+		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+	}
+	return * this;
+}
diff --git a/osal/FreeRTOS/CountingSemaphore.h b/osal/FreeRTOS/CountingSemaphore.h
new file mode 100644
index 00000000..a33a0fa5
--- /dev/null
+++ b/osal/FreeRTOS/CountingSemaphore.h
@@ -0,0 +1,29 @@
+#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_
+#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_
+#include <framework/osal/FreeRTOS/BinarySemaphore.h>
+
+/**
+ * @brief 	Counting semaphores, which can be acquire more than once.
+ * @details
+ * See: https://www.freertos.org/CreateCounting.html
+ * API of counting semaphores is almost identical to binary semaphores,
+ * so we just inherit from binary semaphore and provide the respective
+ * constructors.
+ */
+class CountingSemaphore: public BinarySemaphore {
+public:
+	CountingSemaphore(uint8_t count, uint8_t initCount);
+	//! @brief Copy ctor, disabled
+	CountingSemaphore(const CountingSemaphore&) = delete;
+	//! @brief Copy assignment, disabled
+	CountingSemaphore& operator=(const CountingSemaphore&) = delete;
+	//! @brief Move ctor
+	CountingSemaphore (CountingSemaphore &&);
+	//! @brief Move assignment
+	CountingSemaphore & operator=(CountingSemaphore &&);
+private:
+	uint8_t count = 0;
+	uint8_t initCount = 0;
+};
+
+#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */
diff --git a/osal/FreeRTOS/FixedTimeslotTask.cpp b/osal/FreeRTOS/FixedTimeslotTask.cpp
index 1bb077e2..18ec1092 100644
--- a/osal/FreeRTOS/FixedTimeslotTask.cpp
+++ b/osal/FreeRTOS/FixedTimeslotTask.cpp
@@ -57,7 +57,6 @@ ReturnValue_t FixedTimeslotTask::startTask() {
 
 ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
 		uint32_t slotTimeMs, int8_t executionStep) {
-
 	if (objectManager->get<ExecutableObjectIF>(componentId) != nullptr) {
 		if(slotTimeMs == 0) {
 			// FreeRTOS throws a sanity error for zero values, so we set
diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index 38f425a4..9e4b581b 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -7,11 +7,10 @@
 // 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) :
-defaultDestination(0),lastPartner(0), callContext(CallContext::task)  {
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) {
 	handle = xQueueCreate(messageDepth, maxMessageSize);
 	if (handle == NULL) {
-		sif::error << "MessageQueue creation failed" << std::endl;
+		sif::error << "MessageQueue: Creation failed" << std::endl;
 	}
 }
 
@@ -50,16 +49,18 @@ ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
 ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
 		MessageQueueMessage* message, MessageQueueId_t sentFrom,
 		bool ignoreFault) {
-	return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault, callContext);
+	return sendMessageFromMessageQueue(sendTo, message, sentFrom,
+			ignoreFault, callContext);
 }
 
 
 ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) {
 	if (result != pdPASS) {
-		if (!ignoreFault) {
-			InternalErrorReporterIF* internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
+		if (not ignoreFault) {
+			InternalErrorReporterIF* internalErrorReporter =
+					objectManager->get<InternalErrorReporterIF>(
 					objects::INTERNAL_ERROR_REPORTER);
-			if (internalErrorReporter != NULL) {
+			if (internalErrorReporter != nullptr) {
 				internalErrorReporter->queueMessageNotSent();
 			}
 		}
@@ -78,7 +79,8 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message,
 }
 
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) {
-	BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(message->getBuffer()), 0);
+	BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(
+			message->getBuffer()), 0);
 	if (result == pdPASS){
 		this->lastPartner = message->getSender();
 		return HasReturnvaluesIF::RETURN_OK;
@@ -103,6 +105,7 @@ MessageQueueId_t MessageQueue::getId() const {
 }
 
 void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
+	defaultDestinationSet = true;
 	this->defaultDestination = defaultDestination;
 }
 
@@ -111,7 +114,7 @@ MessageQueueId_t MessageQueue::getDefaultDestination() const {
 }
 
 bool MessageQueue::isDefaultDestinationSet() const {
-	return 0;
+	return defaultDestinationSet;
 }
 
 
@@ -126,9 +129,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
                 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()),
diff --git a/osal/FreeRTOS/MessageQueue.h b/osal/FreeRTOS/MessageQueue.h
index ce6ed46b..988733f6 100644
--- a/osal/FreeRTOS/MessageQueue.h
+++ b/osal/FreeRTOS/MessageQueue.h
@@ -12,55 +12,71 @@ extern "C" {
 }
 
 
-
-//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
+// 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.
+ * @brief		This class manages sending and receiving of
+ * 				message queue messages.
+ * @details
+ * Message queues are used to pass asynchronous messages between processes.
+ * They work like post boxes, where all incoming messages are stored in FIFO
+ * order. This class creates a new receiving queue and provides methods to fetch
+ * received messages. Being a child of MessageQueueSender, this class also
+ * provides methods to send a message to a user-defined or a default destination.
+ * In addition it also provides a reply method to answer to the queue it
+ * received its last message from.
  *
- *	@details	Message queues are used to pass asynchronous messages between processes.
- *				They work like post boxes, where all incoming messages are stored in FIFO
- *				order. This class creates a new receiving queue and provides methods to fetch
- *				received messages. Being a child of MessageQueueSender, this class also provides
- *				methods to send a message to a user-defined or a default destination. In addition
- *				it also provides a reply method to answer to the queue it received its last message
- *				from.
+ * The MessageQueue should be used as "post box" for a single owning object.
+ * So all message queue communication is "n-to-one".
+ * For creating the queue, as well as sending and receiving messages, the class
+ * makes use of the operating system calls provided.
  *
- *				The MessageQueue should be used as "post box" for a single owning object. So all
- *				message queue communication is "n-to-one".
- *				For creating the queue, as well as sending and receiving messages, the class makes
- *				use of the operating system calls provided.
- *
- *				Please keep in mind that FreeRTOS offers
- *				different calls for message queue operations if called from an ISR.
- *				For now, the system context needs to be switched manually.
- *	@ingroup osal
- *	@ingroup message_queue
+ * Please keep in mind that FreeRTOS offers different calls for message queue
+ * operations if called from an ISR.
+ * For now, the system context needs to be switched manually.
+ * @ingroup osal
+ * @ingroup message_queue
  */
 class MessageQueue : public MessageQueueIF {
 	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 i 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.
+	 * @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 i 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 message_depth = 3, size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE );
+	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;
 
 	/**
 	 * @brief	The destructor deletes the formerly created message queue.
-	 * @details	This is accomplished by using the delete call provided by the operating system.
+	 * @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);
 
 	/**
@@ -90,27 +106,28 @@ public:
 	ReturnValue_t reply( MessageQueueMessage* message );
 
 	/**
-	 * \brief	With the sendMessage call, a queue message is sent to a receiving queue.
-	 * \details	This method takes the message provided, adds the sentFrom information and passes
+	 * @brief	With the sendMessage call, a queue message is sent to a receiving queue.
+	 * @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.
+	 * @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 ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
 	 */
 	virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, MessageQueueMessage* message,
 			MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
 
 	/**
-	 * \brief	The sendToDefault method sends a queue message to the default destination.
-	 * \details	In all other aspects, it works identical to the sendMessage method.
-	 * \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.
+	 * @brief	The sendToDefault method sends a queue message to the default destination.
+	 * @details	In all other aspects, it works identical to the sendMessage method.
+	 * @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.
 	 */
-	virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
+	virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessage* message,
+			MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
 
 	/**
 	 * @brief	This function reads available messages from the message queue and returns the sender.
@@ -147,27 +164,38 @@ public:
 	MessageQueueId_t getId() const;
 
 	/**
+<<<<<<< HEAD
 	 * \brief	This method is a simple setter for the default destination.
+=======
+	 * @brief	This method is a simple setter for the default destination.
+>>>>>>> mueller_BinSempahInterface
 	 */
 	void setDefaultDestination(MessageQueueId_t defaultDestination);
 	/**
-	 * \brief	This method is a simple getter for the default destination.
+	 * @brief	This method is a simple getter for the default destination.
 	 */
 	MessageQueueId_t getDefaultDestination() const;
 
 	bool isDefaultDestinationSet() const;
 protected:
 	/**
-	 * 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
+	 * @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,
 			MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE,
@@ -175,12 +203,13 @@ protected:
 
 	static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
 
-
 private:
+	bool defaultDestinationSet = false;
 	QueueHandle_t handle;
-	MessageQueueId_t defaultDestination;
-	MessageQueueId_t lastPartner;
-	CallContext callContext; //!< Stores the current system context
+	MessageQueueId_t defaultDestination = 0;
+	MessageQueueId_t lastPartner = 0;
+	//!< Stores the current system context
+	CallContext callContext =  CallContext::task;
 };
 
 #endif /* MESSAGEQUEUE_H_ */
diff --git a/osal/FreeRTOS/MutexFactory.cpp b/osal/FreeRTOS/MutexFactory.cpp
index b3e3f02c..a1cca54b 100644
--- a/osal/FreeRTOS/MutexFactory.cpp
+++ b/osal/FreeRTOS/MutexFactory.cpp
@@ -1,9 +1,10 @@
 #include <framework/ipc/MutexFactory.h>
 #include <framework/osal/FreeRTOS/Mutex.h>
 
-//TODO: Different variant than the lazy loading in QueueFactory. What's better and why? -> one is on heap the other on bss/data
+//TODO: Different variant than the lazy loading in QueueFactory.
+//What's better and why? -> one is on heap the other on bss/data
 //MutexFactory* MutexFactory::factoryInstance = new MutexFactory();
-MutexFactory* MutexFactory::factoryInstance = NULL;
+MutexFactory* MutexFactory::factoryInstance = nullptr;
 
 MutexFactory::MutexFactory() {
 }
@@ -12,7 +13,7 @@ MutexFactory::~MutexFactory() {
 }
 
 MutexFactory* MutexFactory::instance() {
-	if (factoryInstance == NULL){
+	if (factoryInstance == nullptr){
 		factoryInstance = new MutexFactory();
 	}
 	return MutexFactory::factoryInstance;
diff --git a/osal/FreeRTOS/SemaphoreFactory.cpp b/osal/FreeRTOS/SemaphoreFactory.cpp
new file mode 100644
index 00000000..9a355891
--- /dev/null
+++ b/osal/FreeRTOS/SemaphoreFactory.cpp
@@ -0,0 +1,33 @@
+#include <framework/osal/FreeRTOS/BinarySemaphore.h>
+#include <framework/osal/FreeRTOS/CountingSemaphore.h>
+#include <framework/tasks/SemaphoreFactory.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+
+SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr;
+
+SemaphoreFactory::SemaphoreFactory() {
+}
+
+SemaphoreFactory::~SemaphoreFactory() {
+	delete factoryInstance;
+}
+
+SemaphoreFactory* SemaphoreFactory::instance() {
+	if (factoryInstance == nullptr){
+		factoryInstance = new SemaphoreFactory();
+	}
+	return SemaphoreFactory::factoryInstance;
+}
+
+SemaphoreIF* SemaphoreFactory::createBinarySemaphore() {
+	return new BinarySemaphore();
+}
+
+SemaphoreIF* SemaphoreFactory::createCountingSemaphore(uint8_t count,
+		uint8_t initCount) {
+	return new CountingSemaphore(count, initCount);
+}
+
+void SemaphoreFactory::deleteMutex(SemaphoreIF* semaphore) {
+	delete semaphore;
+}
diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp
index 52222531..4529b371 100644
--- a/osal/FreeRTOS/TaskManagement.cpp
+++ b/osal/FreeRTOS/TaskManagement.cpp
@@ -1,10 +1,11 @@
 #include <framework/osal/FreeRTOS/TaskManagement.h>
-
+
 void TaskManagement::requestContextSwitchFromTask() {
 	vTaskDelay(0);
 }
-
-void TaskManagement::requestContextSwitch(CallContext callContext = CallContext::task) {
+
+void TaskManagement::requestContextSwitch(
+		CallContext callContext = CallContext::task) {
 	if(callContext == CallContext::isr) {
 		// This function depends on the partmacro.h definition for the specific device
 		requestContextSwitchFromISR();
@@ -12,13 +13,11 @@ void TaskManagement::requestContextSwitch(CallContext callContext = CallContext:
 		requestContextSwitchFromTask();
 	}
 }
-
+
 TaskHandle_t TaskManagement::getCurrentTaskHandle() {
 	return xTaskGetCurrentTaskHandle();
 }
 
 configSTACK_DEPTH_TYPE TaskManagement::getTaskStackHighWatermark() {
 	return uxTaskGetStackHighWaterMark(TaskManagement::getCurrentTaskHandle());
-}
-
-
+}
diff --git a/osal/FreeRTOS/TaskManagement.h b/osal/FreeRTOS/TaskManagement.h
index eaaf97e2..b9874db0 100644
--- a/osal/FreeRTOS/TaskManagement.h
+++ b/osal/FreeRTOS/TaskManagement.h
@@ -8,7 +8,7 @@ extern "C" {
 #include <freertos/task.h>
 }
 #include <cstdint>
-
+
 /**
  * Architecture dependant portmacro.h function call.
  * Should be implemented in bsp.
@@ -17,10 +17,10 @@ extern "C" void requestContextSwitchFromISR();
 
 /*!
  * Used by functions to tell if they are being called from
- * within an ISR or from a regular task. This is required because FreeRTOS
- * has different functions for handling semaphores and messages from within an ISR and task.
+ * within an ISR or from a regular task. This is required because FreeRTOS
+ * has different functions for handling semaphores and messages from within
+ * an ISR and task.
  */
-
 enum CallContext {
 	task = 0x00,//!< task_context
 	isr = 0xFF  //!< isr_context
@@ -29,19 +29,19 @@ enum CallContext {
 
 class TaskManagement {
 public:
-	/**
-	 * In this function, a function dependant on the portmacro.h header function calls
-	 * to request a context switch can be specified.
-	 * This can be used if sending to the queue from an ISR caused a task to unblock
-	 * and a context switch is required.
+	/**
+	 * @brief	In this function, a function dependant on the portmacro.h header
+	 * 			function calls to request a context switch can be specified.
+	 * This can be used if sending to the queue from an ISR caused a task
+	 * to unblock and a context switch is required.
 	 */
 	static void requestContextSwitch(CallContext callContext);
 
 	/**
 	 * If task preemption in FreeRTOS is disabled, a context switch
 	 * can be requested manually by calling this function.
-	 */
-	static void requestContextSwitchFromTask(void);
+	 */
+	static void requestContextSwitchFromTask(void);
 
 	/**
 	 * @return The current task handle
@@ -56,8 +56,8 @@ public:
 	 * E.g. on a 32 bit machine, a value of 200 means 800 bytes.
 	 * @return Smallest value of stack remaining since the task was started in
 	 * 		   words.
-	 */
-	static configSTACK_DEPTH_TYPE getTaskStackHighWatermark();
+	 */
+	static configSTACK_DEPTH_TYPE getTaskStackHighWatermark();
 };
 
 #endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */
diff --git a/storagemanager/LocalPool.h b/storagemanager/LocalPool.h
index 6892b881..c155f918 100644
--- a/storagemanager/LocalPool.h
+++ b/storagemanager/LocalPool.h
@@ -12,7 +12,7 @@
 /**
  * @brief	The LocalPool class provides an intermediate data storage with
  * 			a fixed pool size policy.
- * \details	The class implements the StorageManagerIF interface. While the
+ * @details	The class implements the StorageManagerIF interface. While the
  * 			total number of pools is fixed, the element sizes in one pool and
  * 			the number of pool elements per pool are set on construction.
  * 			The full amount of memory is allocated on construction.
diff --git a/storagemanager/LocalPool.tpp b/storagemanager/LocalPool.tpp
index 59d06ab8..32724bd8 100644
--- a/storagemanager/LocalPool.tpp
+++ b/storagemanager/LocalPool.tpp
@@ -1,5 +1,5 @@
-#ifndef LOCALPOOL_TPP
-#define LOCALPOOL_TPP
+#ifndef FRAMEWORK_STORAGEMANAGER_LOCALPOOL_TPP_
+#define FRAMEWORK_STORAGEMANAGER_LOCALPOOL_TPP_
 
 template<uint8_t NUMBER_OF_POOLS>
 inline LocalPool<NUMBER_OF_POOLS>::LocalPool(object_id_t setObjectId,
diff --git a/storagemanager/PoolManager.h b/storagemanager/PoolManager.h
index 87670a82..6e353de2 100644
--- a/storagemanager/PoolManager.h
+++ b/storagemanager/PoolManager.h
@@ -1,5 +1,5 @@
-#ifndef POOLMANAGER_H_
-#define POOLMANAGER_H_
+#ifndef FRAMEWORK_STORAGEMANAGER_POOLMANAGER_H_
+#define FRAMEWORK_STORAGEMANAGER_POOLMANAGER_H_
 
 #include <framework/storagemanager/LocalPool.h>
 #include <framework/ipc/MutexHelper.h>
diff --git a/storagemanager/PoolManager.tpp b/storagemanager/PoolManager.tpp
index 7025d795..854e22da 100644
--- a/storagemanager/PoolManager.tpp
+++ b/storagemanager/PoolManager.tpp
@@ -1,5 +1,5 @@
-#ifndef POOLMANAGER_TPP_
-#define POOLMANAGER_TPP_
+#ifndef FRAMEWORK_STORAGEMANAGER_POOLMANAGER_TPP_
+#define FRAMEWORK_STORAGEMANAGER_POOLMANAGER_TPP_
 
 template<uint8_t NUMBER_OF_POOLS>
 inline PoolManager<NUMBER_OF_POOLS>::PoolManager(object_id_t setObjectId,
diff --git a/storagemanager/StorageAccessor.cpp b/storagemanager/StorageAccessor.cpp
index cc292d6c..c0c8126b 100644
--- a/storagemanager/StorageAccessor.cpp
+++ b/storagemanager/StorageAccessor.cpp
@@ -1,4 +1,5 @@
 #include <framework/storagemanager/StorageAccessor.h>
+#include <framework/storagemanager/StorageManagerIF.h>
 
 ConstStorageAccessor::ConstStorageAccessor(store_address_t storeId):
 		storeId(storeId) {}
diff --git a/storagemanager/StorageAccessor.h b/storagemanager/StorageAccessor.h
index 8949642c..57c9369c 100644
--- a/storagemanager/StorageAccessor.h
+++ b/storagemanager/StorageAccessor.h
@@ -1,22 +1,21 @@
+#ifndef FRAMEWORK_STORAGEMANAGER_STORAGEACCESSOR_H_
+#define FRAMEWORK_STORAGEMANAGER_STORAGEACCESSOR_H_
+
+#include <framework/ipc/MutexHelper.h>
+#include <framework/storagemanager/storeAddress.h>
+
+class StorageManagerIF;
+
 /**
  * @brief 	Helper classes to facilitate safe access to storages which is also
  * 			conforming to RAII principles
- * @details	These helper can be used together with the
- * 			StorageManager classes to manage access to a storage.
- * 			It can take care of thread-safety while also providing
- * 			mechanisms to automatically clear storage data.
- */
-#ifndef TEST_PROTOTYPES_STORAGEACCESSOR_H_
-#define TEST_PROTOTYPES_STORAGEACCESSOR_H_
-
-#include <framework/ipc/MutexHelper.h>
-#include <framework/storagemanager/StorageManagerIF.h>
-#include <memory>
-
-/**
- * @brief   Accessor class which can be returned by pool managers
- *          or passed and set by pool managers to have safe access to the pool
- *          resources.
+ * @details
+ * Accessor class which can be returned by pool manager or passed and set by
+ * pool managers to have safe access to the pool resources.
+ *
+ * These helper can be used together with the StorageManager classes to manage
+ * access to a storage. It can take care of thread-safety while also providing
+ * mechanisms to automatically clear storage data.
  */
 class ConstStorageAccessor {
 	//! StorageManager classes have exclusive access to private variables.
diff --git a/storagemanager/StorageManagerIF.h b/storagemanager/StorageManagerIF.h
index 559c4b9a..af5aeb73 100644
--- a/storagemanager/StorageManagerIF.h
+++ b/storagemanager/StorageManagerIF.h
@@ -3,64 +3,14 @@
 
 #include <framework/events/Event.h>
 #include <framework/returnvalues/HasReturnvaluesIF.h>
-#include <cstddef>
+#include <framework/storagemanager/StorageAccessor.h>
+#include <framework/storagemanager/storeAddress.h>
 #include <utility>
-
-class StorageAccessor;
-class ConstStorageAccessor;
+#include <cstddef>
 
 using AccessorPair = std::pair<ReturnValue_t, StorageAccessor>;
 using ConstAccessorPair = std::pair<ReturnValue_t, ConstStorageAccessor>;
 
-/**
- * This union defines the type that identifies where a data packet is
- * stored in the store. It comprises of a raw part to read it as raw value and
- * a structured part to use it in pool-like stores.
- */
-union store_address_t {
-	/**
-	 * Default Constructor, initializing to INVALID_ADDRESS
-	 */
-	store_address_t():raw(0xFFFFFFFF){}
-	/**
-	 * Constructor to create an address object using the raw address
-	 *
-	 * @param rawAddress
-	 */
-	store_address_t(uint32_t rawAddress):raw(rawAddress){}
-
-	/**
-	 * Constructor to create an address object using pool
-	 * and packet indices
-	 *
-	 * @param poolIndex
-	 * @param packetIndex
-	 */
-	store_address_t(uint16_t poolIndex, uint16_t packetIndex):
-		pool_index(poolIndex),packet_index(packetIndex){}
-	/**
-	 * A structure with two elements to access the store address pool-like.
-	 */
-	struct {
-		/**
-		 * The index in which pool the packet lies.
-		 */
-		uint16_t pool_index;
-		/**
-		 * The position in the chosen pool.
-		 */
-		uint16_t packet_index;
-	};
-	/**
-	 * Alternative access to the raw value.
-	 */
-	uint32_t raw;
-
-	bool operator==(const store_address_t& other) const {
-		return raw == other.raw;
-	}
-};
-
 /**
  * @brief	This class provides an interface for intermediate data storage.
  * @details	The Storage manager classes shall be used to store larger chunks of
diff --git a/storagemanager/storeAddress.h b/storagemanager/storeAddress.h
new file mode 100644
index 00000000..5dd785a3
--- /dev/null
+++ b/storagemanager/storeAddress.h
@@ -0,0 +1,54 @@
+#ifndef FRAMEWORK_STORAGEMANAGER_STOREADDRESS_H_
+#define FRAMEWORK_STORAGEMANAGER_STOREADDRESS_H_
+#include <cstdint>
+
+/**
+ * This union defines the type that identifies where a data packet is
+ * stored in the store. It comprises of a raw part to read it as raw value and
+ * a structured part to use it in pool-like stores.
+ */
+union store_address_t {
+	/**
+	 * Default Constructor, initializing to INVALID_ADDRESS
+	 */
+	store_address_t():raw(0xFFFFFFFF){}
+	/**
+	 * Constructor to create an address object using the raw address
+	 *
+	 * @param rawAddress
+	 */
+	store_address_t(uint32_t rawAddress):raw(rawAddress){}
+
+	/**
+	 * Constructor to create an address object using pool
+	 * and packet indices
+	 *
+	 * @param poolIndex
+	 * @param packetIndex
+	 */
+	store_address_t(uint16_t poolIndex, uint16_t packetIndex):
+		pool_index(poolIndex),packet_index(packetIndex){}
+	/**
+	 * A structure with two elements to access the store address pool-like.
+	 */
+	struct {
+		/**
+		 * The index in which pool the packet lies.
+		 */
+		uint16_t pool_index;
+		/**
+		 * The position in the chosen pool.
+		 */
+		uint16_t packet_index;
+	};
+	/**
+	 * Alternative access to the raw value.
+	 */
+	uint32_t raw;
+
+	bool operator==(const store_address_t& other) const {
+		return raw == other.raw;
+	}
+};
+
+#endif /* FRAMEWORK_STORAGEMANAGER_STOREADDRESS_H_ */
diff --git a/tasks/SemaphoreFactory.h b/tasks/SemaphoreFactory.h
new file mode 100644
index 00000000..fa95399a
--- /dev/null
+++ b/tasks/SemaphoreFactory.h
@@ -0,0 +1,45 @@
+#ifndef FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_
+#define FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_
+#include <framework/tasks/SemaphoreIF.h>
+
+/**
+ * Creates Semaphore.
+ * This class is a "singleton" interface, i.e. it provides an
+ * interface, but also is the base class for a singleton.
+ */
+class SemaphoreFactory {
+public:
+	virtual ~SemaphoreFactory();
+	/**
+	 * Returns the single instance of SemaphoreFactory.
+	 * The implementation of #instance is found in its subclasses.
+	 * Thus, we choose link-time variability of the  instance.
+	 */
+	static SemaphoreFactory* instance();
+
+	/**
+	 * Create a binary semaphore.
+	 * Creator function for a binary semaphore which may only be acquired once
+	 * @return Pointer to newly created semaphore class instance.
+	 */
+	SemaphoreIF* createBinarySemaphore();
+	/**
+	 * Create a counting semaphore.
+	 * Creator functons for a counting semaphore which may be acquired multiple
+	 * times.
+	 * @param count Semaphore can be taken count times.
+	 * @param initCount Initial count value.
+	 * @return
+	 */
+	SemaphoreIF* createCountingSemaphore(uint8_t count, uint8_t initCount);
+	void deleteMutex(SemaphoreIF* mutex);
+
+private:
+	/**
+	 * External instantiation is not allowed.
+	 */
+	SemaphoreFactory();
+	static SemaphoreFactory* factoryInstance;
+};
+
+#endif /* FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_ */
diff --git a/tasks/SemaphoreIF.h b/tasks/SemaphoreIF.h
new file mode 100644
index 00000000..7dde27f4
--- /dev/null
+++ b/tasks/SemaphoreIF.h
@@ -0,0 +1,58 @@
+#ifndef FRAMEWORK_TASKS_SEMAPHOREIF_H_
+#define FRAMEWORK_TASKS_SEMAPHOREIF_H_
+#include <framework/returnvalues/FwClassIds.h>
+#include <framework/returnvalues/HasReturnvaluesIF.h>
+#include <cstdint>
+
+/**
+ * @brief 	Generic interface for semaphores, which can be used to achieve
+ * 			task synchronization. This is a generic interface which can be
+ * 			used for both binary semaphores and counting semaphores.
+ * @details
+ * A semaphore is a synchronization primitive.
+ * See: https://en.wikipedia.org/wiki/Semaphore_(programming)
+ * A semaphore can be used to achieve task synchonization and track the
+ * availability of resources.
+ *
+ * If mutual exlcusion of a resource is desired, a mutex should be used,
+ * which is a special form of a semaphore and has an own interface.
+ */
+class SemaphoreIF {
+public:
+	virtual~ SemaphoreIF() {};
+	//!< Needs to be defined in implementation.
+	static const uint32_t NO_TIMEOUT;
+	static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF;
+	//! Semaphore timeout
+	static constexpr ReturnValue_t SEMAPHORE_TIMEOUT = MAKE_RETURN_CODE(1);
+	//! The current semaphore can not be given, because it is not owned
+	static constexpr ReturnValue_t SEMAPHORE_NOT_OWNED = MAKE_RETURN_CODE(2);
+	static constexpr ReturnValue_t SEMAPHORE_NULLPOINTER = MAKE_RETURN_CODE(3);
+
+	/**
+	 * Generic call to acquire a semaphore.
+	 * If there are no more semaphores to be taken (for a counting semaphore,
+	 * a semaphore may be taken more than once), the taks will block
+	 * for a maximum of timeoutMs while trying to acquire the semaphore.
+	 * This can be used to achieve task synchrnization.
+	 * @param timeoutMs
+	 * @return - c RETURN_OK for successfull acquisition
+	 */
+	virtual ReturnValue_t acquire(uint32_t timeoutMs) = 0;
+
+	/**
+	 * Corrensponding call to release a semaphore.
+	 * @return -@c RETURN_OK for successfull release
+	 */
+	virtual ReturnValue_t release() = 0;
+
+	/**
+	 * If the semaphore is a counting semaphore then the semaphores current
+	 * count value is returned. If the semaphore is a binary semaphore then 1
+	 * is returned if the semaphore is available, and 0 is returned if the
+	 * semaphore is not available.
+	 */
+	virtual uint8_t getSemaphoreCounter() = 0;
+};
+
+#endif /* FRAMEWORK_TASKS_SEMAPHOREIF_H_ */
diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp
index 0972ffff..733c36df 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/TcPacketBase.cpp
@@ -13,28 +13,28 @@ TcPacketBase::~TcPacketBase() {
 }
 
 uint8_t TcPacketBase::getService() {
-	return tcData->data_field.service_type;
+	return tcData->dataField.service_type;
 }
 
 uint8_t TcPacketBase::getSubService() {
-	return tcData->data_field.service_subtype;
+	return tcData->dataField.service_subtype;
 }
 
 uint8_t TcPacketBase::getAcknowledgeFlags() {
-	return tcData->data_field.version_type_ack & 0b00001111;
+	return tcData->dataField.version_type_ack & 0b00001111;
 }
 
 const uint8_t* TcPacketBase::getApplicationData() const {
-	return &tcData->data;
+	return &tcData->appData;
 }
 
 uint16_t TcPacketBase::getApplicationDataSize() {
-	return getPacketDataLength() - sizeof(tcData->data_field) - CRC_SIZE + 1;
+	return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
 }
 
 uint16_t TcPacketBase::getErrorControl() {
 	uint16_t size = getApplicationDataSize() + CRC_SIZE;
-	uint8_t* p_to_buffer = &tcData->data;
+	uint8_t* p_to_buffer = &tcData->appData;
 	return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
 }
 
@@ -42,8 +42,8 @@ void TcPacketBase::setErrorControl() {
 	uint32_t full_size = getFullSize();
 	uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
 	uint32_t size = getApplicationDataSize();
-	(&tcData->data)[size] = (crc & 0XFF00) >> 8;	// CRCH
-	(&tcData->data)[size + 1] = (crc) & 0X00FF; 		// CRCL
+	(&tcData->appData)[size] = (crc & 0XFF00) >> 8;	// CRCH
+	(&tcData->appData)[size + 1] = (crc) & 0X00FF; 		// CRCL
 }
 
 void TcPacketBase::setData(const uint8_t* pData) {
@@ -51,19 +51,18 @@ void TcPacketBase::setData(const uint8_t* pData) {
 	tcData = (TcPacketPointer*) pData;
 }
 
-void TcPacketBase::setApplicationData(const uint8_t * pData, uint16_t dataLen) {
-	setData(pData);
-	// packet data length is actual size of data field minus 1
+void TcPacketBase::setAppData(uint8_t * appData, uint16_t dataLen) {
+	memcpy(&tcData->appData, appData, dataLen);
 	SpacePacketBase::setPacketDataLength(dataLen +
-			sizeof(PUSTcDataFieldHeader) + TcPacketBase::CRC_SIZE - 1);
+				sizeof(PUSTcDataFieldHeader) + TcPacketBase::CRC_SIZE - 1);
 }
 
 uint8_t TcPacketBase::getSecondaryHeaderFlag() {
-	return (tcData->data_field.version_type_ack & 0b10000000) >> 7;
+	return (tcData->dataField.version_type_ack & 0b10000000) >> 7;
 }
 
 uint8_t TcPacketBase::getPusVersionNumber() {
-	return (tcData->data_field.version_type_ack & 0b01110000) >> 4;
+	return (tcData->dataField.version_type_ack & 0b01110000) >> 4;
 }
 
 void TcPacketBase::print() {
@@ -78,14 +77,14 @@ void TcPacketBase::print() {
 void TcPacketBase::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
 		uint8_t ack, uint8_t service, uint8_t subservice) {
 	initSpacePacketHeader(true, true, apid, sequenceCount);
-	memset(&tcData->data_field, 0, sizeof(tcData->data_field));
+	memset(&tcData->dataField, 0, sizeof(tcData->dataField));
 	setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
 	//Data Field Header:
 	//Set CCSDS_secondary_header_flag to 0, version number to 001 and ack to 0000
-	tcData->data_field.version_type_ack = 0b00010000;
-	tcData->data_field.version_type_ack |= (ack & 0x0F);
-	tcData->data_field.service_type = service;
-	tcData->data_field.service_subtype = subservice;
+	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) {
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h
index 136aaa0b..73076176 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/TcPacketBase.h
@@ -24,8 +24,8 @@ struct PUSTcDataFieldHeader {
  */
 struct TcPacketPointer {
 	CCSDSPrimaryHeader primary;
-	PUSTcDataFieldHeader data_field;
-	uint8_t data;
+	PUSTcDataFieldHeader dataField;
+	uint8_t appData;
 };
 
 /**
@@ -168,6 +168,14 @@ public:
 	 * current content of the packet.
 	 */
 	void setErrorControl();
+
+	/**
+	 * Copies the supplied data to the internal TC application data field.
+	 * @param pData
+	 * @param dataLen
+	 */
+	void setAppData(uint8_t * appData, uint16_t dataLen);
+
 	/**
 	 * With this method, the packet data pointer can be redirected to another
 	 * location.
@@ -178,12 +186,7 @@ public:
 	 * @param p_data	A pointer to another PUS Telecommand Packet.
 	 */
 	void setData( const uint8_t* pData );
-	/**
-	 * Set application data and corresponding length field.
-	 * @param pData
-	 * @param dataLen
-	 */
-	void setApplicationData(const uint8_t * pData, uint16_t dataLen);
+
 	/**
 	 * This is a debugging helper method that prints the whole packet content
 	 * to the screen.
diff --git a/tmtcpacket/pus/TcPacketStored.cpp b/tmtcpacket/pus/TcPacketStored.cpp
index 0187b08f..64aa74dc 100644
--- a/tmtcpacket/pus/TcPacketStored.cpp
+++ b/tmtcpacket/pus/TcPacketStored.cpp
@@ -1,35 +1,47 @@
 #include <framework/objectmanager/ObjectManagerIF.h>
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
 #include <framework/tmtcpacket/pus/TcPacketStored.h>
-#include <string.h>
+
+#include <cstring>
 
 TcPacketStored::TcPacketStored(store_address_t setAddress) :
-		TcPacketBase(NULL), storeAddress(setAddress) {
+		TcPacketBase(nullptr), storeAddress(setAddress) {
 	this->setStoreAddress(this->storeAddress);
 }
 
-TcPacketStored::TcPacketStored(uint16_t apid, uint8_t ack, uint8_t service,
-		uint8_t subservice, uint8_t sequence_count, const uint8_t* data,
-		uint32_t size) :
-		TcPacketBase(NULL) {
+TcPacketStored::TcPacketStored(uint8_t service, uint8_t subservice,
+		uint16_t apid, uint8_t sequence_count, const uint8_t* data,
+		size_t size, uint8_t ack ) :
+		TcPacketBase(nullptr) {
 	this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
 	if (!this->checkAndSetStore()) {
 		return;
 	}
-	uint8_t* p_data = NULL;
+	uint8_t* p_data = nullptr;
 	ReturnValue_t returnValue = this->store->getFreeElement(&this->storeAddress,
 			(TC_PACKET_MIN_SIZE + size), &p_data);
 	if (returnValue != this->store->RETURN_OK) {
+		sif::warning << "TcPacketStored: Could not get free element from store!"
+				<< std::endl;
 		return;
 	}
 	this->setData(p_data);
 	initializeTcPacket(apid, sequence_count, ack, service, subservice);
-	memcpy(&tcData->data, data, size);
+	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) {
+		sif::warning << "TcPacketStored: Could not get data!" << std::endl;
+	}
+	return result;
+}
+
 TcPacketStored::TcPacketStored() :
 		TcPacketBase(NULL) {
 	this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
diff --git a/tmtcpacket/pus/TcPacketStored.h b/tmtcpacket/pus/TcPacketStored.h
index c57f0e0f..de3eb85f 100644
--- a/tmtcpacket/pus/TcPacketStored.h
+++ b/tmtcpacket/pus/TcPacketStored.h
@@ -64,7 +64,9 @@ public:
 	 * @param data		The data to be copied to the Application Data Field.
 	 * @param size		The amount of data to be copied.
 	 */
-	TcPacketStored( uint16_t apid, uint8_t ack, uint8_t service, uint8_t subservice, uint8_t sequence_count = 0, const uint8_t* data = NULL, uint32_t size = 0 );
+	TcPacketStored( uint8_t service, uint8_t subservice, uint16_t apid,
+			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.
@@ -72,6 +74,9 @@ public:
 	 * @param Size size of the packet.
 	 */
 	TcPacketStored( const uint8_t* data, uint32_t size);
+
+	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
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp
index a7bda5af..a2c27ce1 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/TmPacketBase.cpp
@@ -102,6 +102,11 @@ void TmPacketBase::initializeTmPacket(uint16_t apid, uint8_t service, uint8_t su
 	}
 }
 
+void TmPacketBase::setSourceData(uint8_t* sourceData, size_t sourceSize) {
+	memcpy(getSourceData(), sourceData, sourceSize);
+	setSourceDataSize(sourceSize);
+}
+
 void TmPacketBase::setSourceDataSize(uint16_t size) {
 	setPacketDataLength(size + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1);
 }
diff --git a/tmtcpacket/pus/TmPacketBase.h b/tmtcpacket/pus/TmPacketBase.h
index d03beba5..6f3c4b59 100644
--- a/tmtcpacket/pus/TmPacketBase.h
+++ b/tmtcpacket/pus/TmPacketBase.h
@@ -125,6 +125,13 @@ public:
 	 * current content of the packet.
 	 */
 	void setErrorControl();
+	/**
+	 * This sets the source data. It copies the provided data to
+	 * the internal TmPacketPointer source data location.
+	 * @param sourceData
+	 * @param sourceSize
+	 */
+	void setSourceData(uint8_t* sourceData, size_t sourceSize);
 	/**
 	 * With this method, the packet data pointer can be redirected to another
 	 * location.