diff --git a/osal/FreeRTOS/FixedTimeslotTask.cpp b/osal/FreeRTOS/FixedTimeslotTask.cpp
index 57c927f9..3bd2aafb 100644
--- a/osal/FreeRTOS/FixedTimeslotTask.cpp
+++ b/osal/FreeRTOS/FixedTimeslotTask.cpp
@@ -157,3 +157,6 @@ ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) {
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
+TaskHandle_t FixedTimeslotTask::getTaskHandle() const {
+    return handle;
+}
diff --git a/osal/FreeRTOS/FixedTimeslotTask.h b/osal/FreeRTOS/FixedTimeslotTask.h
index 27d08190..985b4f90 100644
--- a/osal/FreeRTOS/FixedTimeslotTask.h
+++ b/osal/FreeRTOS/FixedTimeslotTask.h
@@ -57,6 +57,8 @@ public:
 
 	ReturnValue_t sleepFor(uint32_t ms) override;
 
+	TaskHandle_t getTaskHandle() const;
+
 protected:
 	bool started;
 	TaskHandle_t handle;
diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp
index 2ebfebf4..6d013a5b 100644
--- a/osal/FreeRTOS/PeriodicTask.cpp
+++ b/osal/FreeRTOS/PeriodicTask.cpp
@@ -126,6 +126,10 @@ void PeriodicTask::checkMissedDeadline(const TickType_t xLastWakeTime,
     }
 }
 
+TaskHandle_t PeriodicTask::getTaskHandle() const {
+    return handle;
+}
+
 void PeriodicTask::handleMissedDeadline() {
 #ifdef DEBUG
     sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) <<
diff --git a/osal/FreeRTOS/PeriodicTask.h b/osal/FreeRTOS/PeriodicTask.h
index 09aa6bc7..679686f6 100644
--- a/osal/FreeRTOS/PeriodicTask.h
+++ b/osal/FreeRTOS/PeriodicTask.h
@@ -69,6 +69,8 @@ public:
 	uint32_t getPeriodMs() const override;
 
 	ReturnValue_t sleepFor(uint32_t ms) override;
+
+	TaskHandle_t getTaskHandle() const;
 protected:
 	bool started;
 	TaskHandle_t handle;
diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp
index 7871ab2e..9a1e4447 100644
--- a/osal/FreeRTOS/TaskManagement.cpp
+++ b/osal/FreeRTOS/TaskManagement.cpp
@@ -18,6 +18,7 @@ TaskHandle_t TaskManagement::getCurrentTaskHandle() {
 	return xTaskGetCurrentTaskHandle();
 }
 
-configSTACK_DEPTH_TYPE TaskManagement::getTaskStackHighWatermark() {
-	return uxTaskGetStackHighWaterMark(TaskManagement::getCurrentTaskHandle());
+configSTACK_DEPTH_TYPE TaskManagement::getTaskStackHighWatermark(
+        TaskHandle_t task) {
+	return uxTaskGetStackHighWaterMark(task);
 }
diff --git a/osal/FreeRTOS/TaskManagement.h b/osal/FreeRTOS/TaskManagement.h
index 39c24377..cbde510c 100644
--- a/osal/FreeRTOS/TaskManagement.h
+++ b/osal/FreeRTOS/TaskManagement.h
@@ -57,7 +57,8 @@ public:
 	 * @return Smallest value of stack remaining since the task was started in
 	 * 		   words.
 	 */
-	static configSTACK_DEPTH_TYPE getTaskStackHighWatermark();
+	static configSTACK_DEPTH_TYPE getTaskStackHighWatermark(
+	        TaskHandle_t task = nullptr);
 };
 
 #endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */
diff --git a/pus/Service2DeviceAccess.cpp b/pus/Service2DeviceAccess.cpp
index 1d5f21eb..84d6693d 100644
--- a/pus/Service2DeviceAccess.cpp
+++ b/pus/Service2DeviceAccess.cpp
@@ -60,13 +60,11 @@ ReturnValue_t Service2DeviceAccess::prepareCommand(CommandMessage* message,
 		uint32_t* state, object_id_t objectId) {
 	switch(static_cast<Subservice>(subservice)){
 	case Subservice::RAW_COMMANDING: {
-		return prepareRawCommand(dynamic_cast<CommandMessage*>(message),
-				tcData, tcDataLen);
+		return prepareRawCommand(message, tcData, tcDataLen);
 	}
 	break;
 	case Subservice::TOGGLE_WIRETAPPING: {
-		return prepareWiretappingCommand(dynamic_cast<CommandMessage*>(message),
-				tcData, tcDataLen);
+		return prepareWiretappingCommand(message, tcData, tcDataLen);
 	}
 	break;
 	default:
diff --git a/pus/Service8FunctionManagement.cpp b/pus/Service8FunctionManagement.cpp
index 812398b9..50102b04 100644
--- a/pus/Service8FunctionManagement.cpp
+++ b/pus/Service8FunctionManagement.cpp
@@ -59,8 +59,15 @@ ReturnValue_t Service8FunctionManagement::prepareCommand(
 
 ReturnValue_t Service8FunctionManagement::prepareDirectCommand(
 		CommandMessage *message, const uint8_t *tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(object_id_t) + sizeof(ActionId_t)) {
+        sif::debug << "Service8FunctionManagement::prepareDirectCommand:"
+                << " TC size smaller thant minimum size of direct command."
+                << std::endl;
+        return CommandingServiceBase::INVALID_TC;
+    }
+
 	// Create direct command instance by extracting data from Telecommand
-	DirectCommand command(tcData,tcDataLen);
+	DirectCommand command(tcData, tcDataLen);
 
 	// store additional parameters into the IPC Store
 	store_address_t parameterAddress;
diff --git a/pus/servicepackets/Service8Packets.h b/pus/servicepackets/Service8Packets.h
index 5ef91280..8ea0d108 100644
--- a/pus/servicepackets/Service8Packets.h
+++ b/pus/servicepackets/Service8Packets.h
@@ -1,22 +1,3 @@
-/**
- * \file Service8Packets.h
- *
- * \brief Structure of a Direct Command.
- * Normal reply (subservice 130) consists of
- *   1. Target object ID
- *   2. Action ID (taget device has specified functions with action IDs)
- *   3. Return Code
- *   4. Optional step number for step replies
- *
- * Data reply (subservice 132) consists of
- *   1. Target Object ID
- *   2. Action ID
- *   3. Data
- *
- *  \date 01.07.2019
- *  \author R. Mueller
- */
-
 #ifndef FRAMEWORK_PUS_SERVICEPACKETS_SERVICE8PACKETS_H_
 #define FRAMEWORK_PUS_SERVICEPACKETS_SERVICE8PACKETS_H_
 
@@ -29,23 +10,21 @@
 
 
 /**
- * \brief Subservice 128
- * \ingroup spacepackets
+ * @brief Subservice 128
+ * @ingroup spacepackets
  */
 class DirectCommand: public SerialLinkedListAdapter<SerializeIF> { //!< [EXPORT] : [SUBSERVICE] 128
 public:
-	//typedef uint16_t typeOfMaxData;
-	//static const typeOfMaxData MAX_DATA = 256;
-	DirectCommand(const uint8_t* dataBuffer_, uint32_t size_) {
-		size_t size = sizeof(objectId);
-		SerializeAdapter::deSerialize(&objectId,&dataBuffer_,&size,
+
+	DirectCommand(const uint8_t* tcData, size_t size) {
+		SerializeAdapter::deSerialize(&objectId, &tcData, &size,
 		        SerializeIF::Endianness::BIG);
-		size = sizeof(actionId);
-		SerializeAdapter::deSerialize(&actionId,&dataBuffer_,&size,
+		SerializeAdapter::deSerialize(&actionId, &tcData, &size,
 		        SerializeIF::Endianness::BIG);
-		parameterBuffer = dataBuffer_;
-		parametersSize = size_ - sizeof(objectId) - sizeof(actionId);
+		parameterBuffer = tcData;
+		parametersSize = size;
 	}
+
 	ActionId_t getActionId() const {
 		return actionId;
 	}
@@ -73,8 +52,12 @@ private:
 
 
 /**
- * \brief Subservice 130
- * \ingroup spacepackets
+ * @brief Subservice 130
+ * Data reply (subservice 130) consists of
+ *   1. Target Object ID
+ *   2. Action ID
+ *   3. Data
+ * @ingroup spacepackets
  */
 class DataReply: public SerialLinkedListAdapter<SerializeIF> { //!< [EXPORT] : [SUBSERVICE] 130
 public:
@@ -100,8 +83,10 @@ private:
 
 
 /**
- * \brief Subservice 132
- * \ingroup spacepackets
+ * @brief Subservice 132
+ * @details
+ * Not used yet. Telecommand Verification takes care of this.
+ * @ingroup spacepackets
  */
 class DirectReply: public SerialLinkedListAdapter<SerializeIF> { //!< [EXPORT] : [SUBSERVICE] 132
 public:
@@ -124,7 +109,7 @@ private:
 			returnCode.setNext(&step);
 		}
 	}
-	bool isDataReply; //!< [EXPORT] : [IGNORE]
+
 	bool isStep; //!< [EXPORT] : [IGNORE]
 	SerializeElement<object_id_t> objectId; //!< [EXPORT] : [IGNORE]
 	SerializeElement<ActionId_t> actionId; //!< [EXPORT] : [IGNORE]
diff --git a/tmtcservices/PusParser.cpp b/tmtcservices/PusParser.cpp
index 4198e36e..3dd34a84 100644
--- a/tmtcservices/PusParser.cpp
+++ b/tmtcservices/PusParser.cpp
@@ -7,9 +7,8 @@ PusParser::PusParser(uint16_t maxExpectedPusPackets,
 
 ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame,
 		size_t frameSize) {
-	if(frame == nullptr) {
-		sif::error << "PusParser::parsePusPackets: Frame pointers in invalid!"
-				<< std::endl;
+	if(frame == nullptr or frameSize < 5) {
+		sif::error << "PusParser::parsePusPackets: Frame invalid!" << std::endl;
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 
@@ -25,15 +24,13 @@ ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame,
 	}
 
 	size_t packetSize = lengthField + 7;
+	// sif::debug << frameSize << std::endl;
 	// Size of a pus packet is the value in the packet length field plus 7.
-	if(packetSize > frameSize)
-	{
-		if(storeSplitPackets)
-		{
+	if(packetSize > frameSize) {
+		if(storeSplitPackets) {
 			indexSizePairFIFO.insert(indexSizePair(0, frameSize));
 		}
-		else
-		{
+		else {
 			sif::debug << "TcSerialPollingTask::readNextPacket: Next packet "
 					"larger than remaining frame," << std::endl;
 			sif::debug << "Throwing away packet. Detected packet size: "
@@ -41,8 +38,7 @@ ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame,
 		}
 		return SPLIT_PACKET;
 	}
-	else
-	{
+	else {
 		indexSizePairFIFO.insert(indexSizePair(0, packetSize));
 		if(packetSize == frameSize) {
 			return HasReturnvaluesIF::RETURN_OK;
@@ -77,6 +73,11 @@ PusParser::indexSizePair PusParser::getNextFifoPair() {
 ReturnValue_t PusParser::readNextPacket(const uint8_t *frame,
 		size_t frameSize, size_t& currentIndex) {
 	// sif::debug << startIndex << std::endl;
+    if(currentIndex + 5 > frameSize) {
+        currentIndex = frameSize;
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+
 	uint16_t lengthField = frame[currentIndex + 4] << 8 |
 			frame[currentIndex + 5];
 	if(lengthField == 0) {
@@ -88,12 +89,10 @@ ReturnValue_t PusParser::readNextPacket(const uint8_t *frame,
 	size_t remainingSize = frameSize - currentIndex;
 	if(nextPacketSize > remainingSize)
 	{
-		if(storeSplitPackets)
-		{
+		if(storeSplitPackets) {
 			indexSizePairFIFO.insert(indexSizePair(currentIndex, remainingSize));
 		}
-		else
-		{
+		else {
 			sif::debug << "TcSerialPollingTask::readNextPacket: Next packet "
 					"larger than remaining frame," << std::endl;
 			sif::debug << "Throwing away packet. Detected packet size: "
diff --git a/tmtcservices/PusParser.h b/tmtcservices/PusParser.h
index 88570bb2..d59673f6 100644
--- a/tmtcservices/PusParser.h
+++ b/tmtcservices/PusParser.h
@@ -63,11 +63,12 @@ public:
 	 * @return
 	 */
 	indexSizePair getNextFifoPair();
-private:
 
-	//! A FIFO is used to store information about multiple PUS packets
-	//! inside the receive buffer. The maximum number of entries is defined
-	//! by the first constructor argument.
+private:
+	/** A FIFO is used to store information about multiple PUS packets
+	 * inside the receive buffer. The maximum number of entries is defined
+	 * by the first constructor argument.
+	 */
 	DynamicFIFO<indexSizePair> indexSizePairFIFO;
 
 	bool storeSplitPackets = false;