diff --git a/bsp_linux_board/InitMission.cpp b/bsp_linux_board/InitMission.cpp
index b8328ee9..63e06997 100644
--- a/bsp_linux_board/InitMission.cpp
+++ b/bsp_linux_board/InitMission.cpp
@@ -63,6 +63,7 @@ void initmission::initTasks() {
     sif::error << "Object add component failed" << std::endl;
   }
 
+  /* UDP bridge */
   PeriodicTaskIF* tmtcBridgeTask = factory->createPeriodicTask(
       "TMTC_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
   result = tmtcBridgeTask->addComponent(objects::TMTC_BRIDGE);
@@ -80,21 +81,12 @@ void initmission::initTasks() {
   std::vector<PeriodicTaskIF*> pusTasks;
   createPusTasks(*factory, missedDeadlineFunc, pusTasks);
 
-  FixedTimeslotTaskIF* pst = factory.createFixedTimeslotTask(
-      "STAR_TRACKER_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
-  result = pst::pstUart(Pst);
-  if (result != HasReturnvaluesIF::RETURN_OK) {
-    sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
-  }
-  taskVec.push_back(pst);
-
-  PeriodicTaskIF* strHelperTask = factory->createPeriodicTask(
-      "STR_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
-  result = strHelperTask->addComponent(objects::STR_HELPER);
-  if (result != HasReturnvaluesIF::RETURN_OK) {
-    initmission::printAddObjectError("STR_HELPER", objects::STR_HELPER);
-  }
-  taskvec.push_back(strHelperTask);
+  std::vector<PeriodicTaskIF*> pstTasks;
+  createPstTasks(*factory, missedDeadlineFunc, pstTasks);
+#if OBSW_ADD_TEST_CODE == 1
+  std::vector<PeriodicTaskIF*> testTasks;
+  createTestTasks(*factory, missedDeadlineFunc, pstTasks);
+#endif /* OBSW_ADD_TEST_CODE == 1 */
 
   auto taskStarter = [](std::vector<PeriodicTaskIF*>& taskVector, std::string name) {
     for (const auto& task : taskVector) {
@@ -107,12 +99,21 @@ void initmission::initTasks() {
   };
 
   sif::info << "Starting tasks.." << std::endl;
-  tmtcDistributor->startTask();
+  tmTcDistributor->startTask();
   tmtcBridgeTask->startTask();
   tmtcPollingTask->startTask();
 
+  taskStarter(pusTasks, "PUS Tasks");
+#if OBSW_ADD_TEST_CODE == 1
+  taskStarter(testTasks, "Test Tasks");
+#endif /* OBSW_ADD_TEST_CODE == 1 */
   taskStarter(pstTasks, "PST Tasks");
 
+#if OBSW_ADD_TEST_PST == 1
+  if (startTestPst) {
+    pstTestTask->startTask();
+  }
+#endif /* RPI_TEST_ACS_BOARD == 1 */
   sif::info << "Tasks started.." << std::endl;
 }
 
@@ -184,3 +185,60 @@ void initmission::createPusTasks(TaskFactory& factory,
   }
   taskVec.push_back(pusLowPrio);
 }
+
+void initmission::createPstTasks(TaskFactory& factory,
+                                 TaskDeadlineMissedFunction missedDeadlineFunc,
+                                 std::vector<PeriodicTaskIF*>& taskVec) {
+  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+#if OBSW_ADD_SPI_TEST_CODE == 0
+  FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask(
+      "SPI_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
+  result = pst::pstSpi(spiPst);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
+  }
+  taskVec.push_back(spiPst);
+#endif
+}
+
+void initmission::createTestTasks(TaskFactory& factory,
+                                  TaskDeadlineMissedFunction missedDeadlineFunc,
+                                  std::vector<PeriodicTaskIF*>& taskVec) {
+  ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+  PeriodicTaskIF* testTask = factory.createPeriodicTask(
+      "TEST_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
+  result = testTask->addComponent(objects::TEST_TASK);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK);
+  }
+#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 */
+#if RPI_ADD_UART_TEST == 1
+  result = testTask->addComponent(objects::UART_TEST);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    initmission::printAddObjectError("UART_TEST", objects::UART_TEST);
+  }
+#endif /* RPI_ADD_GPIO_TEST == 1 */
+
+  bool startTestPst = true;
+  static_cast<void>(startTestPst);
+#if OBSW_ADD_TEST_PST == 1
+  FixedTimeslotTaskIF* pstTestTask = factory->createFixedTimeslotTask(
+      "TEST_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 2.0, missedDeadlineFunc);
+  result = pst::pstTest(pstTestTask);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    sif::info << "initmission::initTasks: ACS PST empty or invalid" << std::endl;
+    startTestPst = false;
+  }
+#endif /* RPI_TEST_ACS_BOARD == 1 */
+}