diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp
index fd94b863..cd7b0d21 100644
--- a/bsp_q7s/core/InitMission.cpp
+++ b/bsp_q7s/core/InitMission.cpp
@@ -3,20 +3,22 @@
 #include "OBSWConfig.h"
 #include "pollingsequence/pollingSequenceFactory.h"
 
-#include <mission/utility/InitMission.h>
+#include "mission/utility/InitMission.h"
 
-#include <fsfw/objectmanager/ObjectManagerIF.h>
-#include <fsfw/returnvalues/HasReturnvaluesIF.h>
-#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
-#include <fsfw/objectmanager/ObjectManager.h>
-#include <fsfw/tasks/FixedTimeslotTaskIF.h>
-#include <fsfw/tasks/PeriodicTaskIF.h>
-#include <fsfw/tasks/TaskFactory.h>
+#include "fsfw/platform.h"
+#include "fsfw/objectmanager/ObjectManagerIF.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+#include "fsfw/serviceinterface/ServiceInterfaceStream.h"
+#include "fsfw/objectmanager/ObjectManager.h"
+#include "fsfw/tasks/FixedTimeslotTaskIF.h"
+#include "fsfw/tasks/PeriodicTaskIF.h"
+#include "fsfw/tasks/TaskFactory.h"
 
 #include <iostream>
+#include <vector>
 
 /* This is configured for linux without CR */
-#ifdef LINUX
+#ifdef PLATFORM_UNIX
 ServiceInterfaceStream sif::debug("DEBUG");
 ServiceInterfaceStream sif::info("INFO");
 ServiceInterfaceStream sif::warning("WARNING");
@@ -102,15 +104,111 @@ void initmission::initTasks() {
     }
 #endif
 
+    std::vector<PeriodicTaskIF*> pusTasks;
+    createPusTasks(*factory, missedDeadlineFunc, pusTasks);
+    std::vector<PeriodicTaskIF*> pstTasks;
+    createPstTasks(*factory, missedDeadlineFunc, pstTasks);
+    std::vector<PeriodicTaskIF*> testTasks;
+    createTestTasks(*factory, missedDeadlineFunc, testTasks);
+
+    auto taskStarter = [](std::vector<PeriodicTaskIF*>& taskVector, std::string name) {
+        for(const auto& task: taskVector) {
+            if(task != nullptr) {
+                task->startTask();
+            }
+            else {
+                sif::error << "Task in vector " << name << " is invalid!" << std::endl;
+            }
+        }
+    };
+
+    sif::info << "Starting tasks.." << std::endl;
+    tmTcDistributor->startTask();
+    udpBridgeTask->startTask();
+    udpPollingTask->startTask();
+    coreController->startTask();
+
+    taskStarter(pstTasks, "PST task vector");
+    taskStarter(pusTasks, "PUS task vector");
+#if OBSW_ADD_TEST_CODE == 1
+    taskStarter(testTasks, "Test task vector");
+#endif
+
+#if TEST_CCSDS_BRIDGE == 1
+    ptmeTestTask->startTask();
+#endif
+    sif::info << "Tasks started.." << std::endl;
+}
+
+void initmission::createPstTasks(TaskFactory& factory,
+        TaskDeadlineMissedFunction missedDeadlineFunc, std::vector<PeriodicTaskIF*> &taskVec) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+#if BOARD_TE0720 == 0
+    /* Polling Sequence Table Default */
+#if Q7S_ADD_SPI_TEST == 0
+    FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask(
+            "PST_TASK_DEFAULT", 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
+
+    FixedTimeslotTaskIF* uartPst = factory.createFixedTimeslotTask(
+            "UART_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
+    result = pst::pstUart(uartPst);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
+    }
+    taskVec.push_back(uartPst);
+    FixedTimeslotTaskIF* gpioPst = factory.createFixedTimeslotTask(
+            "GPIO_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
+    result = pst::pstGpio(gpioPst);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
+    }
+    taskVec.push_back(gpioPst);
+    FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask(
+            "I2C_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
+    result = pst::pstI2c(i2cPst);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
+    }
+
+    FixedTimeslotTaskIF* gomSpacePstTask = factory.createFixedTimeslotTask(
+            "GS_PST_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc);
+    result = pst::pstGompaceCan(gomSpacePstTask);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        sif::error << "InitMission::initTasks: GomSpace PST initialization failed!" << std::endl;
+    }
+    taskVec.push_back(i2cPst);
+#else /* BOARD_TE7020 == 0 */
+    FixedTimeslotTaskIF * pollingSequenceTaskTE0720 = factory->createFixedTimeslotTask(
+            "PST_TASK_TE0720", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE * 8, 3.0,
+            missedDeadlineFunc);
+    result = pst::pollingSequenceTE0720(pollingSequenceTaskTE0720);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        sif::error << "InitMission::initTasks: Creating TE0720 PST failed!" << std::endl;
+    }
+    taskVec.push_back(pollingSequenceTaskTE0720);
+#endif /* BOARD_TE7020 == 1 */
+}
+
+void initmission::createPusTasks(TaskFactory &factory,
+        TaskDeadlineMissedFunction missedDeadlineFunc, std::vector<PeriodicTaskIF*> &taskVec) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
     /* PUS Services */
-    PeriodicTaskIF* pusVerification = factory->createPeriodicTask(
+    PeriodicTaskIF* pusVerification = factory.createPeriodicTask(
             "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
     result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
     if(result != HasReturnvaluesIF::RETURN_OK) {
         initmission::printAddObjectError("PUS_VERIF", objects::PUS_SERVICE_1_VERIFICATION);
     }
+    taskVec.push_back(pusVerification);
 
-    PeriodicTaskIF* pusEvents = factory->createPeriodicTask(
+    PeriodicTaskIF* pusEvents = factory.createPeriodicTask(
             "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
     result = pusEvents->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
     if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -120,8 +218,9 @@ void initmission::initTasks() {
     if(result != HasReturnvaluesIF::RETURN_OK) {
         initmission::printAddObjectError("PUS_MGMT", objects::EVENT_MANAGER);
     }
+    taskVec.push_back(pusEvents);
 
-    PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
+    PeriodicTaskIF* pusHighPrio = factory.createPeriodicTask(
             "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
     result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
     if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -131,8 +230,9 @@ void initmission::initTasks() {
     if(result != HasReturnvaluesIF::RETURN_OK) {
         initmission::printAddObjectError("PUS_9", objects::PUS_SERVICE_9_TIME_MGMT);
     }
+    taskVec.push_back(pusHighPrio);
 
-    PeriodicTaskIF* pusMedPrio = factory->createPeriodicTask(
+    PeriodicTaskIF* pusMedPrio = factory.createPeriodicTask(
             "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc);
     result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
     if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -150,8 +250,9 @@ void initmission::initTasks() {
     if(result != HasReturnvaluesIF::RETURN_OK) {
         initmission::printAddObjectError("PUS_20", objects::PUS_SERVICE_20_PARAMETERS);
     }
+    taskVec.push_back(pusMedPrio);
 
-    PeriodicTaskIF* pusLowPrio = factory->createPeriodicTask(
+    PeriodicTaskIF* pusLowPrio = factory.createPeriodicTask(
             "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc);
     result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
     if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -161,58 +262,13 @@ void initmission::initTasks() {
     if(result != HasReturnvaluesIF::RETURN_OK) {
         initmission::printAddObjectError("ERROR_REPORTER", objects::INTERNAL_ERROR_REPORTER);
     }
+    taskVec.push_back(pusLowPrio);
+}
 
-#if BOARD_TE0720 == 0
-    //TODO: Add handling of missed deadlines
-    /* Polling Sequence Table Default */
-#if Q7S_ADD_SPI_TEST == 0
-    FixedTimeslotTaskIF* spiPst = factory->createFixedTimeslotTask(
-            "PST_TASK_DEFAULT", 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;
-    }
-#endif
-
-    FixedTimeslotTaskIF* uartPst = factory->createFixedTimeslotTask(
-            "UART_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
-    result = pst::pstUart(uartPst);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
-    }
-    FixedTimeslotTaskIF* gpioPst = factory->createFixedTimeslotTask(
-            "GPIO_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
-    result = pst::pstGpio(gpioPst);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
-    }
-    FixedTimeslotTaskIF* i2cPst = factory->createFixedTimeslotTask(
-            "I2C_PST", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 3.0, missedDeadlineFunc);
-    result = pst::pstI2c(i2cPst);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        sif::error << "InitMission::initTasks: Creating PST failed!" << std::endl;
-    }
-
-    FixedTimeslotTaskIF* gomSpacePstTask = factory->createFixedTimeslotTask(
-            "GS_PST_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc);
-    result = pst::pstGompaceCan(gomSpacePstTask);
-    if(result != HasReturnvaluesIF::RETURN_OK) {
-        sif::error << "InitMission::initTasks: GomSpace PST initialization failed!" << std::endl;
-    }
-
-#else /* BOARD_TE7020 == 0 */
-    FixedTimeslotTaskIF * pollingSequenceTaskTE0720 = factory->createFixedTimeslotTask(
-            "PST_TASK_TE0720", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE * 8, 3.0,
-            missedDeadlineFunc);
-    result = pst::pollingSequenceTE0720(pollingSequenceTaskTE0720);
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        sif::error << "InitMission::initTasks: Creating TE0720 PST failed!" << std::endl;
-    }
-#endif /* BOARD_TE7020 == 1 */
-
-#if OBSW_ADD_TEST_CODE == 1
-    PeriodicTaskIF* testTask = factory->createPeriodicTask(
+void initmission::createTestTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc,
+        std::vector<PeriodicTaskIF*>& taskVec) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    PeriodicTaskIF* testTask = factory.createPeriodicTask(
             "TEST_TASK", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1, missedDeadlineFunc);
     result = testTask->addComponent(objects::TEST_TASK);
     if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -225,45 +281,11 @@ void initmission::initTasks() {
         initmission::printAddObjectError("SPI_TEST", objects::SPI_TEST);
     }
 #endif
-#endif /* OBSW_ADD_TEST_CODE == 1 */
 #if BOARD_TE0720 == 1 && TEST_LIBGPIOD == 1
     result = testTask->addComponent(objects::LIBGPIOD_TEST);
     if(result != HasReturnvaluesIF::RETURN_OK) {
         initmission::printAddObjectError("GPIOD_TEST", objects::LIBGPIOD_TEST);
     }
-#endif /* TE0720 == 1 && TEST_LIBGPIOD == 1 */
-
-    sif::info << "Starting tasks.." << std::endl;
-    tmTcDistributor->startTask();
-    udpBridgeTask->startTask();
-    udpPollingTask->startTask();
-    coreController->startTask();
-
-#if BOARD_TE0720 == 0
-    uartPst->startTask();
-    gpioPst->startTask();
-    i2cPst->startTask();
-#if Q7S_ADD_SPI_TEST == 0
-    gomSpacePstTask->startTask();
-    spiPst->startTask();
-#endif /* Q7S_ADD_SPI_TEST == 0 */
-
-#elif BOARD_TE0720 == 1 && Q7S_ADD_SPI_TEST == 0
-    pollingSequenceTaskTE0720->startTask();
-#endif
-
-    pusVerification->startTask();
-    pusEvents->startTask();
-    pusHighPrio->startTask();
-    pusMedPrio->startTask();
-    pusLowPrio->startTask();
-
-#if OBSW_ADD_TEST_CODE == 1
-    testTask->startTask();
-#endif
-
-#if TEST_CCSDS_BRIDGE == 1
-    ptmeTestTask->startTask();
-#endif
-    sif::info << "Tasks started.." << std::endl;
+#endif /* BOARD_TE0720 == 1 && TEST_LIBGPIOD == 1 */
+    taskVec.push_back(testTask);
 }
diff --git a/bsp_q7s/core/InitMission.h b/bsp_q7s/core/InitMission.h
index 568b26b5..ffdfc11c 100644
--- a/bsp_q7s/core/InitMission.h
+++ b/bsp_q7s/core/InitMission.h
@@ -1,9 +1,22 @@
 #ifndef BSP_Q7S_INITMISSION_H_
 #define BSP_Q7S_INITMISSION_H_
 
+#include "fsfw/tasks/Typedef.h"
+#include <vector>
+
+class PeriodicTaskIF;
+class TaskFactory;
+
 namespace initmission {
 void initMission();
 void initTasks();
+
+void createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc,
+        std::vector<PeriodicTaskIF*>& taskVec);
+void createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc,
+        std::vector<PeriodicTaskIF*>& taskVec);
+void createTestTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc,
+        std::vector<PeriodicTaskIF*>& taskVec);
 };
 
 #endif /* BSP_Q7S_INITMISSION_H_ */
diff --git a/fsfw b/fsfw
index d11e54dc..10f7185e 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit d11e54dc0a296bda86b5597a34e5e8ba59b9653e
+Subproject commit 10f7185e8137b3674a7ee5c24dfbaf8035d2251d