diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/ObjectFactory.cpp
index 743ee03e..acc0be37 100644
--- a/bsp_hosted/ObjectFactory.cpp
+++ b/bsp_hosted/ObjectFactory.cpp
@@ -14,7 +14,7 @@
 #include <fsfw/osal/common/UdpTcPollingTask.h>
 #include <fsfw/osal/common/UdpTmTcBridge.h>
 
-#include <fsfw/tmtcpacket/pus/TmPacketStored.h>
+#include <fsfw/tmtcpacket/pus/tm.h>
 
 
 #if OBSW_ADD_TEST_CODE == 1
diff --git a/bsp_hosted/fsfwconfig/FSFWConfig.h.in b/bsp_hosted/fsfwconfig/FSFWConfig.h.in
index e48895d9..aa3cefb0 100644
--- a/bsp_hosted/fsfwconfig/FSFWConfig.h.in
+++ b/bsp_hosted/fsfwconfig/FSFWConfig.h.in
@@ -67,6 +67,8 @@ static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
 
 static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
 
+static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048;
+
 }
 
 #endif /* CONFIG_FSFWCONFIG_H_ */
diff --git a/bsp_linux_board/InitMission.cpp b/bsp_linux_board/InitMission.cpp
index a94c2e7d..16dab475 100644
--- a/bsp_linux_board/InitMission.cpp
+++ b/bsp_linux_board/InitMission.cpp
@@ -163,6 +163,12 @@ void initmission::initTasks() {
         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 */
 
     sif::info << "Starting tasks.." << std::endl;
     tmTcDistributor->startTask();
diff --git a/bsp_linux_board/ObjectFactory.cpp b/bsp_linux_board/ObjectFactory.cpp
index 648d1376..d6dfd623 100644
--- a/bsp_linux_board/ObjectFactory.cpp
+++ b/bsp_linux_board/ObjectFactory.cpp
@@ -8,32 +8,32 @@
 #include "tmtc/pusIds.h"
 #include "spiConf.h"
 
-#include <linux/boardtest/LibgpiodTest.h>
-#include <linux/boardtest/SpiTestClass.h>
+#include "linux/boardtest/LibgpiodTest.h"
+#include "linux/boardtest/SpiTestClass.h"
+#include "linux/boardtest/UartTestClass.h"
 
-//#include <mission/devices/GyroL3GD20Handler.h>
-#include <mission/core/GenericFactory.h>
-#include <mission/utility/TmFunnel.h>
-#include <mission/devices/MGMHandlerLIS3MDL.h>
-#include <mission/devices/MGMHandlerRM3100.h>
-#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
-#include <mission/devices/GyroADIS16507Handler.h>
+#include "mission/core/GenericFactory.h"
+#include "mission/utility/TmFunnel.h"
+#include "mission/devices/MGMHandlerLIS3MDL.h"
+#include "mission/devices/MGMHandlerRM3100.h"
+#include "mission/devices/GyroADIS16507Handler.h"
 
-#include <fsfw/datapoollocal/LocalDataPoolManager.h>
-#include <fsfw/tmtcservices/CommandingServiceBase.h>
-#include <fsfw/tmtcservices/PusServiceBase.h>
-#include <fsfw/tmtcpacket/pus/TmPacketStored.h>
-#include <fsfw/tasks/TaskFactory.h>
+#include "fsfw/datapoollocal/LocalDataPoolManager.h"
+#include "fsfw/tmtcservices/CommandingServiceBase.h"
+#include "fsfw/tmtcservices/PusServiceBase.h"
+#include "fsfw/tmtcpacket/pus/tm.h"
+#include "fsfw/tasks/TaskFactory.h"
 
 /* UDP server includes */
-#include <fsfw/osal/common/UdpTmTcBridge.h>
-#include <fsfw/osal/common/UdpTcPollingTask.h>
+#include "fsfw/osal/common/UdpTmTcBridge.h"
+#include "fsfw/osal/common/UdpTcPollingTask.h"
 
-#include <fsfw_hal/linux/gpio/LinuxLibgpioIF.h>
-#include <fsfw_hal/linux/rpi/GpioRPi.h>
-#include <fsfw_hal/common/gpio/GpioCookie.h>
-#include <fsfw_hal/linux/spi/SpiCookie.h>
-#include <fsfw_hal/linux/spi/SpiComIF.h>
+#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
+#include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h"
+#include "fsfw_hal/linux/rpi/GpioRPi.h"
+#include "fsfw_hal/common/gpio/GpioCookie.h"
+#include "fsfw_hal/linux/spi/SpiCookie.h"
+#include "fsfw_hal/linux/spi/SpiComIF.h"
 
 void Factory::setStaticFrameworkObjectIds() {
     PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR;
@@ -61,10 +61,15 @@ void ObjectFactory::produce(void* args){
 
     GpioIF* gpioIF = new LinuxLibgpioIF(objects::GPIO_IF);
     GpioCookie* gpioCookie = nullptr;
+    static_cast<void>(gpioCookie);
 #if RPI_ADD_SPI_TEST == 1
     new SpiTestClass(objects::SPI_TEST, gpioIF);
 #endif
 
+#if RPI_ADD_UART_TEST == 1
+    new UartTestClass(objects::UART_TEST);
+#endif
+
 #if RPI_LOOPBACK_TEST_GPIO == 1
     GpioCookie* gpioCookieLoopback = new GpioCookie();
     /* Loopback pins. Adapt according to setup */
@@ -83,6 +88,7 @@ void ObjectFactory::produce(void* args){
 
     std::string spiDev;
     SpiCookie* spiCookie = nullptr;
+    static_cast<void>(spiCookie);
 
 #if RPI_TEST_ACS_BOARD == 1
     if(gpioCookie == nullptr) {
diff --git a/bsp_linux_board/boardconfig/rpiConfig.h.in b/bsp_linux_board/boardconfig/rpiConfig.h.in
index 31d6c157..0094903c 100644
--- a/bsp_linux_board/boardconfig/rpiConfig.h.in
+++ b/bsp_linux_board/boardconfig/rpiConfig.h.in
@@ -6,11 +6,18 @@
 #define RPI_ADD_GPIO_TEST               0
 #define RPI_LOOPBACK_TEST_GPIO          0
 
-#define RPI_TEST_ADIS16507              1
+#define RPI_TEST_ADIS16507              0
 
-/* Only one of those 2 should be enabled! */
+// Only one of those 2 should be enabled!
 #define RPI_ADD_SPI_TEST                0
+#if RPI_ADD_SPI_TEST == 0
 #define RPI_TEST_ACS_BOARD              0
+#endif
+
+#define RPI_ADD_UART_TEST               1
+#if RPI_ADD_UART_TEST == 1
+#define RPI_TEST_GPS_DEVICE             0
+#endif
 
 /* Adapt these values accordingly */
 namespace gpio {
diff --git a/bsp_q7s/ObjectFactory.cpp b/bsp_q7s/ObjectFactory.cpp
index 89a6f375..99ec65c2 100644
--- a/bsp_q7s/ObjectFactory.cpp
+++ b/bsp_q7s/ObjectFactory.cpp
@@ -41,6 +41,7 @@
 
 #include "fsfw_hal/linux/uart/UartComIF.h"
 #include "fsfw_hal/linux/uart/UartCookie.h"
+#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
 #include <fsfw_hal/linux/i2c/I2cCookie.h>
 #include <fsfw_hal/linux/i2c/I2cComIF.h>
 #include <fsfw_hal/linux/spi/SpiCookie.h>
@@ -51,7 +52,7 @@
 #include <fsfw/datapoollocal/LocalDataPoolManager.h>
 #include <fsfw/tmtcservices/CommandingServiceBase.h>
 #include <fsfw/tmtcservices/PusServiceBase.h>
-#include <fsfw/tmtcpacket/pus/TmPacketStored.h>
+#include <fsfw/tmtcpacket/pus/tm.h>
 
 /* UDP server includes */
 #include <fsfw/osal/common/UdpTmTcBridge.h>
@@ -390,8 +391,8 @@ void ObjectFactory::produce(void* args){
             solarArrayDeplCookie, objects::PCDU_HANDLER, pcduSwitches::DEPLOYMENT_MECHANISM,
             gpioIds::DEPLSA1, gpioIds::DEPLSA2, 1000);
 
-    UartCookie* syrlinksUartCookie = new UartCookie(
-            std::string("/dev/ttyUL0"), 38400, SYRLINKS::MAX_REPLY_SIZE);
+    UartCookie* syrlinksUartCookie = new UartCookie(objects::SYRLINKS_HK_HANDLER,
+            std::string("/dev/ttyUL0"), UartModes::NON_CANONICAL, 38400, SYRLINKS::MAX_REPLY_SIZE);
     syrlinksUartCookie->setParityEven();
 
     SyrlinksHkHandler* syrlinksHkHandler = new SyrlinksHkHandler(objects::SYRLINKS_HK_HANDLER,
@@ -543,8 +544,8 @@ void ObjectFactory::produce(void* args){
             std::string("/dev/i2c-0"));
     new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie);
 
-    UartCookie* plocUartCookie = new UartCookie(std::string("/dev/ttyUL3"), 115200,
-            PLOC::MAX_REPLY_SIZE);
+    UartCookie* plocUartCookie = new UartCookie(objects::PLOC_HANDLER, std::string("/dev/ttyUL3"),
+            UartModes::NON_CANONICAL, 115200, PLOC::MAX_REPLY_SIZE);
     PlocHandler* plocHandler = new PlocHandler(objects::PLOC_HANDLER, objects::UART_COM_IF,
             plocUartCookie);
 //    plocHandler->setStartUpImmediately();
diff --git a/cmake/scripts/Linux/create_cmake_debug_cfg.sh b/cmake/scripts/Linux/create_cmake_debug_cfg.sh
index 172f890d..1207884e 100755
--- a/cmake/scripts/Linux/create_cmake_debug_cfg.sh
+++ b/cmake/scripts/Linux/create_cmake_debug_cfg.sh
@@ -16,7 +16,7 @@ fi
 
 build_generator="Unix Makefiles"
 os_fsfw="linux"
-builddir="build-Debug-Linux"
+builddir="build-Debug-Host"
 
 echo "Running command (without the leading +):"
 set -x # Print command 
diff --git a/cmake/scripts/Linux/create_cmake_release_cfg.sh b/cmake/scripts/Linux/create_cmake_release_cfg.sh
index 019a951d..4412e671 100755
--- a/cmake/scripts/Linux/create_cmake_release_cfg.sh
+++ b/cmake/scripts/Linux/create_cmake_release_cfg.sh
@@ -16,7 +16,7 @@ fi
 
 build_generator="Unix Makefiles"
 os_fsfw="linux"
-builddir="build-Release-Linux"
+builddir="build-Release-Host"
 
 echo "Running command (without the leading +):"
 set -x # Print command 
diff --git a/fsfw b/fsfw
index d700fb55..cae69d54 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit d700fb551c85393b58a3ada96fbd3f807217d14b
+Subproject commit cae69d540097acba46bffa47fd7afc6a8a19bd15
diff --git a/fsfw_hal b/fsfw_hal
index 4cff1aa0..fce40ebf 160000
--- a/fsfw_hal
+++ b/fsfw_hal
@@ -1 +1 @@
-Subproject commit 4cff1aa0212e3c1eab3e2af41443fd19c3ffb8a6
+Subproject commit fce40ebf9a4a45bafedaee2fc87e5aa10e49fdcc
diff --git a/linux/boardtest/UartTestClass.cpp b/linux/boardtest/UartTestClass.cpp
index dc9f79c3..e69f0e5f 100644
--- a/linux/boardtest/UartTestClass.cpp
+++ b/linux/boardtest/UartTestClass.cpp
@@ -1,8 +1,114 @@
-#include <linux/boardtest/UartTestClass.h>
+#include "UartTestClass.h"
+#if defined(RASPBERRY_PI)
+#include "rpiConfig.h"
+#elif defined(XIPHOS_Q7S)
+#include "q7sConfig.h"
+#endif
+
+#include "fsfw/serviceinterface/ServiceInterface.h"
+
+#include "lwgps/lwgps.h"
+
+#include <fcntl.h> // Contains file controls like O_RDWR
+#include <errno.h> // Error integer and strerror() function
+#include <unistd.h> // write(), read(), close()
+
+#define GPS_REPLY_WIRETAPPING   0
 
 UartTestClass::UartTestClass(object_id_t objectId): TestTask(objectId) {
 }
 
-ReturnValue_t UartTestClass::performPeriodicAction() {
+ReturnValue_t UartTestClass::initialize() {
+#if RPI_TEST_GPS_DEVICE == 1
+    int result = lwgps_init(&gpsData);
+    if(result == 0) {
+        sif::warning << "lwgps_init error: " << result << std::endl;
+    }
+
+    /* Get file descriptor */
+    serialPort = open("/dev/serial0", O_RDWR);
+    if(serialPort < 0) {
+        sif::warning << "open call failed with error [" << errno << ", " << strerror(errno)
+                                        << std::endl;
+    }
+    /* Setting up UART parameters */
+    tty.c_cflag &= ~PARENB; // Clear parity bit
+    tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication
+    tty.c_cflag &= ~CSIZE; // Clear all the size bits
+    tty.c_cflag |= CS8; // 8 bits per byte
+    tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control
+    tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines (CLOCAL = 1)
+    // Use canonical mode for GPS device
+    tty.c_lflag |= ICANON;
+    tty.c_lflag &= ~ECHO; // Disable echo
+    tty.c_lflag &= ~ECHOE; // Disable erasure
+    tty.c_lflag &= ~ECHONL; // Disable new-line echo
+    tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
+    tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
+    tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // Disable any special handling of received bytes
+    tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
+    tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
+
+    // Non-blocking mode
+    tty.c_cc[VTIME] = 0;
+    tty.c_cc[VMIN] = 0;
+
+    cfsetispeed(&tty, B9600);
+    cfsetospeed(&tty, B9600);
+    if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
+        sif::warning << "tcsetattr call failed with error [" << errno << ", " <<
+                strerror(errno) << std::endl;;
+    }
+    // Flush received and unread data. Those are old NMEA strings which are not relevant anymore
+    tcflush(serialPort, TCIFLUSH);
+#endif
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t UartTestClass::performOneShotAction() {
+#if RPI_TEST_GPS_DEVICE == 1
+#endif
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t UartTestClass::performPeriodicAction() {
+#if RPI_TEST_GPS_DEVICE == 1
+    int bytesRead = 0;
+    do {
+        bytesRead = read(serialPort,
+                reinterpret_cast<void*>(recBuf.data()),
+                static_cast<unsigned int>(recBuf.size()));
+        if(bytesRead < 0) {
+            sif::warning << "UartTestClass::performPeriodicAction: read call failed with error [" <<
+                    errno << ", " << strerror(errno) << "]" << std::endl;
+            break;
+        }
+        else if(bytesRead >= static_cast<int>(recBuf.size())) {
+            sif::debug << "UartTestClass::performPeriodicAction: "
+                    "recv buffer might not be large enough" << std::endl;
+        }
+        else if(bytesRead > 0) {
+            // pass data to lwgps for processing
+#if GPS_REPLY_WIRETAPPING == 1
+            sif::info << recBuf.data() << std::endl;
+#endif
+            int result = lwgps_process(&gpsData, recBuf.data(), bytesRead);
+            if(result == 0) {
+                sif::warning << "UartTestClass::performPeriodicAction: lwgps_process error"
+                        << std::endl;
+            }
+            recvCnt++;
+            if(recvCnt == 6) {
+                recvCnt = 0;
+                sif::info << "GPS Data" << std::endl;
+                // Print messages
+                printf("Valid status: %d\n", gpsData.is_valid);
+                printf("Latitude: %f degrees\n", gpsData.latitude);
+                printf("Longitude: %f degrees\n", gpsData.longitude);
+                printf("Altitude: %f meters\n", gpsData.altitude);
+            }
+        }
+    } while(bytesRead > 0);
+#endif
     return HasReturnvaluesIF::RETURN_OK;
 }
diff --git a/linux/boardtest/UartTestClass.h b/linux/boardtest/UartTestClass.h
index 406d2af5..aeb2c396 100644
--- a/linux/boardtest/UartTestClass.h
+++ b/linux/boardtest/UartTestClass.h
@@ -1,15 +1,27 @@
 #ifndef LINUX_BOARDTEST_UARTTESTCLASS_H_
 #define LINUX_BOARDTEST_UARTTESTCLASS_H_
 
-#include <test/testtasks/TestTask.h>
+#include "test/testtasks/TestTask.h"
+#include "lwgps/lwgps.h"
+
+#include <array>
+#include <termios.h> // Contains POSIX terminal control definitions
 
 class UartTestClass: public TestTask {
 public:
     UartTestClass(object_id_t objectId);
 
+    ReturnValue_t initialize() override;
+    ReturnValue_t performOneShotAction() override;
     ReturnValue_t performPeriodicAction() override;
 private:
 
+    lwgps_t gpsData = {};
+    struct termios tty = {};
+    int serialPort = 0;
+    std::array<uint8_t, 512> recBuf;
+    uint8_t recvCnt = 0;
+
 };
 
 #endif /* LINUX_BOARDTEST_UARTTESTCLASS_H_ */
diff --git a/linux/fsfwconfig/FSFWConfig.h.in b/linux/fsfwconfig/FSFWConfig.h.in
index 8ec01cb9..a8519cff 100644
--- a/linux/fsfwconfig/FSFWConfig.h.in
+++ b/linux/fsfwconfig/FSFWConfig.h.in
@@ -18,6 +18,7 @@
 #endif
 
 #define FSFW_USE_PUS_C_TELEMETRY    1
+#define FSFW_USE_PUS_C_TELECOMMANDS 1
 
 //! Can be used to disable the ANSI color sequences for C stdio.
 #define FSFW_COLORED_OUTPUT 		1
@@ -66,6 +67,9 @@ static constexpr size_t FSFW_EVENTMGMR_RANGEMATCHERS   = 120;
 static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
 
 static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
+
+static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048;
+
 }
 
 #define FSFW_HAL_LINUX_SPI_WIRETAPPING  0
diff --git a/linux/fsfwconfig/objects/systemObjectList.h b/linux/fsfwconfig/objects/systemObjectList.h
index 98d6c448..5d5cdee1 100644
--- a/linux/fsfwconfig/objects/systemObjectList.h
+++ b/linux/fsfwconfig/objects/systemObjectList.h
@@ -62,6 +62,7 @@ namespace objects {
         TEST_TASK = 0x54694269,
         LIBGPIOD_TEST = 0x54123456,
         SPI_TEST = 0x54000010,
+        UART_TEST = 0x54000020,
         DUMMY_INTERFACE = 0x5400CAFE,
         DUMMY_HANDLER = 0x5400AFFE,
         P60DOCK_TEST_TASK = 0x00005060
diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
index 39cbae4d..60059d3c 100644
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@@ -560,7 +560,7 @@ ReturnValue_t pst::pollingSequenceTest(FixedTimeslotTaskIF* thisSequence) {
     thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
 #endif
-
+    static_cast<void>(length);
     if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) {
         return HasReturnvaluesIF::RETURN_OK;
     }
diff --git a/linux/fsfwconfig/returnvalues/classIds.h b/linux/fsfwconfig/returnvalues/classIds.h
index 77d79272..d1bfe7e2 100644
--- a/linux/fsfwconfig/returnvalues/classIds.h
+++ b/linux/fsfwconfig/returnvalues/classIds.h
@@ -12,8 +12,6 @@
 namespace CLASS_ID {
 enum {
 	CLASS_ID_START = COMMON_CLASS_ID_END,
-	LINUX_LIBGPIO_IF, //LINUXGPIO
-	LINUX_SPI_COM_IF, //LINUXSPI
 	SA_DEPL_HANDLER, //SADPL
 	CLASS_ID_END // [EXPORT] : [END]
 };
diff --git a/mission/devices/GPSHandler.cpp b/mission/devices/GPSHandler.cpp
index e53ce347..86733794 100644
--- a/mission/devices/GPSHandler.cpp
+++ b/mission/devices/GPSHandler.cpp
@@ -1,18 +1,35 @@
 #include "GPSHandler.h"
+#include "devicedefinitions/GPSDefinitions.h"
+
+#include "lwgps/lwgps.h"
 
 GPSHandler::GPSHandler(object_id_t objectId, object_id_t deviceCommunication,
 		CookieIF *comCookie):
 		DeviceHandlerBase(objectId, deviceCommunication, comCookie) {
+    lwgps_init(&gpsData);
 }
 
 GPSHandler::~GPSHandler() {}
 
 void GPSHandler::doStartUp() {
+    if(internalState == InternalStates::NONE) {
+        commandExecuted = false;
+        internalState = InternalStates::WAIT_FIRST_MESSAGE;
+    }
 
+    if(internalState == InternalStates::WAIT_FIRST_MESSAGE) {
+        if(commandExecuted) {
+            internalState = InternalStates::IDLE;
+            setMode(MODE_ON);
+            commandExecuted = false;
+        }
+    }
 }
 
 void GPSHandler::doShutDown() {
-
+    internalState = InternalStates::NONE;
+    commandExecuted = false;
+    setMode(MODE_OFF);
 }
 
 ReturnValue_t GPSHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
@@ -31,7 +48,14 @@ ReturnValue_t GPSHandler::buildCommandFromCommand(
 
 ReturnValue_t GPSHandler::scanForReply(const uint8_t *start, size_t len,
 		DeviceCommandId_t *foundId, size_t *foundLen) {
+    // Pass data to GPS library
+    int result = lwgps_process(&gpsData, start, len);
+    if(result != 0) {
+        sif::warning << "GPSHandler::scanForReply: Issue processing GPS data with lwgps"
+                << std::endl;
+    }
 	return HasReturnvaluesIF::RETURN_OK;
+
 }
 
 ReturnValue_t GPSHandler::interpretDeviceReply(DeviceCommandId_t id,
@@ -49,9 +73,10 @@ ReturnValue_t GPSHandler::initializeLocalDataPool(
 }
 
 void GPSHandler::fillCommandAndReplyMap() {
-
+    // Reply length does not matter, packets should always arrive periodically
+    insertInReplyMap(GpsHyperion::GPS_REPLY, 4, nullptr, 0, true);
 }
 
 void GPSHandler::modeChanged() {
-
+    internalState = InternalStates::NONE;
 }
diff --git a/mission/devices/GPSHandler.h b/mission/devices/GPSHandler.h
index 01a13591..298871e4 100644
--- a/mission/devices/GPSHandler.h
+++ b/mission/devices/GPSHandler.h
@@ -2,7 +2,14 @@
 #define MISSION_DEVICES_GPSHANDLER_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
+#include "lwgps/lwgps.h"
 
+/**
+ * @brief   Device handler for the Hyperion HT-GPS200 device
+ * @details
+ * Flight manual:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Hyperion_HT-GPS200
+ */
 class GPSHandler: public DeviceHandlerBase {
 public:
 	GPSHandler(object_id_t objectId, object_id_t deviceCommunication,
@@ -10,6 +17,13 @@ public:
 	virtual ~GPSHandler();
 
 protected:
+	enum class InternalStates {
+	    NONE,
+	    WAIT_FIRST_MESSAGE,
+	    IDLE
+	};
+	InternalStates internalState = InternalStates::NONE;
+	bool commandExecuted = false;
 
 	/* DeviceHandlerBase overrides */
 	ReturnValue_t buildTransitionDeviceCommand(
@@ -33,9 +47,7 @@ protected:
 			LocalDataPoolManager &poolManager) override;
 
 private:
-
+	lwgps_t gpsData = {};
 };
 
-
-
 #endif /* MISSION_DEVICES_GPSHANDLER_H_ */
diff --git a/mission/devices/GomspaceDeviceHandler.h b/mission/devices/GomspaceDeviceHandler.h
index 81c62502..8130d8f7 100644
--- a/mission/devices/GomspaceDeviceHandler.h
+++ b/mission/devices/GomspaceDeviceHandler.h
@@ -7,10 +7,13 @@
 /**
  * @brief	This is the device handler class for all gomspace devices.
  *
- * @details	All gomspace devices are similar with respect to commanding. Thus
- * 			most of the functionality to command a gomspace device can be
- * 			accommodated in one class. For device specific functions, a new
- * 			class could be created by inheriting from the GomspaceDeviceHandler.
+ * @details
+ * All gomspace devices are similar with respect to commanding. Thusmost of the functionality to
+ * command a gomspace device can be accommodated in one class. For device specific functions, a new
+ * class could be created by inheriting from the GomspaceDeviceHandler.
+ *
+ * Flight manual:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/Gomspace_PCDU_P60_System
  */
 class GomspaceDeviceHandler: public DeviceHandlerBase {
 public:
diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp
index 2c3ed5a4..ac49a89d 100644
--- a/mission/devices/GyroADIS16507Handler.cpp
+++ b/mission/devices/GyroADIS16507Handler.cpp
@@ -1,10 +1,10 @@
 #include <fsfw/action/HasActionsIF.h>
 #include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw_hal/linux/utility.h>
 
 #include "GyroADIS16507Handler.h"
 
 #if OBSW_ADIS16507_LINUX_COM_IF == 1
+#include "fsfw_hal/linux/utility.h"
 #include "fsfw_hal/linux/spi/SpiCookie.h"
 #include "fsfw_hal/linux/spi/SpiComIF.h"
 #include "fsfw_hal/linux/UnixFileGuard.h"
diff --git a/mission/devices/GyroADIS16507Handler.h b/mission/devices/GyroADIS16507Handler.h
index 653f56e7..faab3e5d 100644
--- a/mission/devices/GyroADIS16507Handler.h
+++ b/mission/devices/GyroADIS16507Handler.h
@@ -11,6 +11,12 @@ class SpiComIF;
 class SpiCookie;
 #endif
 
+/**
+ * @brief   Device handle for the ADIS16507 Gyroscope by Analog Devices
+ * @details
+ * Flight manual:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro
+ */
 class GyroADIS16507Handler: public DeviceHandlerBase {
 public:
     GyroADIS16507Handler(object_id_t objectId, object_id_t deviceCommunication,
diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h
index fc998f46..3cc08a6b 100644
--- a/mission/devices/MGMHandlerLIS3MDL.h
+++ b/mission/devices/MGMHandlerLIS3MDL.h
@@ -14,6 +14,8 @@
  *          by STMicroeletronics
  * @details
  * Datasheet can be found online by googling LIS3MDL.
+ * Flight manual:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/LIS3MDL_MGM
  * @author  L. Loidold, R. Mueller
  */
 class MGMHandlerLIS3MDL: public DeviceHandlerBase {
diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h
index ce107637..95175b47 100644
--- a/mission/devices/MGMHandlerRM3100.h
+++ b/mission/devices/MGMHandlerRM3100.h
@@ -14,7 +14,7 @@
  * @brief 	Device Handler for the RM3100 geomagnetic magnetometer sensor
  *          (https://www.pnicorp.com/rm3100/)
  * @details
- * Advanced documentation:
+ * Flight manual:
  * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM
  */
 class MGMHandlerRM3100: public DeviceHandlerBase {
diff --git a/mission/devices/PlocHandler.h b/mission/devices/PlocHandler.h
index c92b3705..d400318e 100644
--- a/mission/devices/PlocHandler.h
+++ b/mission/devices/PlocHandler.h
@@ -8,9 +8,11 @@
 /**
  * @brief	This is the device handler for the PLOC.
  *
- * @details The PLOC uses the space packet protocol for communication. To each command the PLOC
- *          answers with at least one acknowledgment and one execution report.
- *
+ * @details
+ * The PLOC uses the space packet protocol for communication. To each command the PLOC
+ * answers with at least one acknowledgment and one execution report.
+ * Flight manual:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/PLOC_Commands
  * @author	J. Meier
  */
 class PlocHandler: public DeviceHandlerBase {
diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h
new file mode 100644
index 00000000..cb3cfd1a
--- /dev/null
+++ b/mission/devices/devicedefinitions/GPSDefinitions.h
@@ -0,0 +1,21 @@
+#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_
+#define MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_
+
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+
+namespace GpsHyperion {
+
+static constexpr DeviceCommandId_t GPS_REPLY = 0;
+
+enum GpsPoolIds: lp_id_t {
+
+};
+
+}
+
+class GpsPrimaryDataset: public StaticLocalDataSet<5> {
+public:
+private:
+};
+
+#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GPSDEFINITIONS_H_ */
diff --git a/mission/utility/TmFunnel.cpp b/mission/utility/TmFunnel.cpp
index 72c41153..c52fa6f9 100644
--- a/mission/utility/TmFunnel.cpp
+++ b/mission/utility/TmFunnel.cpp
@@ -1,8 +1,7 @@
 #include <fsfw/ipc/QueueFactory.h>
-#include <fsfw/tmtcpacket/pus/TmPacketStored.h>
+#include <fsfw/tmtcpacket/pus/tm.h>
 #include <fsfw/objectmanager/ObjectManager.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
-#include <fsfw/tmtcpacket/pus/TmPacketPusC.h>
 #include <mission/utility/TmFunnel.h>
 
 object_id_t TmFunnel::downlinkDestination = objects::NO_OBJECT;
diff --git a/tmtc b/tmtc
index 8a8c77ed..20a44b4b 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 8a8c77ed013f12b178e32b77d2cac9db3dd29c1f
+Subproject commit 20a44b4b67198d42e83a790191ad6740d0aabf7c