diff --git a/bsp_hosted/InitMission.cpp b/bsp_hosted/InitMission.cpp
index 39dcafff..4da13c70 100644
--- a/bsp_hosted/InitMission.cpp
+++ b/bsp_hosted/InitMission.cpp
@@ -11,6 +11,8 @@
 #include <fsfw/tasks/PeriodicTaskIF.h>
 #include <fsfw/tasks/TaskFactory.h>
 
+#include <mission/utility/InitMission.h>
+
 #include <iostream>
 
 // This is configured for linux without \cr
diff --git a/bsp_q7s/InitMission.cpp b/bsp_q7s/InitMission.cpp
index 681d6b48..2930803e 100644
--- a/bsp_q7s/InitMission.cpp
+++ b/bsp_q7s/InitMission.cpp
@@ -2,6 +2,8 @@
 #include "ObjectFactory.h"
 #include <OBSWConfig.h>
 
+#include <mission/utility/InitMission.h>
+
 #include <fsfw/objectmanager/ObjectManagerIF.h>
 #include <fsfw/returnvalues/HasReturnvaluesIF.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
@@ -13,7 +15,7 @@
 
 #include <iostream>
 
-// This is configured for linux without \cr
+/* This is configured for linux without CR */
 #ifdef LINUX
 ServiceInterfaceStream sif::debug("DEBUG");
 ServiceInterfaceStream sif::info("INFO");
@@ -39,144 +41,142 @@ void InitMission::initMission() {
     initTasks();
 }
 
-void InitMission::initTasks(){
+void InitMission::initTasks() {
+    TaskFactory* factory = TaskFactory::instance();
+    if(factory == nullptr) {
+        /* Should never happen ! */
+        return;
+    }
+
 	/* TMTC Distribution */
-	PeriodicTaskIF* TmTcDistributor = TaskFactory::instance()->
-			createPeriodicTask("DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.2, nullptr);
-	ReturnValue_t result = TmTcDistributor->addComponent(
+	PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask(
+	        "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, nullptr);
+	ReturnValue_t result = tmTcDistributor->addComponent(
 			objects::CCSDS_PACKET_DISTRIBUTOR);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+	    InitMission::printAddObjectError("CCSDS_DISTRIB", objects::CCSDS_PACKET_DISTRIBUTOR);
 	}
-	result = TmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS_PACKET_DISTRIB", objects::PUS_PACKET_DISTRIBUTOR);
 	}
-    result = TmTcDistributor->addComponent(objects::TM_FUNNEL);
+    result = tmTcDistributor->addComponent(objects::TM_FUNNEL);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Object add component failed" << std::endl;
+        InitMission::printAddObjectError("TM_FUNNEL", objects::TM_FUNNEL);
     }
 
     /* UDP bridge */
-    PeriodicTaskIF* UdpBridgeTask = TaskFactory::instance()->createPeriodicTask(
-    		"UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-			0.2, nullptr);
-    result = UdpBridgeTask->addComponent(objects::UDP_BRIDGE);
+    PeriodicTaskIF* udpBridgeTask = factory->createPeriodicTask(
+    		"UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, nullptr);
+    result = udpBridgeTask->addComponent(objects::UDP_BRIDGE);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Add component UDP Unix Bridge failed" << std::endl;
+        InitMission::printAddObjectError("UDP_BRIDGE", objects::UDP_BRIDGE);
     }
-    PeriodicTaskIF* UdpPollingTask = TaskFactory::instance()->
-    		createPeriodicTask("UDP_POLLING", 80,
-    		PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, nullptr);
-    result = UdpPollingTask->addComponent(objects::UDP_POLLING_TASK);
+    PeriodicTaskIF* udpPollingTask = factory->createPeriodicTask(
+            "UDP_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, nullptr);
+    result = udpPollingTask->addComponent(objects::UDP_POLLING_TASK);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Add component UDP Polling failed" << std::endl;
+        InitMission::printAddObjectError("UDP_POLLING", objects::UDP_POLLING_TASK);
     }
 
 	/* PUS Services */
-	PeriodicTaskIF* PusVerification = TaskFactory::instance()->
-			createPeriodicTask("PUS_VERIF_1", 40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
-	result = PusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	PeriodicTaskIF* pusVerification = factory->createPeriodicTask(
+	        "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
+	result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS_VERIF", objects::PUS_SERVICE_1_VERIFICATION);
 	}
 
-	PeriodicTaskIF* PusEvents = TaskFactory::instance()->
-			createPeriodicTask("PUS_VERIF_1", 60,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
-	result = PusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	PeriodicTaskIF* pusEvents = factory->createPeriodicTask(
+	        "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
+	result = pusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS_EVENTS", objects::PUS_SERVICE_5_EVENT_REPORTING);
 	}
 
-	PeriodicTaskIF* PusHighPrio = TaskFactory::instance()->
-			createPeriodicTask("PUS_HIGH_PRIO", 50,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.200, nullptr);
-	result = PusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
+	        "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
+	result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS_2", objects::PUS_SERVICE_2_DEVICE_ACCESS);
 	}
-	result = PusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS_9", objects::PUS_SERVICE_9_TIME_MGMT);
 	}
 
-	PeriodicTaskIF* PusMedPrio = TaskFactory::instance()->
-			createPeriodicTask("PUS_HIGH_PRIO", 40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.8, nullptr);
-	result = PusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	PeriodicTaskIF* pusMedPrio = factory->createPeriodicTask(
+	        "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, nullptr);
+	result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS_8", objects::PUS_SERVICE_8_FUNCTION_MGMT);
 	}
-	result = PusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS_200", objects::PUS_SERVICE_200_MODE_MGMT);
+	}
+	result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+	    InitMission::printAddObjectError("PUS_20", objects::PUS_SERVICE_20_PARAMETERS);
 	}
 
-	PeriodicTaskIF* PusLowPrio = TaskFactory::instance()->
-			createPeriodicTask("PUSB", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					1.6, nullptr);
-	result = PusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	PeriodicTaskIF* pusLowPrio = factory->createPeriodicTask(
+	        "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, nullptr);
+	result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS_17", objects::PUS_SERVICE_17_TEST);
 	}
 
 	//TODO: Add handling of missed deadlines
 	/* Polling Sequence Table Default */
-	FixedTimeslotTaskIF * PollingSequenceTableTaskDefault =
-			TaskFactory::instance()->createFixedTimeslotTask("PST_TASK_DEFAULT",
-					50, PeriodicTaskIF::MINIMUM_STACK_SIZE*4, 3.0,
-					nullptr);
-	result = pst::pollingSequenceInitDefault(PollingSequenceTableTaskDefault);
+	FixedTimeslotTaskIF * pollingSequenceTableTaskDefault = factory->createFixedTimeslotTask(
+	        "PST_TASK_DEFAULT", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, nullptr);
+	result = pst::pollingSequenceInitDefault(pollingSequenceTableTaskDefault);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
-		sif::error << "InitMission::initTasks: Creating PST failed!"
-				<< std::endl;
+		sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
 	}
 
 #if TE0720 == 0
-	FixedTimeslotTaskIF* GomSpacePstTask = TaskFactory::instance()->
+	FixedTimeslotTaskIF* gomSpacePstTask = factory->
 			createFixedTimeslotTask("GS_PST_TASK", 50,
 					PeriodicTaskIF::MINIMUM_STACK_SIZE*8, 3.0, nullptr);
-	result = pst::gomspacePstInit(GomSpacePstTask);
+	result = pst::gomspacePstInit(gomSpacePstTask);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
-		sif::error << "InitMission::initTasks: GomSpace PST initialization "
-				<< "failed!" << std::endl;
+		sif::error << "InitMission::initTasks: GomSpace PST initialization failed!" << std::endl;
 	}
 #endif
 
+	PeriodicTaskIF* testTask = factory->createPeriodicTask(
+	        "GPIOD_TEST", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1, nullptr);
+#if OBSW_ADD_TEST_CODE == 1
+	result = testTask->addComponent(objects::TEST_TASK);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+	    InitMission::printAddObjectError("TEST_TASK", objects::TEST_TASK);
+	}
+#endif /* OBSW_ADD_TEST_CODE == 1 */
 #if TE0720 == 1 && TEST_LIBGPIOD == 1
-	PeriodicTaskIF* TestTask = TaskFactory::instance()->
-            createPeriodicTask("Libgpiod Test Task", 60,
-                    PeriodicTaskIF::MINIMUM_STACK_SIZE, 1, nullptr);
-    result = TestTask->addComponent(objects::LIBGPIOD_TEST);
-    if(result != HasReturnvaluesIF::RETURN_OK){
-        sif::error << "Object add component libgpiod test task object" << std::endl;
+    result = testTask->addComponent(objects::LIBGPIOD_TEST);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("GPIOD_TEST", objects::LIBGPIOD_TEST);
     }
-#endif
+#endif /* TE0720 == 1 && TEST_LIBGPIOD == 1 */
 
-	//Main thread sleep
 	sif::info << "Starting tasks.." << std::endl;
-	TmTcDistributor->startTask();
-	UdpBridgeTask->startTask();
-	UdpPollingTask->startTask();
+	tmTcDistributor->startTask();
+	udpBridgeTask->startTask();
+	udpPollingTask->startTask();
 
 #if TE0720 == 0
-	GomSpacePstTask->startTask();
+	gomSpacePstTask->startTask();
 #endif
-	PollingSequenceTableTaskDefault->startTask();
+	pollingSequenceTableTaskDefault->startTask();
 
-	PusVerification->startTask();
-	PusEvents->startTask();
-	PusHighPrio->startTask();
-	PusMedPrio->startTask();
-	PusLowPrio->startTask();
+	pusVerification->startTask();
+	pusEvents->startTask();
+	pusHighPrio->startTask();
+	pusMedPrio->startTask();
+	pusLowPrio->startTask();
 
-#if TE0720 == 1 && TEST_LIBGPIOD == 1
-	TestTask->startTask();
-#endif
+	testTask->startTask();
 	sif::info << "Tasks started.." << std::endl;
 }
diff --git a/bsp_q7s/ObjectFactory.cpp b/bsp_q7s/ObjectFactory.cpp
index e5a1e039..1a54ca34 100644
--- a/bsp_q7s/ObjectFactory.cpp
+++ b/bsp_q7s/ObjectFactory.cpp
@@ -180,14 +180,14 @@ void ObjectFactory::produce(){
     GpioConfig_t gpioConfigMio0(std::string("gpiochip0"), 0,
             std::string("MIO0"), gpio::IN, 0);
     GpioCookie* gpioCookie = new GpioCookie;
-    gpioCookie->addGpio(gpioIds::Test_ID, gpioConfigMio0);
+    gpioCookie->addGpio(gpioIds::TEST_ID_0, gpioConfigMio0);
     new LibgpiodTest(objects::LIBGPIOD_TEST, objects::GPIO_IF, gpioCookie);
 #elif TE0720 == 1
     /* Configuration for MIO0 on TE0720-03-1CFA */
     GpioConfig_t gpioConfigForDummyHeater(std::string("gpiochip0"), 0,
             std::string("Heater0"), gpio::OUT, 0);
     heaterGpiosCookie->addGpio(gpioIds::HEATER_0, gpioConfigForDummyHeater);
-    new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie, objects::PCDU_HANDLER,
-            pcduSwitches::TCS_BOARD_8V_HEATER_IN);
+    new HeaterHandler(objects::HEATER_HANDLER, objects::GPIO_IF, heaterGpiosCookie,
+            objects::PCDU_HANDLER, pcduSwitches::TCS_BOARD_8V_HEATER_IN);
 #endif
 }
diff --git a/bsp_rpi/CMakeLists.txt b/bsp_rpi/CMakeLists.txt
index 281994bf..da35633d 100644
--- a/bsp_rpi/CMakeLists.txt
+++ b/bsp_rpi/CMakeLists.txt
@@ -6,11 +6,8 @@ target_sources(${TARGET_NAME} PUBLIC
 
 add_subdirectory(boardconfig)
 add_subdirectory(boardtest)
-
-# wiringPi is deprecated unfortunately..
-#target_link_libraries(${TARGET_NAME} PRIVATE
-#    wiringPi
-#)
+add_subdirectory(gpio)
+
 
 
 
diff --git a/bsp_rpi/InitMission.cpp b/bsp_rpi/InitMission.cpp
index 197abcc6..f3dc47ed 100644
--- a/bsp_rpi/InitMission.cpp
+++ b/bsp_rpi/InitMission.cpp
@@ -1,6 +1,12 @@
 #include "InitMission.h"
 #include "ObjectFactory.h"
 
+#include <fsfwconfig/objects/systemObjectList.h>
+#include <fsfwconfig/OBSWConfig.h>
+#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
+
+#include <mission/utility/InitMission.h>
+
 #include <fsfw/objectmanager/ObjectManagerIF.h>
 #include <fsfw/returnvalues/HasReturnvaluesIF.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
@@ -8,9 +14,6 @@
 #include <fsfw/tasks/FixedTimeslotTaskIF.h>
 #include <fsfw/tasks/PeriodicTaskIF.h>
 #include <fsfw/tasks/TaskFactory.h>
-#include <fsfwconfig/objects/systemObjectList.h>
-#include <fsfwconfig/OBSWConfig.h>
-#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
 
 #include <iostream>
 
@@ -34,127 +37,129 @@ void InitMission::initMission() {
 
 void InitMission::initTasks(){
 	/* TMTC Distribution */
-	PeriodicTaskIF* TmTcDistributor = TaskFactory::instance()->
+	PeriodicTaskIF* tmTcDistributor = TaskFactory::instance()->
 			createPeriodicTask("DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
 					0.100, nullptr);
-	ReturnValue_t result = TmTcDistributor->addComponent(
+	ReturnValue_t result = tmTcDistributor->addComponent(
 			objects::CCSDS_PACKET_DISTRIBUTOR);
 	if(result != HasReturnvaluesIF::RETURN_OK){
 		sif::error << "Object add component failed" << std::endl;
 	}
-	result = TmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
+	result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
 	if(result != HasReturnvaluesIF::RETURN_OK){
 		sif::error << "Object add component failed" << std::endl;
 	}
-    result = TmTcDistributor->addComponent(objects::TM_FUNNEL);
+    result = tmTcDistributor->addComponent(objects::TM_FUNNEL);
     if(result != HasReturnvaluesIF::RETURN_OK) {
     	sif::error << "Object add component failed" << std::endl;
     }
 
     /* UDP bridge */
-    PeriodicTaskIF* UdpBridgeTask = TaskFactory::instance()->createPeriodicTask(
+    PeriodicTaskIF* udpBridgeTask = TaskFactory::instance()->createPeriodicTask(
     		"UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE,
 			0.2, nullptr);
-    result = UdpBridgeTask->addComponent(objects::UDP_BRIDGE);
+    result = udpBridgeTask->addComponent(objects::UDP_BRIDGE);
     if(result != HasReturnvaluesIF::RETURN_OK) {
     	sif::error << "Add component UDP Unix Bridge failed" << std::endl;
     }
-    PeriodicTaskIF* UdpPollingTask = TaskFactory::instance()->
+    PeriodicTaskIF* udpPollingTask = TaskFactory::instance()->
     		createPeriodicTask("UDP_POLLING", 80,
     		PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, nullptr);
-    result = UdpPollingTask->addComponent(objects::UDP_POLLING_TASK);
+    result = udpPollingTask->addComponent(objects::UDP_POLLING_TASK);
     if(result != HasReturnvaluesIF::RETURN_OK) {
     	sif::error << "Add component UDP Polling failed" << std::endl;
     }
 
 	/* PUS Services */
-	PeriodicTaskIF* PusVerification = TaskFactory::instance()->
-			createPeriodicTask("PUS_VERIF_1", 40,
+	PeriodicTaskIF* pusVerification = TaskFactory::instance()->
+			createPeriodicTask("PUS_VERIF", 40,
 					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
-	result = PusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
+	result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
 	if(result != HasReturnvaluesIF::RETURN_OK){
 		sif::error << "Object add component failed" << std::endl;
 	}
 
-	PeriodicTaskIF* PusEvents = TaskFactory::instance()->
-			createPeriodicTask("PUS_VERIF_1", 60,
+	PeriodicTaskIF* pusEvents = TaskFactory::instance()->
+			createPeriodicTask("PUS_EVENTS", 60,
 					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
-	result = PusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
+	result = pusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
 	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+        InitMission::printAddObjectError("PUS5", objects::PUS_SERVICE_5_EVENT_REPORTING);
 	}
 
-	PeriodicTaskIF* PusHighPrio = TaskFactory::instance()->
+	PeriodicTaskIF* pusHighPrio = TaskFactory::instance()->
 			createPeriodicTask("PUS_HIGH_PRIO", 50,
 					PeriodicTaskIF::MINIMUM_STACK_SIZE,
 					0.200, nullptr);
-	result = PusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS);
 	}
-	result = PusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
+	result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT);
 	}
 
-	PeriodicTaskIF* PusMedPrio = TaskFactory::instance()->
+	PeriodicTaskIF* pusMedPrio = TaskFactory::instance()->
 			createPeriodicTask("PUS_HIGH_PRIO", 40,
 					PeriodicTaskIF::MINIMUM_STACK_SIZE,
 					0.8, nullptr);
-	result = PusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-	result = PusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-
-	PeriodicTaskIF* PusLowPrio = TaskFactory::instance()->
-			createPeriodicTask("PUSB", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					1.6, nullptr);
-	result = PusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-
-
-#if OBSW_ADD_TEST_CODE == 1
-	FixedTimeslotTaskIF* TestTimeslotTask = TaskFactory::instance()->
-			createFixedTimeslotTask("PST_TEST_TASK", 10,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
-	result = pst::pollingSequenceTestFunction(TestTimeslotTask);
+	result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
-		sif::error << "InitMission::createTasks: Test PST initialization "
-				<< "failed!" << std::endl;
+        InitMission::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT);
 	}
-
-#endif
-
-	PeriodicTaskIF* SpiTestTask = TaskFactory::instance()->
-            createPeriodicTask("SPI_TEST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-                    2.0, nullptr);
-    result = SpiTestTask->addComponent(objects::SPI_TEST);
-    if(result != HasReturnvaluesIF::RETURN_OK){
-        sif::error << "Object add SPI test failed" << std::endl;
+	result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT);
+	}
+	result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS);
     }
 
-	//Main thread sleep
+	PeriodicTaskIF* pusLowPrio = TaskFactory::instance()->
+			createPeriodicTask("PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE,
+					1.6, nullptr);
+	result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+	    InitMission::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST);
+	}
+
+	PeriodicTaskIF* testTask = TaskFactory::instance()->
+            createPeriodicTask("SPI_TEST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
+                    2.0, nullptr);
+#if OBSW_ADD_TEST_CODE == 1
+	result = testTask->addComponent(objects::TEST_TASK);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("TEST_TASK", objects::TEST_TASK);
+    }
+#endif /* OBSW_ADD_TEST_CODE == 1 */
+#if RPI_ADD_SPI_TEST == 1
+    result = testTask->addComponent(objects::SPI_TEST);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("SPI_TEST", objects::SPI_TEST);
+    }
+#endif /* RPI_ADD_SPI_TEST == 1 */
+#if RPI_ADD_GPIO_TEST == 1
+    result = testTask->addComponent(objects::LIBGPIOD_TEST);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        InitMission::printAddObjectError("GPIOD_TEST", objects::LIBGPIOD_TEST);
+    }
+#endif /* RPI_ADD_GPIO_TEST == 1 */
+
 	sif::info << "Starting tasks.." << std::endl;
-	TmTcDistributor->startTask();
-	UdpBridgeTask->startTask();
-	UdpPollingTask->startTask();
+	tmTcDistributor->startTask();
+	udpBridgeTask->startTask();
+	udpPollingTask->startTask();
 
-	PusVerification->startTask();
-	PusEvents->startTask();
-	PusHighPrio->startTask();
-	PusMedPrio->startTask();
-	PusLowPrio->startTask();
-
-	SpiTestTask->startTask();
+	pusVerification->startTask();
+	pusEvents->startTask();
+	pusHighPrio->startTask();
+	pusMedPrio->startTask();
+	pusLowPrio->startTask();
 
 #if OBSW_ADD_TEST_CODE == 1
-	TestTimeslotTask->startTask();
-#endif
+	testTask->startTask();
+#endif /* OBSW_ADD_TEST_CODE == 1 */
 	sif::info << "Tasks started.." << std::endl;
 }
diff --git a/bsp_rpi/ObjectFactory.cpp b/bsp_rpi/ObjectFactory.cpp
index 7a7820b8..458456c8 100644
--- a/bsp_rpi/ObjectFactory.cpp
+++ b/bsp_rpi/ObjectFactory.cpp
@@ -1,7 +1,9 @@
 #include "ObjectFactory.h"
 #include <bsp_rpi/boardtest/SpiTest.h>
+#include <bsp_rpi/gpio/GPIORPi.h>
 
 #include <objects/systemObjectList.h>
+#include <devices/gpioIds.h>
 #include <OBSWConfig.h>
 #include <tmtc/apid.h>
 #include <tmtc/pusIds.h>
@@ -47,11 +49,23 @@ void ObjectFactory::produce(){
 			objects::TM_STORE, objects::TC_STORE);
 	new TcUnixUdpPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE);
 
-	new SpiTest(objects::SPI_TEST);
-	new LinuxLibgpioIF(objects::GPIO_IF);
+    new LinuxLibgpioIF(objects::GPIO_IF);
 
-#if RPI_TEST_GPIO == 1
+#if RPI_ADD_SPI_TEST == 1
+	new SpiTest(objects::SPI_TEST);
+#endif
+
+#if RPI_LOOPBACK_TEST_GPIO == 1
 	GpioCookie* gpioCookie = new GpioCookie();
+	/* Loopback pins. Adapt according to setup */
+	gpioId_t gpioIdSender = gpioIds::TEST_ID_0;
+	int bcmPinSender = 26;
+	gpioId_t gpioIdReader = gpioIds::TEST_ID_1;
+	int bcmPinReader = 16;
+	gpio::createRpiGpioConfig(gpioCookie, gpioIdSender, bcmPinSender, "GPIO_LB_SENDER",
+	        gpio::Direction::OUT, 0);
+    gpio::createRpiGpioConfig(gpioCookie, gpioIdReader, bcmPinReader, "GPIO_LB_READER",
+            gpio::Direction::IN, 0);
 	new LibgpiodTest(objects::LIBGPIOD_TEST, objects::GPIO_IF, gpioCookie);
 #endif
 }
diff --git a/bsp_rpi/boardconfig/rpi_config.h b/bsp_rpi/boardconfig/rpi_config.h
index 7d41e302..89141b80 100644
--- a/bsp_rpi/boardconfig/rpi_config.h
+++ b/bsp_rpi/boardconfig/rpi_config.h
@@ -1,7 +1,9 @@
 #ifndef BSP_RPI_BOARDCONFIG_RPI_CONFIG_H_
 #define BSP_RPI_BOARDCONFIG_RPI_CONFIG_H_
 
-#define RPI_TEST_GPIO           1
+#define RPI_ADD_GPIO_TEST               1
+#define RPI_ADD_SPI_TEST                1
+#define RPI_LOOPBACK_TEST_GPIO          1
 
 
 
diff --git a/bsp_rpi/boardtest/CMakeLists.txt b/bsp_rpi/boardtest/CMakeLists.txt
index 1e373efb..768fc52f 100644
--- a/bsp_rpi/boardtest/CMakeLists.txt
+++ b/bsp_rpi/boardtest/CMakeLists.txt
@@ -1,6 +1,5 @@
 target_sources(${TARGET_NAME} PRIVATE
 	SpiTest.cpp
-	RPiGPIO.cpp
 )
 
 
diff --git a/bsp_rpi/gpio/CMakeLists.txt b/bsp_rpi/gpio/CMakeLists.txt
new file mode 100644
index 00000000..85bd6aa6
--- /dev/null
+++ b/bsp_rpi/gpio/CMakeLists.txt
@@ -0,0 +1,9 @@
+target_sources(${TARGET_NAME} PUBLIC
+	GPIORPi.cpp
+)
+
+
+
+
+
+
diff --git a/bsp_rpi/gpio/GPIORPi.cpp b/bsp_rpi/gpio/GPIORPi.cpp
new file mode 100644
index 00000000..84a3365f
--- /dev/null
+++ b/bsp_rpi/gpio/GPIORPi.cpp
@@ -0,0 +1,36 @@
+#include "GPIORPi.h"
+#include <FSFWConfig.h>
+
+#include <fsfw/serviceinterface/ServiceInterface.h>
+#include <linux/gpio/GpioCookie.h>
+
+ReturnValue_t gpio::createRpiGpioConfig(GpioCookie* cookie, gpioId_t gpioId, int bcmPin,
+        std::string consumer, gpio::Direction direction, int initValue) {
+    if(cookie == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    GpioConfig_t config;
+    /* Default chipname for Raspberry Pi. There is still gpiochip1 for expansion, but most users
+    will not need this */
+    config.chipname = "gpiochip0";
+
+    config.consumer = consumer;
+    config.direction = direction;
+    config.initValue = initValue;
+
+    /* Sanity check for the BCM pins before assigning it */
+    if(bcmPin > 27) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "createRpiGpioConfig: BCM pin " << bcmPin << " invalid!" << std::endl;
+#else
+        sif::printError("createRpiGpioConfig: BCM pin %d invalid!\n", bcmPin);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    config.lineNum = bcmPin;
+    cookie->addGpio(gpioId, config);
+    return HasReturnvaluesIF::RETURN_OK;
+}
diff --git a/bsp_rpi/gpio/GPIORPi.h b/bsp_rpi/gpio/GPIORPi.h
new file mode 100644
index 00000000..a2c11879
--- /dev/null
+++ b/bsp_rpi/gpio/GPIORPi.h
@@ -0,0 +1,26 @@
+#ifndef BSP_RPI_GPIO_GPIORPI_H_
+#define BSP_RPI_GPIO_GPIORPI_H_
+
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <linux/gpio/gpioDefinitions.h>
+
+class GpioCookie;
+
+namespace gpio {
+
+/**
+ * Create a GpioConfig_t. This function does a sanity check on the BCM pin number and fails if the
+ * BCM pin is invalid.
+ * @param cookie    Adds the configuration to this cookie directly
+ * @param gpioId    ID which identifies the GPIO configuration
+ * @param bcmPin    Raspberry Pi BCM pin
+ * @param consumer  Information string
+ * @param direction GPIO direction
+ * @param initValue Intial value for output pins, 0 for low, 1 for high
+ * @return
+ */
+ReturnValue_t createRpiGpioConfig(GpioCookie* cookie, gpioId_t gpioId, int bcmPin,
+        std::string consumer, gpio::Direction direction, int initValue);
+}
+
+#endif /* BSP_RPI_GPIO_GPIORPI_H_ */
diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h
index abfb7456..5f841062 100644
--- a/fsfwconfig/OBSWConfig.h
+++ b/fsfwconfig/OBSWConfig.h
@@ -11,18 +11,18 @@
 #endif
 #include "OBSWVersion.h"
 
-#define TEST_LIBGPIOD       0
-
 /* These defines should be disabled for mission code but are useful for
 debugging. */
-#define OBSW_VERBOSE_LEVEL  1
+#define OBSW_VERBOSE_LEVEL      1
+#define OBSW_ADD_TEST_CODE      1
+#define TEST_LIBGPIOD           0
 
-#define TE0720              0
+#define TE0720                  0
 
-#define P60DOCK_DEBUG   0
-#define PDU1_DEBUG  0
-#define PDU2_DEBUG  0
-#define ACU_DEBUG  1
+#define P60DOCK_DEBUG           0
+#define PDU1_DEBUG              0
+#define PDU2_DEBUG              0
+#define ACU_DEBUG               1
 
 #ifdef __cplusplus
 
diff --git a/fsfwconfig/devices/gpioIds.h b/fsfwconfig/devices/gpioIds.h
index 70507b42..1e95ab9d 100644
--- a/fsfwconfig/devices/gpioIds.h
+++ b/fsfwconfig/devices/gpioIds.h
@@ -15,7 +15,8 @@ namespace gpioIds {
 		HEATER_7,
 		DEPLSA1,
 		DEPLSA2,
-		Test_ID
+		TEST_ID_0,
+		TEST_ID_1
 	};
 }
 
diff --git a/linux/boardtest/LibgpiodTest.cpp b/linux/boardtest/LibgpiodTest.cpp
index cc60ce18..5dec5581 100644
--- a/linux/boardtest/LibgpiodTest.cpp
+++ b/linux/boardtest/LibgpiodTest.cpp
@@ -13,7 +13,7 @@ LibgpiodTest::LibgpiodTest(object_id_t objectId, object_id_t gpioIfobjectId,
         sif::error << "LibgpiodTest::LibgpiodTest: Invalid Gpio interface." << std::endl;
     }
     gpioInterface->initialize(gpioCookie);
-    testCase = TestCases::READ;
+    testCase = TestCases::LOOPBACK;
 }
 
 LibgpiodTest::~LibgpiodTest() {
@@ -25,7 +25,7 @@ ReturnValue_t LibgpiodTest::performPeriodicAction() {
 
     switch(testCase) {
     case(TestCases::READ): {
-        result = gpioInterface->readGpio(gpioIds::Test_ID, &gpioState);
+        result = gpioInterface->readGpio(gpioIds::TEST_ID_0, &gpioState);
         if (result != RETURN_OK) {
             sif::debug << "LibgpiodTest::performPeriodicAction: Failed to read gpio "
                     << std::endl;
@@ -38,7 +38,7 @@ ReturnValue_t LibgpiodTest::performPeriodicAction() {
         break;
     }
     case(TestCases::LOOPBACK): {
-
+        break;
     }
     }
 
@@ -46,3 +46,37 @@ ReturnValue_t LibgpiodTest::performPeriodicAction() {
     return RETURN_OK;
 }
 
+ReturnValue_t LibgpiodTest::performOneShotAction() {
+    int gpioState;
+    ReturnValue_t result;
+
+    switch(testCase) {
+    case(TestCases::READ): {
+        break;
+    }
+    case(TestCases::LOOPBACK): {
+        result = gpioInterface->pullHigh(gpioIds::TEST_ID_0);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            sif::info << "LibgpiodTest::performOneShotAction:"
+                    "GPIO pulled high successfully for loopback test" << std::endl;
+        }
+        else {
+            sif::warning << "LibgpiodTest::performOneShotAction: Could not pull GPIO high!"
+                    << std::endl;
+            return HasReturnvaluesIF::RETURN_OK;
+        }
+        result = gpioInterface->readGpio(gpioIds::TEST_ID_1, &gpioState);
+        if(result == HasReturnvaluesIF::RETURN_OK and gpioState == 1) {
+            sif::info << "LibgpiodTest::performOneShotAction:"
+                    "GPIO state read successfully and is high" << std::endl;
+        }
+        else {
+            sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not high!"
+                    << std::endl;
+            return HasReturnvaluesIF::RETURN_OK;
+        }
+        break;
+    }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
diff --git a/linux/boardtest/LibgpiodTest.h b/linux/boardtest/LibgpiodTest.h
index 642d4cd7..e9c6c030 100644
--- a/linux/boardtest/LibgpiodTest.h
+++ b/linux/boardtest/LibgpiodTest.h
@@ -23,7 +23,8 @@ public:
     virtual ~LibgpiodTest();
 
 protected:
-    virtual ReturnValue_t performPeriodicAction() override;
+    ReturnValue_t performOneShotAction() override;
+    ReturnValue_t performPeriodicAction() override;
 
 private:
     GpioIF* gpioInterface;
diff --git a/linux/gpio/GpioCookie.cpp b/linux/gpio/GpioCookie.cpp
index 7f80b550..cd73574c 100644
--- a/linux/gpio/GpioCookie.cpp
+++ b/linux/gpio/GpioCookie.cpp
@@ -4,17 +4,23 @@
 GpioCookie::GpioCookie() {
 }
 
-void GpioCookie::addGpio(gpioId_t gpioId, GpioConfig_t gpioConfig){
+ReturnValue_t GpioCookie::addGpio(gpioId_t gpioId, GpioConfig_t& gpioConfig){
 	auto gpioMapIter = gpioMap.find(gpioId);
 	if(gpioMapIter == gpioMap.end()) {
 		auto statusPair = gpioMap.emplace(gpioId, gpioConfig);
 		if (statusPair.second == false) {
+#if FSFW_VERBOSE_LEVEL >= 1
 			sif::error << "GpioCookie::addGpio: Failed to add GPIO " << gpioId <<
 			        "to GPIO map" << std::endl;
+#endif
+			return HasReturnvaluesIF::RETURN_FAILED;
 		}
-		return;
+		return HasReturnvaluesIF::RETURN_OK;
 	}
+#if FSFW_VERBOSE_LEVEL >= 1
 	sif::error << "GpioCookie::addGpio: GPIO already exists in GPIO map " << std::endl;
+#endif
+	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
 GpioMap GpioCookie::getGpioMap() const {
diff --git a/linux/gpio/GpioCookie.h b/linux/gpio/GpioCookie.h
index 05e80a2e..e1436813 100644
--- a/linux/gpio/GpioCookie.h
+++ b/linux/gpio/GpioCookie.h
@@ -23,7 +23,7 @@ public:
 
     virtual ~GpioCookie();
 
-    void addGpio(gpioId_t gpioId, GpioConfig_t gpioConfig);
+    ReturnValue_t addGpio(gpioId_t gpioId, GpioConfig_t& gpioConfig);
     /**
      * @brief	Get map with registered GPIOs.
      */
diff --git a/linux/gpio/GpioIF.h b/linux/gpio/GpioIF.h
index 269174e4..80d3a727 100644
--- a/linux/gpio/GpioIF.h
+++ b/linux/gpio/GpioIF.h
@@ -29,6 +29,7 @@ public:
 	 * 			functionality to pull a certain GPIO to high logic level.
 	 *
 	 * @param gpioId	A unique number which specifies the GPIO to drive.
+	 * @return  Returns RETURN_OK for success. This should never return RETURN_FAILED.
 	 */
 	virtual ReturnValue_t pullHigh(gpioId_t gpioId) = 0;
 
diff --git a/linux/gpio/LinuxLibgpioIF.h b/linux/gpio/LinuxLibgpioIF.h
index 6a412733..a849bb09 100644
--- a/linux/gpio/LinuxLibgpioIF.h
+++ b/linux/gpio/LinuxLibgpioIF.h
@@ -58,6 +58,7 @@ private:
 	 * @brief   Performs the initial configuration of all GPIOs specified in the GpioMap mapToAdd.
 	 */
 	ReturnValue_t configureGpios(GpioMap& mapToAdd);
+
 };
 
 #endif /* LINUX_GPIO_LINUXLIBGPIOIF_H_ */
diff --git a/linux/gpio/gpioDefinitions.h b/linux/gpio/gpioDefinitions.h
index bbc599ee..ce26ec78 100644
--- a/linux/gpio/gpioDefinitions.h
+++ b/linux/gpio/gpioDefinitions.h
@@ -28,6 +28,9 @@ enum Direction {
  *                      pointer.
  */
 typedef struct GpioConfig {
+    GpioConfig(): chipname(), lineNum(0), consumer(), direction(gpio::Direction::OUT),
+            initValue(0) {};
+
     GpioConfig(std::string chipname_, int lineNum_, std::string consumer_,
             gpio::Direction direction_, int initValue_):
             chipname(chipname_), lineNum(lineNum_), consumer(consumer_),
diff --git a/bsp_rpi/boardtest/RPiGPIO.cpp b/misc/archive/RPiGPIO.cpp
similarity index 100%
rename from bsp_rpi/boardtest/RPiGPIO.cpp
rename to misc/archive/RPiGPIO.cpp
diff --git a/bsp_rpi/boardtest/RPiGPIO.h b/misc/archive/RPiGPIO.h
similarity index 100%
rename from bsp_rpi/boardtest/RPiGPIO.h
rename to misc/archive/RPiGPIO.h
diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp
index 2a254cf0..2fcb4450 100644
--- a/mission/core/GenericFactory.cpp
+++ b/mission/core/GenericFactory.cpp
@@ -11,6 +11,7 @@
 #include <fsfw/pus/CService200ModeCommanding.h>
 #include <fsfw/pus/Service17Test.h>
 #include <fsfw/pus/Service1TelecommandVerification.h>
+#include <fsfw/pus/Service20ParameterManagement.h>
 #include <fsfw/pus/Service3Housekeeping.h>
 #include <fsfw/pus/Service2DeviceAccess.h>
 #include <fsfw/pus/Service5EventReporting.h>
@@ -78,11 +79,13 @@ void ObjectFactory::produceGenericObjects() {
             apid::EIVE_OBSW, pus::PUS_SERVICE_9);
     new Service17Test(objects::PUS_SERVICE_17_TEST, apid::EIVE_OBSW,
             pus::PUS_SERVICE_17);
+    new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, apid::EIVE_OBSW,
+            pus::PUS_SERVICE_20);
     new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT,
             apid::EIVE_OBSW, pus::PUS_SERVICE_200);
 
     /* Test Device Handler */
 #if OBSW_ADD_TEST_CODE == 1
-    	new TestTask(objects::TEST_TASK);
+    new TestTask(objects::TEST_TASK);
 #endif
 }
diff --git a/mission/utility/InitMission.h b/mission/utility/InitMission.h
new file mode 100644
index 00000000..93ebc9bc
--- /dev/null
+++ b/mission/utility/InitMission.h
@@ -0,0 +1,22 @@
+#ifndef MISSION_UTILITY_INITMISSION_H_
+#define MISSION_UTILITY_INITMISSION_H_
+
+#include <fsfw/objectmanager/SystemObjectIF.h>
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+namespace InitMission {
+
+void printAddObjectError(const char* name, object_id_t objectId) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "InitMission::printAddError: Adding object " << name << " with object ID 0x"
+            << std::hex << std::setfill('0') << std::setw(8) << objectId
+            << " failed!" << std::dec << std::endl;
+#else
+    sif::printError("InitMission::printAddError: Adding object %s with object ID 0x%08x failed!\n" ,
+            name, objectId);
+#endif
+}
+
+}
+
+#endif /* MISSION_UTILITY_INITMISSION_H_ */