diff --git a/bsp_linux/InitMission.cpp b/bsp_linux/InitMission.cpp
index 94f58991..4be8be72 100644
--- a/bsp_linux/InitMission.cpp
+++ b/bsp_linux/InitMission.cpp
@@ -130,8 +130,16 @@ void InitMission::initTasks(){
 		sif::error << "Object add component failed" << std::endl;
 	}
 
+//	PeriodicTaskIF* P60DockTask = TaskFactory::instance()->
+//			createPeriodicTask("P60Dock Task", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE*4,
+//					1.6, nullptr);
+//	result = P60DockTask->addComponent(objects::P60DOCK_HANDLER);
+//	if(result!=HasReturnvaluesIF::RETURN_OK){
+//		sif::error << "Object add component failed" << std::endl;
+//	}
+
 	FixedTimeslotTaskIF* GomSpacePstTask = TaskFactory::instance()->
-			createFixedTimeslotTask("GS_PST_TASK", 10,
+			createFixedTimeslotTask("GS_PST_TASK", 50,
 					PeriodicTaskIF::MINIMUM_STACK_SIZE*4, 1.0, nullptr);
 	result = pst::gomspacePstInit(GomSpacePstTask);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -173,6 +181,8 @@ void InitMission::initTasks(){
 	PusMedPrio->startTask();
 	PusLowPrio->startTask();
 
+//	P60DockTask->startTask();
+
 #if ADD_TEST_CODE == 1
 //	TestTimeslotTask->startTask();
 	P60DockTestTask->startTask();
diff --git a/bsp_linux/comIF/P60DockComIF.cpp b/bsp_linux/comIF/P60DockComIF.cpp
index 7445c03b..07af81e6 100644
--- a/bsp_linux/comIF/P60DockComIF.cpp
+++ b/bsp_linux/comIF/P60DockComIF.cpp
@@ -20,14 +20,16 @@ ReturnValue_t P60DockComIF::initializeInterface(CookieIF *cookie)  {
 	}
 
 	P60DockCookie* p60DockCookie = dynamic_cast<P60DockCookie*>(cookie);
-	uint8_t cspAddress = p60DockCookie->getCspAddress();
+	if(p60DockCookie == nullptr) {
+		return NULLPOINTER;
+	}
 	char* canInterface = p60DockCookie->getCanIf();
 	int bitrate = p60DockCookie->getBitrate();
-
+	/* Define the memory to allocate for the CSP stack */
 	int buf_count = 10;
 	int buf_size = 300;
 	/* Init CSP and CSP buffer system */
-	if (csp_init(cspAddress) != CSP_ERR_NONE
+	if (csp_init(cspClientAddress) != CSP_ERR_NONE
 			|| csp_buffer_init(buf_count, buf_size) != CSP_ERR_NONE) {
 		sif::error << "Failed to init CSP\r\n" << std::endl;
 		return HasReturnvaluesIF::RETURN_FAILED;
@@ -49,22 +51,42 @@ ReturnValue_t P60DockComIF::initializeInterface(CookieIF *cookie)  {
 	}
 
 	/* Start the route task */
-	unsigned int task_stack_size = 512;
+	unsigned int task_stack_size = 500;
 	unsigned int priority = 0;
 	result = csp_route_start_task(task_stack_size, priority);
 	if(result != CSP_ERR_NONE){
 		sif::error << "Failed to start csp route task" << std::endl;
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
+
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t P60DockComIF::sendMessage(CookieIF *cookie,
 		const uint8_t * sendData, size_t sendLen) {
+	if(cookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
 	P60DockCookie* p60DockCookie = dynamic_cast<P60DockCookie*> (cookie);
+	if(p60DockCookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
 	MessageType_t messageType = p60DockCookie->getMessageType();
 
 	switch(messageType){
+	case(P60DockCookie::PING):{
+		uint32_t timeout = 1000; // ms
+		unsigned int pingSize = 100; // 100 bytes
+		uint8_t p60DockAddress = p60DockCookie->getCspAddress();
+		uint32_t replyTime = csp_ping(p60DockAddress, timeout, pingSize,
+				CSP_O_NONE);
+		sif::info << "Ping address: " << p60DockAddress << ", reply after "
+				<< replyTime << " ms" << std::endl;
+		/* Store reply time in reply buffer * */
+		memcpy(replyBuffer, &replyTime, sizeof(replyTime));
+		break;
+	}
 	case(P60DockCookie::REBOOT):{
 		csp_reboot(p60DockCookie->getCspAddress());
 		break;
@@ -98,19 +120,19 @@ ReturnValue_t P60DockComIF::readReceivedMessage(CookieIF *cookie,
 
 	switch(messageType){
 	case(P60DockCookie::READ_MODULE_CONFIG):{
-		uint32_t timeout = 1000;
+		uint32_t timeout = 1000; // ms
 		uint8_t p60dockAddress = p60DockCookie->getCspAddress();
-		gs_param_table_instance_t moduleConfig;
-		moduleConfig.rows = (gs_param_table_row_t*)p60dock_config;
-		moduleConfig.id = moduleCfgTableNum;
-		moduleConfig.row_count = p60dock_config_count;
-		moduleConfig.memory_size = p60dock_config_size;
-		moduleConfig.memory = replyBuffer;
+		gs_param_table_instance_t table;
+		table.rows = (gs_param_table_row_t*)p60dock_config;
+		table.id = moduleCfgTableNum;
+		table.row_count = p60dock_config_count;
+		table.memory_size = P60DOCK_PARAM_SIZE;
+		table.memory = replyBuffer;
 		/* Read complete module configuration table from P60 Dock and store data
 		 * in buffer */
-		int result = gs_rparam_get_full_table(&moduleConfig, p60dockAddress,
-				moduleConfig.id, GS_RPARAM_MAGIC_CHECKSUM, timeout);
-		*size = moduleCfgTableSize;
+		int result = gs_rparam_get_full_table(&table, p60dockAddress,
+				table.id, GS_RPARAM_MAGIC_CHECKSUM, timeout);
+		*size = P60DOCK_PARAM_SIZE;
 		if (result != GS_OK) {
 			sif::info
 					<< "Failed retrieving module configuration from P60 dock "
@@ -120,18 +142,18 @@ ReturnValue_t P60DockComIF::readReceivedMessage(CookieIF *cookie,
 		break;
 	}
 	case(P60DockCookie::READ_HK):{
-		uint32_t timeout = 1000;
+		uint32_t timeout = 1000; // ms
 		uint8_t p60dockAddress = p60DockCookie->getCspAddress();
-		tmData.rows = (gs_param_table_row_t*)p60dock_hk;
-		tmData.id = tmTableNum;
-		tmData.row_count = p60dock_hk_count;
-		tmData.memory_size = P60DOCK_HK_SIZE;
-		tmData.memory = replyBuffer;
+		table.rows = (gs_param_table_row_t*)p60dock_hk;
+		table.id = tmTableNum;
+		table.row_count = p60dock_hk_count;
+		table.memory_size = P60DOCK_HK_SIZE;
+		table.memory = replyBuffer;
 		/* Read complete module configuration table from P60 Dock and store data
 		 * in buffer */
-		int result = gs_rparam_get_full_table(&tmData, p60dockAddress,
-				tmData.id, GS_RPARAM_MAGIC_CHECKSUM, timeout);
-		*size = tmTableSize;
+		int result = gs_rparam_get_full_table(&table, p60dockAddress,
+				table.id, GS_RPARAM_MAGIC_CHECKSUM, timeout);
+		*size = P60DOCK_HK_SIZE;
 		if (result != GS_OK) {
 			sif::info
 					<< "Failed retrieving telemetry from P60 dock with error "
diff --git a/bsp_linux/comIF/P60DockComIF.h b/bsp_linux/comIF/P60DockComIF.h
index 3279f64e..3aed2f25 100644
--- a/bsp_linux/comIF/P60DockComIF.h
+++ b/bsp_linux/comIF/P60DockComIF.h
@@ -25,7 +25,7 @@
  */
 class P60DockComIF: public DeviceCommunicationIF, public SystemObject {
 public:
-	static const uint8_t maxReplyLength = 188;
+	static const uint16_t maxReplyLength = 412;
 
 	P60DockComIF(object_id_t objectId);
 	virtual ~P60DockComIF();
@@ -40,19 +40,18 @@ public:
 			uint8_t **readData, size_t *readLen) override;
 
 private:
+	/* This is the CSP address of the OBC. */
+	uint8_t cspClientAddress = 1;
 	/* Interface struct for csp protocol stack */
 	csp_iface_t csp_if;
 	/* Table definitions. According to gomspace software documentation there
 	 * exist four tables each identified by a number*/
-	uint8_t boardConfigTableNum = 0;
 	uint8_t moduleCfgTableNum = 1;
 	uint8_t calibrationParamTableNum = 2;
 	uint8_t tmTableNum = 4;
-	unsigned int moduleConfigTableRows = 32;
-	uint8_t moduleCfgTableSize = 188;
-	uint8_t tmTableSize = 188;
-	uint8_t replyBuffer[188];
-	gs_param_table_instance_t tmData;
+	/* Replies of P60 dock are written to this buffer */
+	uint8_t replyBuffer[P60DockComIF::maxReplyLength];
+	gs_param_table_instance_t table;
 
 };
 
diff --git a/bsp_linux/comIF/cookies/P60DockCookie.cpp b/bsp_linux/comIF/cookies/P60DockCookie.cpp
index b4b83bbe..4f1c87d2 100644
--- a/bsp_linux/comIF/cookies/P60DockCookie.cpp
+++ b/bsp_linux/comIF/cookies/P60DockCookie.cpp
@@ -21,6 +21,10 @@ int P60DockCookie::getBitrate(){
 	return bitrate;
 }
 
+void P60DockCookie::resetMessageType(){
+	nextMessage = MESSAGE_NONE;
+}
+
 void P60DockCookie::setPingMessage(){
 	nextMessage = PING;
 }
diff --git a/bsp_linux/comIF/cookies/P60DockCookie.h b/bsp_linux/comIF/cookies/P60DockCookie.h
index e8a1c42c..8d54104c 100644
--- a/bsp_linux/comIF/cookies/P60DockCookie.h
+++ b/bsp_linux/comIF/cookies/P60DockCookie.h
@@ -29,6 +29,7 @@ public:
 	void setReadModuleCfgMessage();
 	void setReadHkMessage();
 	MessageType_t getMessageType();
+	void resetMessageType();
 
 	/* Message type defines the type of the next data transfer between the
 	 * CSP device and the OBC. */
diff --git a/gomspace/p60-dock_client/include/gs/p60-dock/param/p60dock_hk.h b/gomspace/p60-dock_client/include/gs/p60-dock/param/p60dock_hk.h
index 3db66959..b9c9f17a 100644
--- a/gomspace/p60-dock_client/include/gs/p60-dock/param/p60dock_hk.h
+++ b/gomspace/p60-dock_client/include/gs/p60-dock/param/p60dock_hk.h
@@ -42,11 +42,9 @@
 #define P60DOCK_HK_ANT6_DEPL             0xBA            /* int8_t */
 #define P60DOCK_HK_AR6_DEPL              0xBB            /* int8_t */
 
-/** Define the memory size */
 #define P60DOCK_HK_SIZE                  0xBC
 
 extern const param_table_t p60dock_hk[];
 extern const int p60dock_hk_count;
-extern const int p60dock_config_size;
 
 #endif /* P60DOCK_HK_H_ */
diff --git a/gomspace/p60-dock_client/src/p60dock_client.c b/gomspace/p60-dock_client/src/p60dock_client.c
index 5ca69126..2fa3232b 100644
--- a/gomspace/p60-dock_client/src/p60dock_client.c
+++ b/gomspace/p60-dock_client/src/p60dock_client.c
@@ -64,7 +64,6 @@ const param_table_t p60dock_config[] = {
 };
 
 const int p60dock_config_count = sizeof(p60dock_config) / sizeof(p60dock_config[0]);
-const int p60dock_config_size = sizeof(p60dock_config);
 
 /**
  * Setup info about calibration parameters
diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp
index 24a9c200..fa4ebebb 100644
--- a/mission/devices/P60DockHandler.cpp
+++ b/mission/devices/P60DockHandler.cpp
@@ -40,19 +40,16 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand(
 		DeviceCommandId_t deviceCommand, const uint8_t * commandData,
 		size_t commandDataLen) {
 	switch(deviceCommand) {
+	case(PING): {
+		p60DockCookie->setPingMessage();
+		break;
+	}
 	case(READ_MODULE_CFG):{
 		p60DockCookie->setReadModuleCfgMessage();
-		internalState = InternalStates::READ_MODULE_CFG;
-		this->pstStep = GET_READ;
 		break;
 	}
 	case(READ_HK):{
 		p60DockCookie->setReadHkMessage();
-		internalState = InternalStates::READ_HK;
-		this->pstStep = GET_READ;
-		break;
-	}
-	case(PING): {
 		break;
 	}
 	default:
@@ -62,19 +59,25 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand(
 }
 
 void P60DockHandler::fillCommandAndReplyMap(){
+	this->insertInCommandAndReplyMap(PING, 3);
 	this->insertInCommandAndReplyMap(READ_MODULE_CFG, 3);
 	this->insertInCommandAndReplyMap(READ_HK, 3);
 }
 
 ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start,
         size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
-	switch(internalState) {
-	case(InternalStates::READ_MODULE_CFG): {
+	MessageType_t messageType = p60DockCookie->getMessageType();
+	switch(messageType) {
+	case(PING):
+			*foundId = PING;
+			*foundLen = 4;
+		break;
+	case(READ_MODULE_CFG): {
 			*foundId = READ_MODULE_CFG;
 			*foundLen = moduleCfgTableSize;
 		break;
 	}
-	case(InternalStates::READ_HK): {
+	case(READ_HK): {
 			*foundId = READ_HK;
 			*foundLen = hkTableSize;
 		break;
@@ -82,6 +85,7 @@ ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start,
 	default:
 		return IGNORE_REPLY_DATA;
 	}
+	p60DockCookie->resetMessageType();
     return HasReturnvaluesIF::RETURN_OK;
 }
 
diff --git a/mission/devices/P60DockHandler.h b/mission/devices/P60DockHandler.h
index a55580ea..40161e42 100644
--- a/mission/devices/P60DockHandler.h
+++ b/mission/devices/P60DockHandler.h
@@ -30,13 +30,8 @@ private:
 	static const DeviceCommandId_t READ_MODULE_CFG = 0x71; //!< [EXPORT] : [COMMAND]
 	static const DeviceCommandId_t READ_HK = 0x72; //!< [EXPORT] : [COMMAND]
 
-	enum class InternalStates {
-		STATE_NONE,
-		READ_MODULE_CFG,
-		READ_HK
-	};
-	InternalStates internalState = InternalStates::STATE_NONE;
-	uint8_t moduleCfgTableSize = 188;
+	uint16_t moduleCfgTableSize = 412;
+	uint8_t calibrationTableSize = 174;
 	uint8_t hkTableSize = 188;
 
 	P60DockCookie* p60DockCookie;
diff --git a/test/testtasks/P60DockTestTask.cpp b/test/testtasks/P60DockTestTask.cpp
index 42067b01..985e69a3 100644
--- a/test/testtasks/P60DockTestTask.cpp
+++ b/test/testtasks/P60DockTestTask.cpp
@@ -36,7 +36,7 @@ ReturnValue_t P60DockTestTask::pingP60dock(void){
 	unsigned int pingSize = 100; // 100 bytes
 	uint32_t replyTime = csp_ping(p60dockAddress, timeout, pingSize, CSP_O_NONE);
 	sif::info << "Ping address: " << p60dockAddress << ", reply after "
-			<< replyTime << "ms" << std::endl;
+			<< replyTime << " ms" << std::endl;
 	return HasReturnvaluesIF::RETURN_OK;
 }