diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h
index 53182058..c6f21195 100644
--- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h
+++ b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h
@@ -9,12 +9,13 @@ using gpioId_t = uint16_t;
 
 namespace gpio {
 
-enum Levels {
+enum Levels: uint8_t {
     LOW = 0,
-    HIGH = 1
+    HIGH = 1,
+    NONE = 99
 };
 
-enum Direction {
+enum Direction: uint8_t {
     IN = 0,
     OUT = 1
 };
@@ -24,7 +25,7 @@ enum GpioOperation {
     WRITE
 };
 
-enum GpioTypes {
+enum class GpioTypes {
     NONE,
     GPIO_REGULAR_BY_CHIP,
     GPIO_REGULAR_BY_LABEL,
@@ -34,7 +35,8 @@ enum GpioTypes {
 
 static constexpr gpioId_t NO_GPIO = -1;
 
-using gpio_cb_t = void (*) (gpioId_t gpioId, gpio::GpioOperation gpioOp, int value, void* args);
+using gpio_cb_t = void (*) (gpioId_t gpioId, gpio::GpioOperation gpioOp, gpio::Levels value,
+        void* args);
 
 }
 
@@ -58,7 +60,7 @@ public:
     GpioBase() = default;
 
     GpioBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction,
-            int initValue):
+            gpio::Levels initValue):
             gpioType(gpioType), consumer(consumer),direction(direction), initValue(initValue) {}
 
     virtual~ GpioBase() {};
@@ -67,19 +69,19 @@ public:
     gpio::GpioTypes gpioType = gpio::GpioTypes::NONE;
     std::string consumer;
     gpio::Direction direction = gpio::Direction::IN;
-    int initValue = 0;
+    gpio::Levels initValue = gpio::Levels::NONE;
 };
 
 class GpiodRegularBase: public GpioBase {
 public:
     GpiodRegularBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction,
-            int initValue, int lineNum): GpioBase(gpioType, consumer, direction, initValue),
-            lineNum(lineNum) {
+            gpio::Levels initValue, int lineNum):
+            GpioBase(gpioType, consumer, direction, initValue), lineNum(lineNum) {
     }
 
     // line number will be configured at a later point for the open by line name configuration
     GpiodRegularBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction,
-            int initValue): GpioBase(gpioType, consumer, direction, initValue) {
+            gpio::Levels initValue): GpioBase(gpioType, consumer, direction, initValue) {
     }
 
     int lineNum = 0;
@@ -94,7 +96,7 @@ public:
     }
 
     GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_,
-            gpio::Direction direction_, int initValue_) :
+            gpio::Direction direction_, gpio::Levels initValue_) :
             GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP,
                     consumer_, direction_, initValue_, lineNum_),
             chipname(chipname_){
@@ -112,7 +114,7 @@ public:
 class GpiodRegularByLabel: public GpiodRegularBase {
 public:
     GpiodRegularByLabel(std::string label_, int lineNum_, std::string consumer_,
-            gpio::Direction direction_, int initValue_) :
+            gpio::Direction direction_, gpio::Levels initValue_) :
             GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_,
                     direction_, initValue_, lineNum_),
             label(label_) {
@@ -135,14 +137,14 @@ public:
 class GpiodRegularByLineName: public GpiodRegularBase {
 public:
     GpiodRegularByLineName(std::string lineName_, std::string consumer_, gpio::Direction direction_,
-            int initValue_) :
+            gpio::Levels initValue_) :
             GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, direction_,
                     initValue_), lineName(lineName_) {
     }
 
     GpiodRegularByLineName(std::string lineName_, std::string consumer_) :
-            GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, gpio::Direction::IN,
-                    gpio::LOW), lineName(lineName_) {
+            GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_,
+                    gpio::Direction::IN, gpio::LOW), lineName(lineName_) {
     }
 
     std::string lineName;
@@ -150,7 +152,7 @@ public:
 
 class GpioCallback: public GpioBase {
 public:
-    GpioCallback(std::string consumer, gpio::Direction direction_, int initValue_,
+    GpioCallback(std::string consumer, gpio::Direction direction_, gpio::Levels initValue_,
             gpio::gpio_cb_t callback, void* callbackArgs):
             GpioBase(gpio::GpioTypes::CALLBACK, consumer, direction_, initValue_),
             callback(callback), callbackArgs(callbackArgs) {}
diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
index 70ba49eb..d27351d7 100644
--- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
+++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
@@ -1,7 +1,9 @@
-#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
+#include "GyroL3GD20Handler.h"
 
 #include "fsfw/datapool/PoolReadGuard.h"
 
+#include <cmath>
+
 GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication,
         CookieIF *comCookie, uint32_t transitionDelayMs):
         DeviceHandlerBase(objectId, deviceCommunication, comCookie),
diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
index 870f551d..e73d2fbb 100644
--- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
+++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
@@ -59,7 +59,6 @@ protected:
             LocalDataPoolManager &poolManager) override;
 
 private:
-    uint8_t switchId = 0;
     uint32_t transitionDelayMs = 0;
     GyroPrimaryDataset dataset;
 
diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
index a0059451..1a61bfe2 100644
--- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
+++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
@@ -5,6 +5,8 @@
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 #endif
 
+#include <cmath>
+
 MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCommunication,
         CookieIF* comCookie, uint32_t transitionDelay):
         DeviceHandlerBase(objectId, deviceCommunication, comCookie),
diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
index 20cf95d2..124eebbc 100644
--- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
+++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp
@@ -89,9 +89,16 @@ ReturnValue_t MgmRM3100Handler::buildTransitionDeviceCommand(
         break;
     }
     default:
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         // Might be a configuration error
-        sif::warning << "MgmRM3100Handler::buildTransitionDeviceCommand: Unknown internal state!" <<
-                std::endl;
+        sif::warning << "MgmRM3100Handler::buildTransitionDeviceCommand: "
+                "Unknown internal state" << std::endl;
+#else
+        sif::printWarning("MgmRM3100Handler::buildTransitionDeviceCommand: "
+                "Unknown internal state\n");
+#endif
+#endif
         return HasReturnvaluesIF::RETURN_OK;
     }
 
@@ -342,12 +349,18 @@ ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) {
 
 #if FSFW_HAL_RM3100_MGM_DEBUG == 1
     if(debugDivider->checkAndIncrement()) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "MgmRM3100Handler: Magnetic field strength in"
                 " microtesla:" << std::endl;
-        /* Set terminal to utf-8 if there is an issue with micro printout. */
         sif::info << "X: " << fieldStrengthX << " uT" << std::endl;
         sif::info << "Y: " << fieldStrengthY << " uT" << std::endl;
         sif::info << "Z: " << fieldStrengthZ << " uT" << std::endl;
+#else
+        sif::printInfo("MgmRM3100Handler: Magnetic field strength in microtesla:\n");
+        sif::printInfo("X: %f uT\n", fieldStrengthX);
+        sif::printInfo("Y: %f uT\n", fieldStrengthY);
+        sif::printInfo("Z: %f uT\n", fieldStrengthZ);
+#endif
     }
 #endif
 
diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h
index b6f3fd84..9d65aae2 100644
--- a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h
+++ b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h
@@ -154,7 +154,7 @@ enum MgmPoolIds: lp_id_t {
     TEMPERATURE_CELCIUS
 };
 
-class MgmPrimaryDataset: public StaticLocalDataSet<5> {
+class MgmPrimaryDataset: public StaticLocalDataSet<4> {
 public:
     MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner):
         StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {}
diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h
index 08f80dd9..0ee2c7f6 100644
--- a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h
+++ b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h
@@ -108,7 +108,7 @@ enum MgmPoolIds: lp_id_t {
     FIELD_STRENGTH_Z,
 };
 
-class Rm3100PrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
+class Rm3100PrimaryDataset: public StaticLocalDataSet<3> {
 public:
 	Rm3100PrimaryDataset(HasLocalDataPoolIF* hkOwner):
         StaticLocalDataSet(hkOwner, MGM_DATASET_ID) {}
diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp
index 43cd63ce..004e1e7f 100644
--- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp
+++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp
@@ -1,8 +1,9 @@
-#include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h"
+#include "LinuxLibgpioIF.h"
+
 #include "fsfw_hal/common/gpio/gpioDefinitions.h"
 #include "fsfw_hal/common/gpio/GpioCookie.h"
 
-#include <fsfw/serviceinterface/ServiceInterface.h>
+#include "fsfw/serviceinterface/ServiceInterface.h"
 
 #include <utility>
 #include <unistd.h>
@@ -122,7 +123,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByLineName(gpioId_t gpioId,
 
     int result = gpiod_ctxless_find_line(lineName.c_str(), chipname, MAX_CHIPNAME_LENGTH,
             &lineOffset);
-     if (result != LINE_FOUND) {
+    if (result != LINE_FOUND) {
         parseFindeLineResult(result, lineName);
         return RETURN_FAILED;
     }
@@ -140,7 +141,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByLineName(gpioId_t gpioId,
 }
 
 ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod_chip* chip,
-    GpiodRegularBase& regularGpio, std::string failOutput) {
+        GpiodRegularBase& regularGpio, std::string failOutput) {
     unsigned int lineNum;
     gpio::Direction direction;
     std::string consumer;
@@ -165,22 +166,10 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod
     case(gpio::OUT): {
         result = gpiod_line_request_output(lineHandle, consumer.c_str(),
                 regularGpio.initValue);
-        if (result < 0) {
-            sif::error << "LinuxLibgpioIF::configureRegularGpio: Failed to request line " << lineNum <<
-                    " from GPIO instance with ID: " << gpioId << std::endl;
-            gpiod_line_release(lineHandle);
-            return RETURN_FAILED;
-        }
         break;
     }
     case(gpio::IN): {
         result = gpiod_line_request_input(lineHandle, consumer.c_str());
-        if (result < 0) {
-            sif::error << "LinuxLibgpioIF::configureGpios: Failed to request line "
-                    << lineNum << " from GPIO instance with ID: " << gpioId << std::endl;
-            gpiod_line_release(lineHandle);
-            return RETURN_FAILED;
-        }
         break;
     }
     default: {
@@ -189,6 +178,18 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod
         return GPIO_INVALID_INSTANCE;
     }
 
+    if (result < 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "LinuxLibgpioIF::configureRegularGpio: Failed to request line " <<
+                lineNum << " from GPIO instance with ID: " << gpioId << std::endl;
+#else
+        sif::printError("LinuxLibgpioIF::configureRegularGpio: "
+                "Failed to request line %d from GPIO instance with ID: %d\n", lineNum, gpioId);
+#endif
+        gpiod_line_release(lineHandle);
+        return RETURN_FAILED;
+    }
+
     }
     /**
      * Write line handle to GPIO configuration instance so it can later be used to set or
@@ -221,7 +222,7 @@ ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) {
             return GPIO_INVALID_INSTANCE;
         }
         gpioCallback->callback(gpioMapIter->first, gpio::GpioOperation::WRITE,
-                1, gpioCallback->callbackArgs);
+                gpio::Levels::HIGH, gpioCallback->callbackArgs);
         return RETURN_OK;
     }
     return GPIO_TYPE_FAILURE;
@@ -230,7 +231,11 @@ ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) {
 ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) {
     gpioMapIter = gpioMap.find(gpioId);
     if (gpioMapIter == gpioMap.end()) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::warning << "LinuxLibgpioIF::pullLow: Unknown GPIO ID " << gpioId << std::endl;
+#else
+        sif::printWarning("LinuxLibgpioIF::pullLow: Unknown GPIO ID %d\n", gpioId);
+#endif
         return UNKNOWN_GPIO_ID;
     }
 
@@ -250,7 +255,7 @@ ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) {
             return GPIO_INVALID_INSTANCE;
         }
         gpioCallback->callback(gpioMapIter->first, gpio::GpioOperation::WRITE,
-                0, gpioCallback->callbackArgs);
+                gpio::Levels::LOW, gpioCallback->callbackArgs);
         return RETURN_OK;
     }
     return GPIO_TYPE_FAILURE;
@@ -260,8 +265,13 @@ ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId,
         GpiodRegularBase& regularGpio, gpio::Levels logicLevel) {
     int result = gpiod_line_set_value(regularGpio.lineHandle, logicLevel);
     if (result < 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::warning << "LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID " << gpioId <<
                 " to logic level " << logicLevel << std::endl;
+#else
+        sif::printWarning("LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID %d to "
+                "logic level %d\n", gpioId, logicLevel);
+#endif
         return DRIVE_GPIO_FAILURE;
     }
 
@@ -271,7 +281,11 @@ ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId,
 ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) {
     gpioMapIter = gpioMap.find(gpioId);
     if (gpioMapIter == gpioMap.end()){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::warning << "LinuxLibgpioIF::readGpio: Unknown GPIOD ID " << gpioId << std::endl;
+#else
+        sif::printWarning("LinuxLibgpioIF::readGpio: Unknown GPIOD ID %d\n", gpioId);
+#endif
         return UNKNOWN_GPIO_ID;
     }
 
@@ -286,10 +300,14 @@ ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) {
         *gpioState = gpiod_line_get_value(regularGpio->lineHandle);
     }
     else {
-
+        auto gpioCallback = dynamic_cast<GpioCallback*>(gpioMapIter->second);
+        if(gpioCallback->callback == nullptr) {
+            return GPIO_INVALID_INSTANCE;
+        }
+        gpioCallback->callback(gpioMapIter->first, gpio::GpioOperation::READ,
+                gpio::Levels::NONE, gpioCallback->callbackArgs);
+        return RETURN_OK;
     }
-
-
     return RETURN_OK;
 }
 
@@ -299,13 +317,14 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd){
     for(auto& gpioConfig: mapToAdd) {
         switch(gpioConfig.second->gpioType) {
         case(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP):
-        case(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): {
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL):
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME): {
             auto regularGpio = dynamic_cast<GpiodRegularBase*>(gpioConfig.second);
             if(regularGpio == nullptr)  {
                 return GPIO_TYPE_FAILURE;
             }
-            /* Check for conflicts and remove duplicates if necessary */
-            result = checkForConflictsRegularGpio(gpioConfig.first, *regularGpio, mapToAdd);
+            // Check for conflicts and remove duplicates if necessary
+            result = checkForConflictsById(gpioConfig.first, gpioConfig.second->gpioType, mapToAdd);
             if(result != HasReturnvaluesIF::RETURN_OK) {
                 status = result;
             }
@@ -316,15 +335,22 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd){
             if(callbackGpio == nullptr)  {
                 return GPIO_TYPE_FAILURE;
             }
-            /* Check for conflicts and remove duplicates if necessary */
-            result = checkForConflictsCallbackGpio(gpioConfig.first, callbackGpio, mapToAdd);
+            // Check for conflicts and remove duplicates if necessary
+            result = checkForConflictsById(gpioConfig.first,
+                    gpioConfig.second->gpioType, mapToAdd);
             if(result != HasReturnvaluesIF::RETURN_OK) {
                 status = result;
             }
             break;
         }
         default: {
-
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::warning << "Invalid GPIO type detected for GPIO ID " << gpioConfig.first
+                    << std::endl;
+#else
+            sif::printWarning("Invalid GPIO type detected for GPIO ID %d\n", gpioConfig.first);
+#endif
+            status = GPIO_TYPE_FAILURE;
         }
         }
     }
@@ -332,68 +358,86 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd){
 }
 
 
-ReturnValue_t LinuxLibgpioIF::checkForConflictsRegularGpio(gpioId_t gpioIdToCheck,
-        GpiodRegularBase& gpioToCheck, GpioMap& mapToAdd) {
-    /* Cross check with private map */
+ReturnValue_t LinuxLibgpioIF::checkForConflictsById(gpioId_t gpioIdToCheck,
+        gpio::GpioTypes expectedType, GpioMap& mapToAdd) {
+    // Cross check with private map
     gpioMapIter = gpioMap.find(gpioIdToCheck);
     if(gpioMapIter != gpioMap.end()) {
         auto& gpioType = gpioMapIter->second->gpioType;
-        if(gpioType != gpio::GpioTypes::GPIO_REGULAR_BY_CHIP and
-                gpioType != gpio::GpioTypes::GPIO_REGULAR_BY_LABEL) {
-            sif::warning << "LinuxLibgpioIF::checkForConflicts: ID already exists for different "
-                    "GPIO type" << gpioIdToCheck << ". Removing duplicate." << std::endl;
+        bool eraseDuplicateDifferentType = false;
+        switch(expectedType) {
+        case(gpio::GpioTypes::NONE): {
+            break;
+        }
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP):
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL):
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME): {
+            if(gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::CALLBACK) {
+                eraseDuplicateDifferentType = true;
+            }
+            break;
+        }
+        case(gpio::GpioTypes::CALLBACK): {
+            if(gpioType != gpio::GpioTypes::CALLBACK) {
+                eraseDuplicateDifferentType = true;
+            }
+        }
+        }
+        if(eraseDuplicateDifferentType) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::warning << "LinuxLibgpioIF::checkForConflicts: ID already exists for "
+                    "different GPIO type " << gpioIdToCheck <<
+                    ". Removing duplicate from map to add" << std::endl;
+#else
+            sif::printWarning("LinuxLibgpioIF::checkForConflicts: ID already exists for "
+                    "different GPIO type %d. Removing duplicate from map to add\n", gpioIdToCheck);
+#endif
             mapToAdd.erase(gpioIdToCheck);
-            return HasReturnvaluesIF::RETURN_OK;
-        }
-        auto ownRegularGpio = dynamic_cast<GpiodRegularBase*>(gpioMapIter->second);
-        if(ownRegularGpio == nullptr) {
-            return GPIO_TYPE_FAILURE;
+            return GPIO_DUPLICATE_DETECTED;
         }
 
-        /* Remove element from map to add because a entry for this GPIO
-        already exists */
-        sif::warning << "LinuxLibgpioIF::checkForConflictsRegularGpio: Duplicate GPIO definition"
-                << " detected. Duplicate will be removed from map to add." << std::endl;
-        mapToAdd.erase(gpioIdToCheck);
-    }
-    return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t LinuxLibgpioIF::checkForConflictsCallbackGpio(gpioId_t gpioIdToCheck,
-        GpioCallback *callbackGpio, GpioMap& mapToAdd) {
-    /* Cross check with private map */
-    gpioMapIter = gpioMap.find(gpioIdToCheck);
-    if(gpioMapIter != gpioMap.end()) {
-        if(gpioMapIter->second->gpioType != gpio::GpioTypes::CALLBACK) {
-            sif::warning << "LinuxLibgpioIF::checkForConflicts: ID already exists for different "
-                    "GPIO type" << gpioIdToCheck << ". Removing duplicate." << std::endl;
-            mapToAdd.erase(gpioIdToCheck);
-            return HasReturnvaluesIF::RETURN_OK;
-        }
-
-        /* Remove element from map to add because a entry for this GPIO
-        already exists */
-        sif::warning << "LinuxLibgpioIF::checkForConflictsRegularGpio: Duplicate GPIO definition"
-                << " detected. Duplicate will be removed from map to add." << std::endl;
+        // Remove element from map to add because a entry for this GPIO already exists
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "LinuxLibgpioIF::checkForConflictsRegularGpio: Duplicate GPIO "
+                "definition with ID " << gpioIdToCheck << " detected. " <<
+                "Duplicate will be removed from map to add" << std::endl;
+#else
+        sif::printWarning("LinuxLibgpioIF::checkForConflictsRegularGpio: Duplicate GPIO definition "
+                "with ID %d detected. Duplicate will be removed from map to add\n", gpioIdToCheck);
+#endif
         mapToAdd.erase(gpioIdToCheck);
+        return GPIO_DUPLICATE_DETECTED;
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
 
 void LinuxLibgpioIF::parseFindeLineResult(int result, std::string& lineName) {
     switch (result) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
     case LINE_NOT_EXISTS:
-        sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Line with name " << lineName
-            << " does not exist" << std::endl;
+    case LINE_ERROR: {
+        sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Line with name " << lineName <<
+                " does not exist" << std::endl;
         break;
-    case LINE_ERROR:
-        sif::warning << "LinuxLibgpioIF::parseFindeLineResult: " << "Line with name "
-                        << lineName << " does not exist" << std::endl;
+    }
+    default: {
+        sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Unknown return code for line "
+                "with name " << lineName << std::endl;
         break;
-    default:
-        sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Unknown return code for line with "
-            << "name " << lineName << std::endl;
+    }
+#else
+    case LINE_NOT_EXISTS:
+    case LINE_ERROR: {
+        sif::printWarning("LinuxLibgpioIF::parseFindeLineResult: Line with name %s "
+                "does not exist\n", lineName);
         break;
     }
+    default: {
+        sif::printWarning("LinuxLibgpioIF::parseFindeLineResult: Unknown return code for line "
+                "with name %s\n", lineName);
+        break;
+    }
+#endif
+    }
 
 }
diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h
index d458c362..cc32bd70 100644
--- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h
+++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h
@@ -1,19 +1,19 @@
 #ifndef LINUX_GPIO_LINUXLIBGPIOIF_H_
 #define LINUX_GPIO_LINUXLIBGPIOIF_H_
 
-#include "../../common/gpio/GpioIF.h"
-#include <returnvalues/classIds.h>
-#include <fsfw/objectmanager/SystemObject.h>
+#include "fsfw/returnvalues/FwClassIds.h"
+#include "fsfw_hal/common/gpio/GpioIF.h"
+#include "fsfw/objectmanager/SystemObject.h"
 
 class GpioCookie;
 class GpiodRegularIF;
 
 /**
- * @brief	This class implements the GpioIF for a linux based system. The
- * 			implementation is based on the libgpiod lib which requires linux 4.8
- * 			or higher.
- * @note	The Petalinux SDK from Xilinx supports libgpiod since Petalinux
- * 			2019.1.
+ * @brief	This class implements the GpioIF for a linux based system.
+ * @details
+ * This implementation is based on the libgpiod lib which requires Linux 4.8 or higher.
+ * @note
+ * The Petalinux SDK from Xilinx supports libgpiod since Petalinux 2019.1.
  */
 class LinuxLibgpioIF : public GpioIF, public SystemObject {
 public:
@@ -28,6 +28,8 @@ public:
             HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 3);
     static constexpr ReturnValue_t GPIO_INVALID_INSTANCE =
             HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 4);
+    static constexpr ReturnValue_t GPIO_DUPLICATE_DETECTED =
+            HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 5);
 
 	LinuxLibgpioIF(object_id_t objectId);
 	virtual ~LinuxLibgpioIF();
@@ -44,7 +46,7 @@ private:
 	static const int LINE_ERROR = -1;
 	static const int LINE_FOUND = 1;
 
-	/* Holds the information and configuration of all used GPIOs */
+	// Holds the information and configuration of all used GPIOs
 	GpioUnorderedMap gpioMap;
 	GpioUnorderedMapIter gpioMapIter;
 
@@ -75,10 +77,8 @@ private:
 	 */
 	ReturnValue_t checkForConflicts(GpioMap& mapToAdd);
 
-	ReturnValue_t checkForConflictsRegularGpio(gpioId_t gpiodId, GpiodRegularBase& regularGpio,
+	ReturnValue_t checkForConflictsById(gpioId_t gpiodId, gpio::GpioTypes type,
 	        GpioMap& mapToAdd);
-    ReturnValue_t checkForConflictsCallbackGpio(gpioId_t gpiodId, GpioCallback* regularGpio,
-            GpioMap& mapToAdd);
 
 	/**
 	 * @brief   Performs the initial configuration of all GPIOs specified in the GpioMap mapToAdd.
diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp
index 1fa87121..9c4e66ae 100644
--- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp
+++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp
@@ -227,7 +227,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie *spiCookie, const
             utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
             result = FULL_DUPLEX_TRANSFER_FAILED;
         }
-#if FSFW_HAL_LINUX_SPI_WIRETAPPING == 1
+#if FSFW_HAL_SPI_WIRETAPPING == 1
         performSpiWiretapping(spiCookie);
 #endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
     }
diff --git a/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in
index 3f005387..4da49b16 100644
--- a/src/fsfw/FSFW.h.in
+++ b/src/fsfw/FSFW.h.in
@@ -20,9 +20,9 @@
 #define FSFW_TCP_RECV_WIRETAPPING_ENABLED   0
 #endif
 
-/* Can be used for low-level debugging of the SPI bus */
-#ifndef FSFW_HAL_LINUX_SPI_WIRETAPPING
-#define FSFW_HAL_LINUX_SPI_WIRETAPPING      0
+// Can be used for low-level debugging of the SPI bus
+#ifndef FSFW_HAL_SPI_WIRETAPPING
+#define FSFW_HAL_SPI_WIRETAPPING            0
 #endif
 
 #ifndef FSFW_HAL_L3GD20_GYRO_DEBUG
@@ -34,7 +34,7 @@
 #endif /* FSFW_HAL_RM3100_MGM_DEBUG */
 
 #ifndef FSFW_HAL_LIS3MDL_MGM_DEBUG
-#define FSFW_HAL_LIS3MDL_MGM_DEBUG           0
+#define FSFW_HAL_LIS3MDL_MGM_DEBUG          0
 #endif /* FSFW_HAL_LIS3MDL_MGM_DEBUG */
 
 #endif /* FSFW_FSFW_H_ */
diff --git a/src/fsfw/events/fwSubsystemIdRanges.h b/src/fsfw/events/fwSubsystemIdRanges.h
index 88dee9b4..08fb878d 100644
--- a/src/fsfw/events/fwSubsystemIdRanges.h
+++ b/src/fsfw/events/fwSubsystemIdRanges.h
@@ -29,6 +29,8 @@ enum: uint8_t {
     PUS_SERVICE_9 = 89,
     PUS_SERVICE_17 = 97,
     PUS_SERVICE_23 = 103,
+    MGM_LIS3MDL = 106,
+    MGM_RM3100 = 107,
 
     FW_SUBSYSTEM_ID_RANGE
 };
diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h
index ce093b09..4a3a578a 100644
--- a/src/fsfw/returnvalues/FwClassIds.h
+++ b/src/fsfw/returnvalues/FwClassIds.h
@@ -78,6 +78,8 @@ enum: uint8_t {
     HAL_I2C, //HI2C
     HAL_GPIO, //HGIO
     FIXED_SLOT_TASK_IF, //FTIF
+    MGM_LIS3MDL, //MGMLIS3
+    MGM_RM3100, //MGMRM3100
     FW_CLASS_ID_COUNT // [EXPORT] : [END]
 
 };
diff --git a/tests/src/fsfw_tests/internal/InternalUnitTester.cpp b/tests/src/fsfw_tests/internal/InternalUnitTester.cpp
index f9fc1932..20998d64 100644
--- a/tests/src/fsfw_tests/internal/InternalUnitTester.cpp
+++ b/tests/src/fsfw_tests/internal/InternalUnitTester.cpp
@@ -16,15 +16,18 @@ InternalUnitTester::~InternalUnitTester() {}
 ReturnValue_t InternalUnitTester::performTests(
         const struct InternalUnitTester::TestConfig& testConfig) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	sif::info << "Running internal unit tests.." << std::endl;
+	sif::info << "Running internal unit tests.. Error messages might follow" <<
+	        std::endl;
 #else
 	sif::printInfo("Running internal unit tests..\n");
 #endif
 
 	testserialize::test_serialization();
 	testmq::testMq();
-	testsemaph::testBinSemaph();
-	testsemaph::testCountingSemaph();
+	if(testConfig.testSemaphores) {
+	    testsemaph::testBinSemaph();
+	    testsemaph::testCountingSemaph();
+	}
 	testmutex::testMutex();
 	if(testConfig.testArrayPrinter) {
 	    arrayprinter::testArrayPrinter();
diff --git a/tests/src/fsfw_tests/internal/InternalUnitTester.h b/tests/src/fsfw_tests/internal/InternalUnitTester.h
index 50c89d77..433a0f1f 100644
--- a/tests/src/fsfw_tests/internal/InternalUnitTester.h
+++ b/tests/src/fsfw_tests/internal/InternalUnitTester.h
@@ -18,6 +18,7 @@ class InternalUnitTester: public HasReturnvaluesIF {
 public:
     struct TestConfig {
         bool testArrayPrinter = false;
+        bool testSemaphores = true;
     };
 
 	InternalUnitTester();
diff --git a/tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp b/tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp
index b1699a46..d9184cd8 100644
--- a/tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp
+++ b/tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp
@@ -1,10 +1,12 @@
 #include "fsfw_tests/internal/osal/IntTestMutex.h"
 #include "fsfw_tests/internal/UnittDefinitions.h"
+#include "fsfw/platform.h"
 
 #include <fsfw/ipc/MutexFactory.h>
 
-#if defined(WIN32) || defined(UNIX)
-#include <fsfw/osal/host/Mutex.h>
+#if defined PLATFORM_WIN || defined PLATFORM_UNIX
+#include "fsfw/osal/host/Mutex.h"
+
 #include <thread>
 #include <future>
 #endif
@@ -20,7 +22,7 @@ void testmutex::testMutex() {
 	// timed_mutex from the C++ library specifies undefined behaviour if
 	// the timed mutex is locked twice from the same thread.
     // TODO: we should pass a define like FSFW_OSAL_HOST to the build.
-#if defined(WIN32) || defined(UNIX)
+#if defined PLATFORM_WIN || defined PLATFORM_UNIX
 	// This calls the function from
 	// another thread and stores the returnvalue in a future.
 	auto future = std::async(&MutexIF::lockMutex, mutex, MutexIF::TimeoutType::WAITING, 1);
@@ -37,8 +39,7 @@ void testmutex::testMutex() {
 		unitt::put_error(id);
 	}
 
-	// TODO: we should pass a define like FSFW_OSAL_HOST to the build.
-#if !defined(WIN32) && !defined(UNIX)
+#if !defined PLATFORM_WIN && !defined PLATFORM_UNIX
     result = mutex->unlockMutex();
 	if(result != MutexIF::CURR_THREAD_DOES_NOT_OWN_MUTEX) {
 		unitt::put_error(id);
diff --git a/tests/src/fsfw_tests/internal/osal/IntTestSemaphore.cpp b/tests/src/fsfw_tests/internal/osal/IntTestSemaphore.cpp
index 8b79f33b..4b28f961 100644
--- a/tests/src/fsfw_tests/internal/osal/IntTestSemaphore.cpp
+++ b/tests/src/fsfw_tests/internal/osal/IntTestSemaphore.cpp
@@ -1,9 +1,10 @@
+#include "fsfw/FSFW.h"
 #include "fsfw_tests/internal/osal/IntTestSemaphore.h"
 #include "fsfw_tests/internal/UnittDefinitions.h"
 
-#include <fsfw/tasks/SemaphoreFactory.h>
-#include <fsfw/serviceinterface/ServiceInterface.h>
-#include <fsfw/timemanager/Stopwatch.h>
+#include "fsfw/tasks/SemaphoreFactory.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/timemanager/Stopwatch.h"
 
 #include <cstdlib>
 
@@ -16,7 +17,7 @@ void testsemaph::testBinSemaph() {
 	}
 	testBinSemaphoreImplementation(binSemaph, id);
 	SemaphoreFactory::instance()->deleteSemaphore(binSemaph);
-#if defined(freeRTOS)
+#if defined FSFW_OSAL_FREERTOS
 	SemaphoreIF* binSemaphUsingTask =
 			SemaphoreFactory::instance()->createBinarySemaphore(1);
 	testBinSemaphoreImplementation(binSemaphUsingTask, id);
@@ -36,7 +37,7 @@ void testsemaph::testCountingSemaph() {
 		}
 		testBinSemaphoreImplementation(countingSemaph, id);
 		SemaphoreFactory::instance()->deleteSemaphore(countingSemaph);
-#if defined(freeRTOS)
+#if defined FSFW_OSAL_FREERTOS
 		countingSemaph = SemaphoreFactory::instance()->
 				createCountingSemaphore(1, 1, 1);
 		testBinSemaphoreImplementation(countingSemaph, id);
@@ -50,7 +51,7 @@ void testsemaph::testCountingSemaph() {
 				createCountingSemaphore(3,3);
 		testCountingSemaphImplementation(countingSemaph, id);
 		SemaphoreFactory::instance()->deleteSemaphore(countingSemaph);
-#if defined(freeRTOS)
+#if defined FSFW_OSAL_FREERTOS
 		countingSemaph = SemaphoreFactory::instance()->
 				createCountingSemaphore(3, 0, 1);
 		uint8_t semaphCount = countingSemaph->getSemaphoreCounter();