fix for raspberry pi code

Reviewed-on: #15

hal/src/fsfw_hal/common/gpio/gpioDefinitions.h

@@ -26,7 +26,8 @@ enum GpioOperation {
 
 enum GpioTypes {
     NONE,
-    GPIO_REGULAR,
+    GPIO_REGULAR_BY_CHIP,
+    GPIO_REGULAR_BY_LABEL,
     CALLBACK
 };
 
@@ -68,28 +69,57 @@ public:
     int initValue = 0;
 };
 
-class GpiodRegular: public GpioBase {
+class GpiodRegularBase: public GpioBase {
 public:
-    GpiodRegular() :
-            GpioBase(gpio::GpioTypes::GPIO_REGULAR, std::string(), gpio::Direction::IN, 0) {
+    GpiodRegularBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction,
+            int initValue, int lineNum): GpioBase(gpioType, consumer, direction, initValue),
+            lineNum(lineNum) {
     }
-    ;
-
-    GpiodRegular(std::string chipname_, int lineNum_, std::string consumer_,
-            gpio::Direction direction_, int initValue_) :
-            GpioBase(gpio::GpioTypes::GPIO_REGULAR, consumer_, direction_, initValue_),
-            chipname(chipname_), lineNum(lineNum_) {
-    }
-
-    GpiodRegular(std::string chipname_, int lineNum_, std::string consumer_) :
-            GpioBase(gpio::GpioTypes::GPIO_REGULAR, consumer_, gpio::Direction::IN, 0),
-            chipname(chipname_), lineNum(lineNum_) {
-    }
-    std::string chipname;
     int lineNum = 0;
     struct gpiod_line* lineHandle = nullptr;
 };
 
+class GpiodRegularByChip: public GpiodRegularBase {
+public:
+    GpiodRegularByChip() :
+            GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP,
+                    std::string(), gpio::Direction::IN, gpio::LOW, 0) {
+    }
+
+    GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_,
+            gpio::Direction direction_, int initValue_) :
+            GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP,
+                    consumer_, direction_, initValue_, lineNum_),
+            chipname(chipname_){
+    }
+
+    GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_) :
+            GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_,
+                    gpio::Direction::IN, gpio::LOW, lineNum_),
+            chipname(chipname_) {
+    }
+
+    std::string chipname;
+};
+
+class GpiodRegularByLabel: public GpiodRegularBase {
+public:
+    GpiodRegularByLabel(std::string label_, int lineNum_, std::string consumer_,
+            gpio::Direction direction_, int initValue_) :
+            GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_,
+                    direction_, initValue_, lineNum_),
+            label(label_) {
+    }
+
+    GpiodRegularByLabel(std::string label_, int lineNum_, std::string consumer_) :
+            GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_,
+                    gpio::Direction::IN, gpio::LOW, lineNum_),
+            label(label_) {
+    }
+
+    std::string label;
+};
+
 class GpioCallback: public GpioBase {
 public:
     GpioCallback(std::string consumer, gpio::Direction direction_, int initValue_,

hal/src/fsfw_hal/devicehandlers/CMakeLists.txt

@@ -1,3 +1,5 @@
 target_sources(${LIB_FSFW_NAME} PRIVATE
     GyroL3GD20Handler.cpp
+    MgmRM3100Handler.cpp
+    MgmLIS3MDLHandler.cpp
 )

hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp

@@ -0,0 +1,505 @@
+#include "MgmLIS3MDLHandler.h"
+
+#include "fsfw/datapool/PoolReadGuard.h"
+#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1
+#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
+#endif
+
+MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCommunication,
+        CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelay):
+        DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+        dataset(this), transitionDelay(transitionDelay) {
+#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1
+    debugDivider = new PeriodicOperationDivider(5);
+#endif
+    /* Set to default values right away. */
+    registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
+    registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT;
+    registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT;
+    registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT;
+    registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT;
+
+}
+
+MgmLIS3MDLHandler::~MgmLIS3MDLHandler() {
+}
+
+
+void MgmLIS3MDLHandler::doStartUp() {
+    switch (internalState) {
+    case(InternalState::STATE_NONE): {
+        internalState = InternalState::STATE_FIRST_CONTACT;
+        break;
+    }
+    case(InternalState::STATE_FIRST_CONTACT): {
+        /* Will be set by checking device ID (WHO AM I register) */
+        if(commandExecuted) {
+            commandExecuted = false;
+            internalState = InternalState::STATE_SETUP;
+        }
+        break;
+    }
+    case(InternalState::STATE_SETUP): {
+        internalState = InternalState::STATE_CHECK_REGISTERS;
+        break;
+    }
+    case(InternalState::STATE_CHECK_REGISTERS): {
+        /* Set up cached registers which will be used to configure the MGM. */
+        if(commandExecuted) {
+            commandExecuted = false;
+            if(goToNormalMode) {
+                setMode(MODE_NORMAL);
+            }
+            else {
+                setMode(_MODE_TO_ON);
+            }
+        }
+        break;
+    }
+    default:
+        break;
+    }
+
+}
+
+void MgmLIS3MDLHandler::doShutDown() {
+    setMode(_MODE_POWER_DOWN);
+}
+
+ReturnValue_t MgmLIS3MDLHandler::buildTransitionDeviceCommand(
+        DeviceCommandId_t *id) {
+    switch (internalState) {
+    case(InternalState::STATE_NONE):
+    case(InternalState::STATE_NORMAL): {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(InternalState::STATE_FIRST_CONTACT): {
+        *id = MGMLIS3MDL::IDENTIFY_DEVICE;
+        break;
+    }
+    case(InternalState::STATE_SETUP): {
+        *id = MGMLIS3MDL::SETUP_MGM;
+        break;
+    }
+    case(InternalState::STATE_CHECK_REGISTERS): {
+        *id = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+        break;
+    }
+    default: {
+        /* might be a configuration error. */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" <<
+                std::endl;
+#else
+        sif::printWarning("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+
+    }
+    return buildCommandFromCommand(*id, NULL, 0);
+}
+
+uint8_t MgmLIS3MDLHandler::readCommand(uint8_t command, bool continuousCom) {
+    command |= (1 << MGMLIS3MDL::RW_BIT);
+    if (continuousCom == true) {
+        command |= (1 << MGMLIS3MDL::MS_BIT);
+    }
+    return command;
+}
+
+uint8_t MgmLIS3MDLHandler::writeCommand(uint8_t command, bool continuousCom) {
+    command &= ~(1 << MGMLIS3MDL::RW_BIT);
+    if (continuousCom == true) {
+        command |= (1 << MGMLIS3MDL::MS_BIT);
+    }
+    return command;
+}
+
+void MgmLIS3MDLHandler::setupMgm() {
+
+    registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
+    registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT;
+    registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT;
+    registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT;
+    registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT;
+
+    prepareCtrlRegisterWrite();
+}
+
+ReturnValue_t MgmLIS3MDLHandler::buildNormalDeviceCommand(
+        DeviceCommandId_t *id) {
+    // Data/config register will be read in an alternating manner.
+    if(communicationStep == CommunicationStep::DATA) {
+        *id = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+        communicationStep = CommunicationStep::TEMPERATURE;
+        return buildCommandFromCommand(*id, NULL, 0);
+    }
+    else {
+        *id = MGMLIS3MDL::READ_TEMPERATURE;
+        communicationStep = CommunicationStep::DATA;
+        return buildCommandFromCommand(*id, NULL, 0);
+    }
+
+}
+
+ReturnValue_t MgmLIS3MDLHandler::buildCommandFromCommand(
+        DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+        size_t commandDataLen) {
+    switch(deviceCommand) {
+    case(MGMLIS3MDL::READ_CONFIG_AND_DATA): {
+        std::memset(commandBuffer, 0, sizeof(commandBuffer));
+        commandBuffer[0] = readCommand(MGMLIS3MDL::CTRL_REG1, true);
+
+        rawPacket = commandBuffer;
+        rawPacketLen = MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1;
+        return RETURN_OK;
+    }
+    case(MGMLIS3MDL::READ_TEMPERATURE): {
+        std::memset(commandBuffer, 0, 3);
+        commandBuffer[0] = readCommand(MGMLIS3MDL::TEMP_LOWBYTE, true);
+
+        rawPacket = commandBuffer;
+        rawPacketLen = 3;
+        return RETURN_OK;
+    }
+    case(MGMLIS3MDL::IDENTIFY_DEVICE): {
+        return identifyDevice();
+    }
+    case(MGMLIS3MDL::TEMP_SENSOR_ENABLE): {
+        return enableTemperatureSensor(commandData, commandDataLen);
+    }
+    case(MGMLIS3MDL::SETUP_MGM): {
+        setupMgm();
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(MGMLIS3MDL::ACCURACY_OP_MODE_SET): {
+        return setOperatingMode(commandData, commandDataLen);
+    }
+    default:
+        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+    }
+    return HasReturnvaluesIF::RETURN_FAILED;
+}
+
+ReturnValue_t MgmLIS3MDLHandler::identifyDevice() {
+    uint32_t size = 2;
+    commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR);
+    commandBuffer[1] = 0x00;
+
+    rawPacket = commandBuffer;
+    rawPacketLen = size;
+
+    return RETURN_OK;
+}
+
+ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start,
+        size_t len, DeviceCommandId_t *foundId, size_t *foundLen) {
+    *foundLen = len;
+    if (len == MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1) {
+        *foundLen = len;
+        *foundId = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+        // Check validity by checking config registers
+        if (start[1] != registers[0] or start[2] != registers[1] or
+                start[3] != registers[2] or start[4] != registers[3] or
+                start[5] != registers[4]) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::warning << "MGMHandlerLIS3MDL::scanForReply: Invalid registers!" << std::endl;
+#else
+            sif::printWarning("MGMHandlerLIS3MDL::scanForReply: Invalid registers!\n");
+#endif
+#endif
+            return DeviceHandlerIF::INVALID_DATA;
+        }
+        if(mode == _MODE_START_UP) {
+            commandExecuted = true;
+        }
+
+    }
+    else if(len == MGMLIS3MDL::TEMPERATURE_REPLY_LEN) {
+        *foundLen = len;
+        *foundId = MGMLIS3MDL::READ_TEMPERATURE;
+    }
+    else if (len == MGMLIS3MDL::SETUP_REPLY_LEN) {
+        *foundLen = len;
+        *foundId = MGMLIS3MDL::SETUP_MGM;
+    }
+    else if (len == SINGLE_COMMAND_ANSWER_LEN) {
+        *foundLen = len;
+        *foundId = getPendingCommand();
+        if(*foundId == MGMLIS3MDL::IDENTIFY_DEVICE) {
+            if(start[1] != MGMLIS3MDL::DEVICE_ID) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+                sif::warning << "MGMHandlerLIS3MDL::scanForReply: "
+                        "Device identification failed!" << std::endl;
+#else
+                sif::printWarning("MGMHandlerLIS3MDL::scanForReply: "
+                        "Device identification failed!\n");
+#endif
+#endif
+                return DeviceHandlerIF::INVALID_DATA;
+            }
+
+            if(mode == _MODE_START_UP) {
+                commandExecuted = true;
+            }
+        }
+    }
+    else {
+        return DeviceHandlerIF::INVALID_DATA;
+    }
+
+    /* Data with SPI Interface always has this answer */
+    if (start[0] == 0b11111111) {
+        return RETURN_OK;
+    }
+    else {
+        return DeviceHandlerIF::INVALID_DATA;
+    }
+
+}
+ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id,
+        const uint8_t *packet) {
+
+    switch (id) {
+    case MGMLIS3MDL::IDENTIFY_DEVICE: {
+        break;
+    }
+    case MGMLIS3MDL::SETUP_MGM: {
+        break;
+    }
+    case MGMLIS3MDL::READ_CONFIG_AND_DATA: {
+        // TODO: Store configuration in new local datasets.
+        float sensitivityFactor = getSensitivityFactor(getSensitivity(registers[2]));
+
+        int16_t mgmMeasurementRawX = packet[MGMLIS3MDL::X_HIGHBYTE_IDX] << 8
+                | packet[MGMLIS3MDL::X_LOWBYTE_IDX] ;
+        int16_t mgmMeasurementRawY = packet[MGMLIS3MDL::Y_HIGHBYTE_IDX] << 8
+                | packet[MGMLIS3MDL::Y_LOWBYTE_IDX] ;
+        int16_t mgmMeasurementRawZ = packet[MGMLIS3MDL::Z_HIGHBYTE_IDX] << 8
+                | packet[MGMLIS3MDL::Z_LOWBYTE_IDX] ;
+
+        /* Target value in microtesla */
+        float mgmX = static_cast<float>(mgmMeasurementRawX) * sensitivityFactor
+                *  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+        float mgmY = static_cast<float>(mgmMeasurementRawY) * sensitivityFactor
+                *  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+        float mgmZ = static_cast<float>(mgmMeasurementRawZ) * sensitivityFactor
+                *  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+
+#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1
+        if(debugDivider->checkAndIncrement()) {
+            /* Set terminal to utf-8 if there is an issue with micro printout. */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::info << "MGMHandlerLIS3: Magnetic field strength in"
+                    " microtesla:" << std::endl;
+            sif::info << "X: " << mgmX << " uT" << std::endl;
+            sif::info << "Y: " << mgmY << " uT" << std::endl;
+            sif::info << "Z: " << mgmZ << " uT" << std::endl;
+#else
+            sif::printInfo("MGMHandlerLIS3: Magnetic field strength in microtesla:\n");
+            sif::printInfo("X: %f uT\n", mgmX);
+            sif::printInfo("Y: %f uT\n", mgmY);
+            sif::printInfo("Z: %f uT\n", mgmZ);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
+        }
+#endif /* OBSW_VERBOSE_LEVEL >= 1 */
+        PoolReadGuard readHelper(&dataset);
+        if(readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
+            dataset.fieldStrengthX = mgmX;
+            dataset.fieldStrengthY = mgmY;
+            dataset.fieldStrengthZ = mgmZ;
+            dataset.setValidity(true, true);
+        }
+        break;
+    }
+
+    case MGMLIS3MDL::READ_TEMPERATURE: {
+        int16_t tempValueRaw = packet[2] << 8 | packet[1];
+        float tempValue = 25.0 + ((static_cast<float>(tempValueRaw)) / 8.0);
+    #if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1
+        if(debugDivider->check()) {
+            /* Set terminal to utf-8 if there is an issue with micro printout. */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::info << "MGMHandlerLIS3: Temperature: " << tempValue << " C" <<
+                    std::endl;
+#else
+            sif::printInfo("MGMHandlerLIS3: Temperature: %f C\n");
+#endif
+        }
+#endif
+        ReturnValue_t result = dataset.read();
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            dataset.temperature = tempValue;
+            dataset.commit();
+        }
+        break;
+    }
+
+    default: {
+        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
+    }
+
+    }
+    return RETURN_OK;
+}
+
+MGMLIS3MDL::Sensitivies MgmLIS3MDLHandler::getSensitivity(uint8_t ctrlRegister2) {
+    bool fs0Set = ctrlRegister2 & (1 << MGMLIS3MDL::FSO);  // Checks if FS0 bit is set
+    bool fs1Set = ctrlRegister2 & (1 << MGMLIS3MDL::FS1);  // Checks if FS1 bit is set
+
+    if (fs0Set && fs1Set)
+        return MGMLIS3MDL::Sensitivies::GAUSS_16;
+    else if (!fs0Set && fs1Set)
+        return MGMLIS3MDL::Sensitivies::GAUSS_12;
+    else if (fs0Set && !fs1Set)
+        return MGMLIS3MDL::Sensitivies::GAUSS_8;
+    else
+        return MGMLIS3MDL::Sensitivies::GAUSS_4;
+}
+
+float MgmLIS3MDLHandler::getSensitivityFactor(MGMLIS3MDL::Sensitivies sens) {
+    switch(sens) {
+    case(MGMLIS3MDL::GAUSS_4): {
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_4_SENS;
+    }
+    case(MGMLIS3MDL::GAUSS_8): {
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_8_SENS;
+    }
+    case(MGMLIS3MDL::GAUSS_12): {
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_12_SENS;
+    }
+    case(MGMLIS3MDL::GAUSS_16): {
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_16_SENS;
+    }
+    default: {
+        // Should never happen
+        return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_4_SENS;
+    }
+    }
+}
+
+
+ReturnValue_t MgmLIS3MDLHandler::enableTemperatureSensor(
+        const uint8_t *commandData, size_t commandDataLen) {
+    triggerEvent(CHANGE_OF_SETUP_PARAMETER);
+    uint32_t size = 2;
+    commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1);
+    if (commandDataLen > 1) {
+        return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
+    }
+    switch (*commandData) {
+    case (MGMLIS3MDL::ON): {
+        commandBuffer[1] = registers[0] | (1 << 7);
+        break;
+    }
+    case (MGMLIS3MDL::OFF): {
+        commandBuffer[1] = registers[0] & ~(1 << 7);
+        break;
+    }
+    default:
+        return INVALID_COMMAND_PARAMETER;
+    }
+    registers[0] = commandBuffer[1];
+
+    rawPacket = commandBuffer;
+    rawPacketLen = size;
+
+    return RETURN_OK;
+}
+
+ReturnValue_t MgmLIS3MDLHandler::setOperatingMode(const uint8_t *commandData,
+        size_t commandDataLen) {
+    triggerEvent(CHANGE_OF_SETUP_PARAMETER);
+    if (commandDataLen != 1) {
+        return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
+    }
+
+    switch (commandData[0]) {
+    case MGMLIS3MDL::LOW:
+        registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) & (~(1 << MGMLIS3MDL::OM0));
+        registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) & (~(1 << MGMLIS3MDL::OMZ0));
+        break;
+    case MGMLIS3MDL::MEDIUM:
+        registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) | (1 << MGMLIS3MDL::OM0);
+        registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) | (1 << MGMLIS3MDL::OMZ0);
+        break;
+
+    case MGMLIS3MDL::HIGH:
+        registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) & (~(1 << MGMLIS3MDL::OM0));
+        registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) & (~(1 << MGMLIS3MDL::OMZ0));
+        break;
+
+    case MGMLIS3MDL::ULTRA:
+        registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) | (1 << MGMLIS3MDL::OM0);
+        registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) | (1 << MGMLIS3MDL::OMZ0);
+        break;
+    default:
+        break;
+    }
+
+    return prepareCtrlRegisterWrite();
+}
+
+void MgmLIS3MDLHandler::fillCommandAndReplyMap() {
+    /*
+     * Regarding ArduinoBoard:
+     * Actually SPI answers directly, but as commanding ArduinoBoard the
+     * communication could be delayed
+     * SPI always has to be triggered, so there could be no periodic answer of
+     * the device, the device has to asked with a command, so periodic is zero.
+     *
+     * We dont read single registers, we just expect special
+     * reply from he Readall_MGM
+     */
+    insertInCommandAndReplyMap(MGMLIS3MDL::READ_CONFIG_AND_DATA, 1, &dataset);
+    insertInCommandAndReplyMap(MGMLIS3MDL::READ_TEMPERATURE, 1);
+    insertInCommandAndReplyMap(MGMLIS3MDL::SETUP_MGM, 1);
+    insertInCommandAndReplyMap(MGMLIS3MDL::IDENTIFY_DEVICE, 1);
+    insertInCommandAndReplyMap(MGMLIS3MDL::TEMP_SENSOR_ENABLE, 1);
+    insertInCommandAndReplyMap(MGMLIS3MDL::ACCURACY_OP_MODE_SET, 1);
+}
+
+void MgmLIS3MDLHandler::setToGoToNormalMode(bool enable) {
+    this->goToNormalMode = enable;
+}
+
+ReturnValue_t MgmLIS3MDLHandler::prepareCtrlRegisterWrite() {
+    commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1, true);
+
+    for (size_t i = 0; i < MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) {
+        commandBuffer[i + 1] = registers[i];
+    }
+    rawPacket = commandBuffer;
+    rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS + 1;
+
+    /* We dont have to check if this is working because we just did it */
+    return RETURN_OK;
+}
+
+void MgmLIS3MDLHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
+
+}
+
+uint32_t MgmLIS3MDLHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
+    return transitionDelay;
+}
+
+void MgmLIS3MDLHandler::modeChanged(void) {
+    internalState = InternalState::STATE_NONE;
+}
+
+ReturnValue_t MgmLIS3MDLHandler::initializeLocalDataPool(
+        localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
+    localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_X,
+            new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Y,
+            new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Z,
+            new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS,
+            new PoolEntry<float>({0.0}));
+    return HasReturnvaluesIF::RETURN_OK;
+}

hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h

@@ -0,0 +1,167 @@
+#ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_
+#define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_
+
+#include "fsfw/FSFW.h"
+#include "events/subsystemIdRanges.h"
+#include "devicedefinitions/MgmLIS3HandlerDefs.h"
+
+#include "fsfw/devicehandlers/DeviceHandlerBase.h"
+
+class PeriodicOperationDivider;
+
+/**
+ * @brief   Device handler object for the LIS3MDL 3-axis magnetometer
+ *          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 MgmLIS3MDLHandler: public DeviceHandlerBase {
+public:
+    enum class CommunicationStep {
+        DATA,
+        TEMPERATURE
+    };
+
+    static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL;
+    static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL;
+    //Notifies a command to change the setup parameters
+    static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW);
+
+    MgmLIS3MDLHandler(uint32_t objectId, object_id_t deviceCommunication, CookieIF* comCookie,
+            uint8_t switchId, uint32_t transitionDelay = 10000);
+    virtual ~MgmLIS3MDLHandler();
+
+    void setToGoToNormalMode(bool enable);
+
+protected:
+
+    /** DeviceHandlerBase overrides */
+    void doShutDown() override;
+    void doStartUp() override;
+    void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override;
+    uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
+    ReturnValue_t buildCommandFromCommand(
+            DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+            size_t commandDataLen) override;
+    ReturnValue_t buildTransitionDeviceCommand(
+            DeviceCommandId_t *id) override;
+    ReturnValue_t buildNormalDeviceCommand(
+            DeviceCommandId_t *id) override;
+    ReturnValue_t scanForReply(const uint8_t *start, size_t len,
+            DeviceCommandId_t *foundId, size_t *foundLen) override;
+    ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
+            const uint8_t *packet) override;
+    void fillCommandAndReplyMap() override;
+    void modeChanged(void) override;
+    ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+            LocalDataPoolManager &poolManager) override;
+
+private:
+    MGMLIS3MDL::MgmPrimaryDataset dataset;
+    //Length a sindgle command SPI answer
+    static const uint8_t SINGLE_COMMAND_ANSWER_LEN = 2;
+
+    uint32_t transitionDelay;
+    //Single SPIcommand has 2 bytes, first for adress, second for content
+    size_t singleComandSize = 2;
+    //has the size for all adresses of the lis3mdl + the continous write bit
+    uint8_t commandBuffer[MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1];
+
+    /**
+     * We want to save the registers we set, so we dont have to read the
+     * registers when we want to change something.
+     * --> everytime we change set a register we have to save it
+     */
+    uint8_t registers[MGMLIS3MDL::NR_OF_CTRL_REGISTERS];
+
+    uint8_t statusRegister = 0;
+    bool goToNormalMode = false;
+
+    enum class InternalState {
+        STATE_NONE,
+        STATE_FIRST_CONTACT,
+        STATE_SETUP,
+        STATE_CHECK_REGISTERS,
+        STATE_NORMAL
+    };
+
+    InternalState internalState = InternalState::STATE_NONE;
+    CommunicationStep communicationStep = CommunicationStep::DATA;
+    bool commandExecuted = false;
+
+    /*------------------------------------------------------------------------*/
+    /* Device specific commands and variables */
+    /*------------------------------------------------------------------------*/
+    /**
+     * Sets the read bit for the command
+     * @param single command to set the read-bit at
+     * @param boolean to select a continuous read bit, default = false
+     */
+    uint8_t readCommand(uint8_t command, bool continuousCom = false);
+
+    /**
+     * Sets the write bit for the command
+     * @param single command to set the write-bit at
+     * @param boolean to select a continuous write bit, default = false
+     */
+    uint8_t writeCommand(uint8_t command, bool continuousCom = false);
+
+    /**
+     * This Method gets the full scale for the measurement range
+     * e.g.: +- 4 gauss. See p.25 datasheet.
+     *  @return The ReturnValue does not contain the sign of the value
+     */
+    MGMLIS3MDL::Sensitivies getSensitivity(uint8_t ctrlReg2);
+
+    /**
+     * The 16 bit value needs to be multiplied with a sensitivity factor
+     * which depends on the sensitivity configuration
+     *
+     * @param sens Configured sensitivity of the LIS3 device
+     * @return Multiplication factor to get the sensor value from raw data.
+     */
+    float getSensitivityFactor(MGMLIS3MDL::Sensitivies sens);
+
+    /**
+     * This Command detects the device ID
+     */
+    ReturnValue_t identifyDevice();
+
+    virtual void setupMgm();
+
+    /*------------------------------------------------------------------------*/
+    /* Non normal commands */
+    /*------------------------------------------------------------------------*/
+    /**
+     * Enables/Disables the integrated Temperaturesensor
+     * @param commandData On or Off
+     * @param length of the commandData: has to be 1
+     */
+    virtual ReturnValue_t enableTemperatureSensor(const uint8_t *commandData,
+            size_t commandDataLen);
+
+    /**
+     * Sets the accuracy of the measurement of the axis. The noise is changing.
+     * @param commandData LOW, MEDIUM, HIGH, ULTRA
+     * @param length of the command, has to be 1
+     */
+    virtual ReturnValue_t setOperatingMode(const uint8_t *commandData,
+            size_t commandDataLen);
+
+    /**
+     * We always update all registers together, so this method updates
+     * the rawpacket and rawpacketLen, so we just manipulate the local
+     * saved register
+     *
+     */
+    ReturnValue_t prepareCtrlRegisterWrite();
+
+#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1
+    PeriodicOperationDivider* debugDivider;
+#endif
+};
+
+#endif /* MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ */

hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp

@@ -0,0 +1,370 @@
+#include "MgmRM3100Handler.h"
+
+#include "fsfw/datapool/PoolReadGuard.h"
+#include "fsfw/globalfunctions/bitutility.h"
+#include "fsfw/devicehandlers/DeviceHandlerMessage.h"
+#include "fsfw/objectmanager/SystemObjectIF.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+
+
+MgmRM3100Handler::MgmRM3100Handler(object_id_t objectId,
+        object_id_t deviceCommunication, CookieIF* comCookie, uint8_t switchId,
+        uint32_t transitionDelay):
+        DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+        primaryDataset(this), switchId(switchId), transitionDelay(transitionDelay) {
+#if FSFW_HAL_RM3100_MGM_DEBUG == 1
+    debugDivider = new PeriodicOperationDivider(3);
+#endif
+}
+
+MgmRM3100Handler::~MgmRM3100Handler() {}
+
+void MgmRM3100Handler::doStartUp() {
+    switch(internalState) {
+    case(InternalState::NONE): {
+        internalState = InternalState::CONFIGURE_CMM;
+        break;
+    }
+    case(InternalState::CONFIGURE_CMM): {
+        internalState = InternalState::READ_CMM;
+        break;
+    }
+    case(InternalState::READ_CMM): {
+        if(commandExecuted) {
+            internalState = InternalState::STATE_CONFIGURE_TMRC;
+        }
+        break;
+    }
+    case(InternalState::STATE_CONFIGURE_TMRC): {
+        if(commandExecuted) {
+            internalState = InternalState::STATE_READ_TMRC;
+        }
+        break;
+    }
+    case(InternalState::STATE_READ_TMRC): {
+        if(commandExecuted) {
+            internalState = InternalState::NORMAL;
+            if(goToNormalModeAtStartup) {
+                setMode(MODE_NORMAL);
+            }
+            else {
+                setMode(_MODE_TO_ON);
+            }
+        }
+        break;
+    }
+    default: {
+        break;
+    }
+    }
+}
+
+void MgmRM3100Handler::doShutDown() {
+    setMode(_MODE_POWER_DOWN);
+}
+
+ReturnValue_t MgmRM3100Handler::buildTransitionDeviceCommand(
+        DeviceCommandId_t *id) {
+    size_t commandLen = 0;
+    switch(internalState) {
+    case(InternalState::NONE):
+    case(InternalState::NORMAL): {
+        return NOTHING_TO_SEND;
+    }
+    case(InternalState::CONFIGURE_CMM): {
+        *id = RM3100::CONFIGURE_CMM;
+        break;
+    }
+    case(InternalState::READ_CMM): {
+        *id = RM3100::READ_CMM;
+        break;
+    }
+    case(InternalState::STATE_CONFIGURE_TMRC): {
+        commandBuffer[0] = RM3100::TMRC_DEFAULT_VALUE;
+        commandLen = 1;
+        *id = RM3100::CONFIGURE_TMRC;
+        break;
+    }
+    case(InternalState::STATE_READ_TMRC): {
+        *id = RM3100::READ_TMRC;
+        break;
+    }
+    default:
+        // Might be a configuration error
+        sif::warning << "MgmRM3100Handler::buildTransitionDeviceCommand: Unknown internal state!" <<
+                std::endl;
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+
+    return buildCommandFromCommand(*id, commandBuffer, commandLen);
+}
+
+ReturnValue_t MgmRM3100Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+        const uint8_t *commandData, size_t commandDataLen) {
+    switch(deviceCommand) {
+    case(RM3100::CONFIGURE_CMM): {
+        commandBuffer[0] = RM3100::CMM_REGISTER;
+        commandBuffer[1] = RM3100::CMM_VALUE;
+        rawPacket = commandBuffer;
+        rawPacketLen = 2;
+        break;
+    }
+    case(RM3100::READ_CMM): {
+        commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK;
+        commandBuffer[1] = 0;
+        rawPacket = commandBuffer;
+        rawPacketLen = 2;
+        break;
+    }
+    case(RM3100::CONFIGURE_TMRC): {
+        return handleTmrcConfigCommand(deviceCommand, commandData, commandDataLen);
+    }
+    case(RM3100::READ_TMRC): {
+        commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK;
+        commandBuffer[1] = 0;
+        rawPacket = commandBuffer;
+        rawPacketLen = 2;
+        break;
+    }
+    case(RM3100::CONFIGURE_CYCLE_COUNT): {
+        return handleCycleCountConfigCommand(deviceCommand, commandData, commandDataLen);
+    }
+    case(RM3100::READ_CYCLE_COUNT): {
+        commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER | RM3100::READ_MASK;
+        std::memset(commandBuffer + 1, 0, 6);
+        rawPacket = commandBuffer;
+        rawPacketLen = 7;
+        break;
+    }
+    case(RM3100::READ_DATA): {
+        commandBuffer[0] = RM3100::MEASUREMENT_REG_START | RM3100::READ_MASK;
+        std::memset(commandBuffer + 1, 0,  9);
+        rawPacketLen = 10;
+        break;
+    }
+    default:
+        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+    }
+    return RETURN_OK;
+}
+
+ReturnValue_t MgmRM3100Handler::buildNormalDeviceCommand(
+        DeviceCommandId_t *id) {
+    *id = RM3100::READ_DATA;
+    return buildCommandFromCommand(*id, nullptr, 0);
+}
+
+ReturnValue_t MgmRM3100Handler::scanForReply(const uint8_t *start,
+        size_t len, DeviceCommandId_t *foundId,
+        size_t *foundLen) {
+
+    // For SPI, ID will always be the one of the last sent command
+    *foundId = this->getPendingCommand();
+    *foundLen = len;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    switch(id) {
+    case(RM3100::CONFIGURE_CMM):
+    case(RM3100::CONFIGURE_CYCLE_COUNT):
+    case(RM3100::CONFIGURE_TMRC): {
+        // We can only check whether write was successful with read operation
+        if(mode == _MODE_START_UP) {
+            commandExecuted = true;
+        }
+        break;
+    }
+    case(RM3100::READ_CMM): {
+        uint8_t cmmValue = packet[1];
+        // We clear the seventh bit in any case
+        // because this one is zero sometimes for some reason
+        bitutil::bitClear(&cmmValue, 6);
+        if(cmmValue == cmmRegValue and internalState == InternalState::READ_CMM) {
+            commandExecuted = true;
+        }
+        else {
+            // Attempt reconfiguration
+            internalState = InternalState::CONFIGURE_CMM;
+            return DeviceHandlerIF::DEVICE_REPLY_INVALID;
+        }
+        break;
+    }
+    case(RM3100::READ_TMRC): {
+        if(packet[1] == tmrcRegValue) {
+            commandExecuted = true;
+            // Reading TMRC was commanded. Trigger event to inform ground
+            if(mode != _MODE_START_UP) {
+                triggerEvent(tmrcSet, tmrcRegValue, 0);
+            }
+        }
+        else {
+            // Attempt reconfiguration
+            internalState = InternalState::STATE_CONFIGURE_TMRC;
+            return DeviceHandlerIF::DEVICE_REPLY_INVALID;
+        }
+        break;
+    }
+    case(RM3100::READ_CYCLE_COUNT): {
+        uint16_t cycleCountX = packet[1] << 8 | packet[2];
+        uint16_t cycleCountY = packet[3] << 8 | packet[4];
+        uint16_t cycleCountZ = packet[5] << 8 | packet[6];
+        if(cycleCountX != cycleCountRegValueX or cycleCountY != cycleCountRegValueY or
+                cycleCountZ != cycleCountRegValueZ) {
+            return DeviceHandlerIF::DEVICE_REPLY_INVALID;
+        }
+        // Reading TMRC was commanded. Trigger event to inform ground
+        if(mode != _MODE_START_UP) {
+            uint32_t eventParam1 = (cycleCountX << 16) | cycleCountY;
+            triggerEvent(cycleCountersSet, eventParam1, cycleCountZ);
+        }
+        break;
+    }
+    case(RM3100::READ_DATA): {
+        result = handleDataReadout(packet);
+        break;
+    }
+    default:
+        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
+    }
+
+    return result;
+}
+
+ReturnValue_t MgmRM3100Handler::handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand,
+        const uint8_t *commandData, size_t commandDataLen) {
+    if(commandData == nullptr) {
+        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+    }
+
+    // Set cycle count
+    if(commandDataLen == 2) {
+        handleCycleCommand(true, commandData, commandDataLen);
+    }
+    else if(commandDataLen == 6) {
+        handleCycleCommand(false, commandData, commandDataLen);
+    }
+    else {
+        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+    }
+
+    commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE;
+    std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2);
+    std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2);
+    std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2);
+    rawPacketLen = 7;
+    rawPacket = commandBuffer;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t MgmRM3100Handler::handleCycleCommand(bool oneCycleValue,
+        const uint8_t *commandData, size_t commandDataLen) {
+    RM3100::CycleCountCommand command(oneCycleValue);
+    ReturnValue_t result = command.deSerialize(&commandData, &commandDataLen,
+            SerializeIF::Endianness::BIG);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    // Data sheet p.30 "while noise limits the useful upper range to ~400 cycle counts."
+    if(command.cycleCountX > 450 ) {
+        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+    }
+
+    if(not oneCycleValue and (command.cycleCountY > 450 or command.cycleCountZ > 450)) {
+        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+    }
+
+    cycleCountRegValueX = command.cycleCountX;
+    cycleCountRegValueY = command.cycleCountY;
+    cycleCountRegValueZ = command.cycleCountZ;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t MgmRM3100Handler::handleTmrcConfigCommand(DeviceCommandId_t deviceCommand,
+        const uint8_t *commandData, size_t commandDataLen) {
+    if(commandData == nullptr or commandDataLen != 1) {
+        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
+    }
+
+    commandBuffer[0] = RM3100::TMRC_REGISTER;
+    commandBuffer[1] = commandData[0];
+    tmrcRegValue = commandData[0];
+    rawPacketLen = 2;
+    rawPacket = commandBuffer;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void MgmRM3100Handler::fillCommandAndReplyMap() {
+    insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 3);
+    insertInCommandAndReplyMap(RM3100::READ_CMM, 3);
+
+    insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 3);
+    insertInCommandAndReplyMap(RM3100::READ_TMRC, 3);
+
+    insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 3);
+    insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 3);
+
+    insertInCommandAndReplyMap(RM3100::READ_DATA, 3, &primaryDataset);
+}
+
+void MgmRM3100Handler::modeChanged(void) {
+    internalState = InternalState::NONE;
+}
+
+ReturnValue_t MgmRM3100Handler::initializeLocalDataPool(
+        localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
+    localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_X, new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Y, new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Z, new PoolEntry<float>({0.0}));
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+uint32_t MgmRM3100Handler::getTransitionDelayMs(Mode_t from, Mode_t to) {
+    return 25000;
+}
+
+ReturnValue_t MgmRM3100Handler::getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) {
+    *switches = &switchId;
+    *numberOfSwitches = 1;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void MgmRM3100Handler::setToGoToNormalMode(bool enable) {
+    goToNormalModeAtStartup = enable;
+}
+
+ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) {
+    // Analyze data here. The sensor generates 24 bit signed values so we need to do some bitshift
+    // trickery here to calculate the raw values first
+    int32_t fieldStrengthRawX = ((packet[1] << 24) | (packet[2] << 16) | (packet[3] << 8)) >> 8;
+    int32_t fieldStrengthRawY = ((packet[4] << 24) | (packet[5] << 16) | (packet[6] << 8)) >> 8;
+    int32_t fieldStrengthRawZ  = ((packet[7] << 24) | (packet[8] << 16) | (packet[3] << 8)) >> 8;
+
+    // Now scale to physical value in microtesla
+    float fieldStrengthX = fieldStrengthRawX * scaleFactorX;
+    float fieldStrengthY = fieldStrengthRawY * scaleFactorX;
+    float fieldStrengthZ = fieldStrengthRawZ * scaleFactorX;
+
+#if FSFW_HAL_RM3100_MGM_DEBUG == 1
+    if(debugDivider->checkAndIncrement()) {
+        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;
+    }
+#endif
+
+    // TODO: Sanity check on values?
+    PoolReadGuard readGuard(&primaryDataset);
+    if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
+        primaryDataset.fieldStrengthX = fieldStrengthX;
+        primaryDataset.fieldStrengthY = fieldStrengthY;
+        primaryDataset.fieldStrengthZ = fieldStrengthZ;
+        primaryDataset.setValidity(true, true);
+    }
+    return RETURN_OK;
+}

hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h

@@ -0,0 +1,117 @@
+#ifndef MISSION_DEVICES_MGMRM3100HANDLER_H_
+#define MISSION_DEVICES_MGMRM3100HANDLER_H_
+
+#include "fsfw/FSFW.h"
+#include "devices/powerSwitcherList.h"
+#include "devicedefinitions/MgmRM3100HandlerDefs.h"
+#include "fsfw/devicehandlers/DeviceHandlerBase.h"
+
+#if FSFW_HAL_RM3100_MGM_DEBUG == 1
+#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
+#endif
+
+/**
+ * @brief 	Device Handler for the RM3100 geomagnetic magnetometer sensor
+ *          (https://www.pnicorp.com/rm3100/)
+ * @details
+ * Flight manual:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM
+ */
+class MgmRM3100Handler: public DeviceHandlerBase {
+public:
+	static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100;
+
+	//! [EXPORT] : [COMMENT] P1: TMRC value which was set, P2: 0
+	static constexpr Event tmrcSet = event::makeEvent(SUBSYSTEM_ID::MGM_RM3100,
+			0x00, severity::INFO);
+
+	//! [EXPORT] : [COMMENT] Cycle counter set. P1: First two bytes new Cycle Count X
+	//! P1: Second two bytes new Cycle Count Y
+	//! P2: New cycle count Z
+	static constexpr Event cycleCountersSet = event::makeEvent(
+			SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO);
+
+	MgmRM3100Handler(object_id_t objectId, object_id_t deviceCommunication,
+	        CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelay = 10000);
+	virtual ~MgmRM3100Handler();
+
+	/**
+	 * Configure device handler to go to normal mode after startup immediately
+	 * @param enable
+	 */
+	void setToGoToNormalMode(bool enable);
+
+protected:
+
+	/* DeviceHandlerBase overrides */
+	ReturnValue_t buildTransitionDeviceCommand(
+	        DeviceCommandId_t *id) override;
+	void doStartUp() override;
+	void doShutDown() override;
+	ReturnValue_t buildNormalDeviceCommand(
+	        DeviceCommandId_t *id) override;
+	ReturnValue_t buildCommandFromCommand(
+	        DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+			size_t commandDataLen) override;
+	ReturnValue_t scanForReply(const uint8_t *start, size_t len,
+			DeviceCommandId_t *foundId, size_t *foundLen) override;
+	ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
+	        const uint8_t *packet) override;
+    ReturnValue_t getSwitches(const uint8_t **switches,
+            uint8_t *numberOfSwitches) override;
+
+	void fillCommandAndReplyMap() override;
+	void modeChanged(void) override;
+	uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
+	ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+			LocalDataPoolManager &poolManager) override;
+
+private:
+
+	enum class InternalState {
+	    NONE,
+		CONFIGURE_CMM,
+		READ_CMM,
+		// The cycle count states are propably not going to be used because
+		// the default cycle count will be used.
+		STATE_CONFIGURE_CYCLE_COUNT,
+		STATE_READ_CYCLE_COUNT,
+		STATE_CONFIGURE_TMRC,
+		STATE_READ_TMRC,
+		NORMAL
+	};
+	InternalState internalState = InternalState::NONE;
+	bool commandExecuted = false;
+	RM3100::Rm3100PrimaryDataset primaryDataset;
+
+	uint8_t commandBuffer[10];
+	uint8_t commandBufferLen = 0;
+
+	uint8_t cmmRegValue = RM3100::CMM_VALUE;
+	uint8_t tmrcRegValue = RM3100::TMRC_DEFAULT_VALUE;
+	uint16_t cycleCountRegValueX = RM3100::CYCLE_COUNT_VALUE;
+	uint16_t cycleCountRegValueY = RM3100::CYCLE_COUNT_VALUE;
+	uint16_t cycleCountRegValueZ = RM3100::CYCLE_COUNT_VALUE;
+	float scaleFactorX = 1.0 / RM3100::DEFAULT_GAIN;
+	float scaleFactorY = 1.0 / RM3100::DEFAULT_GAIN;
+	float scaleFactorZ = 1.0 / RM3100::DEFAULT_GAIN;
+
+	bool goToNormalModeAtStartup = false;
+	uint8_t switchId;
+	uint32_t transitionDelay;
+
+	ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand,
+			const uint8_t *commandData,size_t commandDataLen);
+	ReturnValue_t handleCycleCommand(bool oneCycleValue,
+			const uint8_t *commandData, size_t commandDataLen);
+
+	ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand,
+			const uint8_t *commandData,size_t commandDataLen);
+
+	ReturnValue_t handleDataReadout(const uint8_t* packet);
+#if FSFW_HAL_RM3100_MGM_DEBUG == 1
+	PeriodicOperationDivider* debugDivider;
+#endif
+};
+
+#endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */

hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h

@@ -0,0 +1,178 @@
+#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_
+#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_
+
+#include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <fsfw/datapoollocal/LocalPoolVariable.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <cstdint>
+
+namespace MGMLIS3MDL {
+
+enum Set {
+    ON, OFF
+};
+enum OpMode {
+    LOW, MEDIUM, HIGH, ULTRA
+};
+
+enum Sensitivies: uint8_t {
+    GAUSS_4 = 4,
+    GAUSS_8 = 8,
+    GAUSS_12 = 12,
+    GAUSS_16 = 16
+};
+
+/* Actually 15, we just round up a bit */
+static constexpr size_t MAX_BUFFER_SIZE = 16;
+
+/* Field data register scaling */
+static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100;
+static constexpr float FIELD_LSB_PER_GAUSS_4_SENS = 1.0 / 6842.0;
+static constexpr float FIELD_LSB_PER_GAUSS_8_SENS = 1.0 / 3421.0;
+static constexpr float FIELD_LSB_PER_GAUSS_12_SENS = 1.0 / 2281.0;
+static constexpr float FIELD_LSB_PER_GAUSS_16_SENS = 1.0 / 1711.0;
+
+static const DeviceCommandId_t READ_CONFIG_AND_DATA = 0x00;
+static const DeviceCommandId_t SETUP_MGM = 0x01;
+static const DeviceCommandId_t READ_TEMPERATURE = 0x02;
+static const DeviceCommandId_t IDENTIFY_DEVICE = 0x03;
+static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x04;
+static const DeviceCommandId_t ACCURACY_OP_MODE_SET = 0x05;
+
+/* Number of all control registers */
+static const uint8_t NR_OF_CTRL_REGISTERS = 5;
+/* Number of registers in the MGM */
+static const uint8_t NR_OF_REGISTERS = 19;
+/* Total number of adresses for all registers */
+static const uint8_t TOTAL_NR_OF_ADRESSES = 52;
+static const uint8_t NR_OF_DATA_AND_CFG_REGISTERS = 14;
+static const uint8_t TEMPERATURE_REPLY_LEN = 3;
+static const uint8_t SETUP_REPLY_LEN = 6;
+
+/*------------------------------------------------------------------------*/
+/* Register adresses */
+/*------------------------------------------------------------------------*/
+/* Register adress returns identifier of device with default 0b00111101 */
+static const uint8_t IDENTIFY_DEVICE_REG_ADDR = 0b00001111;
+static const uint8_t DEVICE_ID = 0b00111101;            // Identifier for Device
+
+/* Register adress to access register 1 */
+static const uint8_t CTRL_REG1 = 0b00100000;
+/* Register adress to access register 2 */
+static const uint8_t CTRL_REG2 = 0b00100001;
+/* Register adress to access register 3 */
+static const uint8_t CTRL_REG3 = 0b00100010;
+/* Register adress to access register 4 */
+static const uint8_t CTRL_REG4 = 0b00100011;
+/* Register adress to access register 5 */
+static const uint8_t CTRL_REG5 = 0b00100100;
+
+/* Register adress to access status register */
+static const uint8_t STATUS_REG_IDX = 8;
+static const uint8_t STATUS_REG = 0b00100111;
+
+/* Register adress to access low byte of x-axis */
+static const uint8_t X_LOWBYTE_IDX = 9;
+static const uint8_t X_LOWBYTE = 0b00101000;
+/* Register adress to access high byte of x-axis */
+static const uint8_t X_HIGHBYTE_IDX = 10;
+static const uint8_t X_HIGHBYTE = 0b00101001;
+/* Register adress to access low byte of y-axis */
+static const uint8_t Y_LOWBYTE_IDX = 11;
+static const uint8_t Y_LOWBYTE = 0b00101010;
+/* Register adress to access high byte of y-axis */
+static const uint8_t Y_HIGHBYTE_IDX = 12;
+static const uint8_t Y_HIGHBYTE = 0b00101011;
+/* Register adress to access low byte of z-axis */
+static const uint8_t Z_LOWBYTE_IDX = 13;
+static const uint8_t Z_LOWBYTE = 0b00101100;
+/* Register adress to access high byte of z-axis */
+static const uint8_t Z_HIGHBYTE_IDX = 14;
+static const uint8_t Z_HIGHBYTE = 0b00101101;
+
+/* Register adress to access low byte of temperature sensor */
+static const uint8_t TEMP_LOWBYTE = 0b00101110;
+/* Register adress to access high byte of temperature sensor */
+static const uint8_t TEMP_HIGHBYTE = 0b00101111;
+
+/*------------------------------------------------------------------------*/
+/* Initialize Setup Register set bits                                     */
+/*------------------------------------------------------------------------*/
+/* General transfer bits */
+// Read=1 / Write=0 Bit
+static const uint8_t RW_BIT = 7;
+// Continous Read/Write Bit, increment adress
+static const uint8_t MS_BIT = 6;
+
+/* CTRL_REG1 bits */
+static const uint8_t ST = 0;        // Self test enable bit, enabled = 1
+// Enable rates higher than 80 Hz enabled = 1
+static const uint8_t FAST_ODR = 1;
+static const uint8_t DO0 = 2;       // Output data rate bit 2
+static const uint8_t DO1 = 3;       // Output data rate bit 3
+static const uint8_t DO2 = 4;       // Output data rate bit 4
+static const uint8_t OM0 = 5;       // XY operating mode bit 5
+static const uint8_t OM1 = 6;       // XY operating mode bit 6
+static const uint8_t TEMP_EN = 7;   // Temperature sensor enable enabled = 1
+static const uint8_t CTRL_REG1_DEFAULT = (1 << TEMP_EN) | (1 << OM1) |
+        (1 << DO0) | (1 << DO1) | (1 << DO2);
+
+/* CTRL_REG2 bits */
+//reset configuration registers and user registers
+static const uint8_t SOFT_RST = 2;
+static const uint8_t REBOOT = 3;    //reboot memory content
+static const uint8_t FSO = 5;       //full-scale selection bit 5
+static const uint8_t FS1 = 6;       //full-scale selection bit 6
+static const uint8_t CTRL_REG2_DEFAULT = 0;
+
+/* CTRL_REG3 bits */
+static const uint8_t MD0 = 0;       //Operating mode bit 0
+static const uint8_t MD1 = 1;       //Operating mode bit 1
+//SPI serial interface mode selection enabled = 3-wire-mode
+static const uint8_t SIM = 2;
+static const uint8_t LP = 5;        //low-power mode
+static const uint8_t CTRL_REG3_DEFAULT = 0;
+
+/* CTRL_REG4 bits */
+//big/little endian data selection enabled = MSb at lower adress
+static const uint8_t BLE = 1;
+static const uint8_t OMZ0 = 2;      //Z operating mode bit 2
+static const uint8_t OMZ1 = 3;      //Z operating mode bit 3
+static const uint8_t CTRL_REG4_DEFAULT = (1 << OMZ1);
+
+/* CTRL_REG5 bits */
+static const uint8_t BDU = 6;       //Block data update
+static const uint8_t FAST_READ = 7; //Fast read enabled = 1
+static const uint8_t CTRL_REG5_DEFAULT = 0;
+
+static const uint32_t MGM_DATA_SET_ID = READ_CONFIG_AND_DATA;
+
+enum MgmPoolIds: lp_id_t {
+    FIELD_STRENGTH_X,
+    FIELD_STRENGTH_Y,
+    FIELD_STRENGTH_Z,
+    TEMPERATURE_CELCIUS
+};
+
+class MgmPrimaryDataset: public StaticLocalDataSet<5> {
+public:
+    MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner):
+        StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {}
+
+    MgmPrimaryDataset(object_id_t mgmId):
+        StaticLocalDataSet(sid_t(mgmId, MGM_DATA_SET_ID)) {}
+
+    lp_var_t<float> fieldStrengthX = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_X, this);
+    lp_var_t<float> fieldStrengthY = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Y, this);
+    lp_var_t<float> fieldStrengthZ = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Z, this);
+    lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
+            TEMPERATURE_CELCIUS, this);
+};
+
+}
+
+
+#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ */

hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h

@@ -0,0 +1,132 @@
+#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_
+#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_
+
+#include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <fsfw/datapoollocal/LocalPoolVariable.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/serialize/SerialLinkedListAdapter.h>
+#include <cstdint>
+
+namespace RM3100 {
+
+/* Actually 10, we round up a little bit */
+static constexpr size_t MAX_BUFFER_SIZE = 12;
+
+static constexpr uint8_t READ_MASK = 0x80;
+
+/*----------------------------------------------------------------------------*/
+/* CMM Register */
+/*----------------------------------------------------------------------------*/
+static constexpr uint8_t SET_CMM_CMZ = 1 << 6;
+static constexpr uint8_t SET_CMM_CMY = 1 << 5;
+static constexpr uint8_t SET_CMM_CMX = 1 << 4;
+static constexpr uint8_t SET_CMM_DRDM = 1 << 2;
+static constexpr uint8_t SET_CMM_START = 1;
+static constexpr uint8_t CMM_REGISTER = 0x01;
+
+static constexpr uint8_t CMM_VALUE = SET_CMM_CMZ | SET_CMM_CMY | SET_CMM_CMX |
+		SET_CMM_DRDM | SET_CMM_START;
+
+/*----------------------------------------------------------------------------*/
+/* Cycle count register */
+/*----------------------------------------------------------------------------*/
+// Default value (200)
+static constexpr uint8_t CYCLE_COUNT_VALUE = 0xC8;
+
+static constexpr float DEFAULT_GAIN = static_cast<float>(CYCLE_COUNT_VALUE) /
+		100 * 38;
+static constexpr uint8_t CYCLE_COUNT_START_REGISTER = 0x04;
+
+/*----------------------------------------------------------------------------*/
+/* TMRC register */
+/*----------------------------------------------------------------------------*/
+static constexpr uint8_t TMRC_150HZ_VALUE = 0x94;
+static constexpr uint8_t TMRC_75HZ_VALUE = 0x95;
+static constexpr uint8_t TMRC_DEFAULT_37HZ_VALUE = 0x96;
+
+static constexpr uint8_t TMRC_REGISTER = 0x0B;
+static constexpr uint8_t TMRC_DEFAULT_VALUE = TMRC_DEFAULT_37HZ_VALUE;
+
+static constexpr uint8_t MEASUREMENT_REG_START = 0x24;
+static constexpr uint8_t BIST_REGISTER = 0x33;
+static constexpr uint8_t DATA_READY_VAL = 0b1000'0000;
+static constexpr uint8_t STATUS_REGISTER = 0x34;
+static constexpr uint8_t REVID_REGISTER = 0x36;
+
+// Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM)
+static constexpr uint16_t RANGE = 800;
+
+static constexpr DeviceCommandId_t READ_DATA = 0;
+
+static constexpr DeviceCommandId_t CONFIGURE_CMM = 1;
+static constexpr DeviceCommandId_t READ_CMM = 2;
+
+static constexpr DeviceCommandId_t CONFIGURE_TMRC = 3;
+static constexpr DeviceCommandId_t READ_TMRC = 4;
+
+static constexpr DeviceCommandId_t CONFIGURE_CYCLE_COUNT = 5;
+static constexpr DeviceCommandId_t READ_CYCLE_COUNT = 6;
+
+class CycleCountCommand: public SerialLinkedListAdapter<SerializeIF> {
+public:
+	CycleCountCommand(bool oneCycleCount = true): oneCycleCount(oneCycleCount) {
+		setLinks(oneCycleCount);
+	}
+
+	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+			Endianness streamEndianness) override {
+		ReturnValue_t result = SerialLinkedListAdapter::deSerialize(buffer,
+				size, streamEndianness);
+		if(oneCycleCount) {
+			cycleCountY = cycleCountX;
+			cycleCountZ = cycleCountX;
+		}
+		return result;
+	}
+
+	SerializeElement<uint16_t> cycleCountX;
+	SerializeElement<uint16_t> cycleCountY;
+	SerializeElement<uint16_t> cycleCountZ;
+
+private:
+	void setLinks(bool oneCycleCount) {
+		setStart(&cycleCountX);
+		if(not oneCycleCount) {
+			cycleCountX.setNext(&cycleCountY);
+			cycleCountY.setNext(&cycleCountZ);
+		}
+	}
+
+	bool oneCycleCount;
+};
+
+static constexpr uint32_t MGM_DATASET_ID = READ_DATA;
+
+enum MgmPoolIds: lp_id_t {
+    FIELD_STRENGTH_X,
+    FIELD_STRENGTH_Y,
+    FIELD_STRENGTH_Z,
+};
+
+class Rm3100PrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
+public:
+	Rm3100PrimaryDataset(HasLocalDataPoolIF* hkOwner):
+        StaticLocalDataSet(hkOwner, MGM_DATASET_ID) {}
+
+	Rm3100PrimaryDataset(object_id_t mgmId):
+        StaticLocalDataSet(sid_t(mgmId, MGM_DATASET_ID)) {}
+
+	// Field strengths in micro Tesla.
+    lp_var_t<float> fieldStrengthX = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_X, this);
+    lp_var_t<float> fieldStrengthY = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Y, this);
+    lp_var_t<float> fieldStrengthZ = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Z, this);
+};
+
+}
+
+
+
+#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ */

hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp

@@ -20,7 +20,7 @@ LinuxLibgpioIF::~LinuxLibgpioIF() {
 ReturnValue_t LinuxLibgpioIF::addGpios(GpioCookie* gpioCookie) {
     ReturnValue_t result;
     if(gpioCookie == nullptr) {
-        sif::error << "LinuxLibgpioIF::initialize: Invalid cookie" << std::endl;
+        sif::error << "LinuxLibgpioIF::addGpios: Invalid cookie" << std::endl;
         return RETURN_FAILED;
     }
 
@@ -45,16 +45,25 @@ ReturnValue_t LinuxLibgpioIF::addGpios(GpioCookie* gpioCookie) {
 
 ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) {
     for(auto& gpioConfig: mapToAdd) {
-        switch(gpioConfig.second->gpioType) {
+        auto& gpioType = gpioConfig.second->gpioType;
+        switch(gpioType) {
         case(gpio::GpioTypes::NONE): {
             return GPIO_INVALID_INSTANCE;
         }
-        case(gpio::GpioTypes::GPIO_REGULAR): {
-            GpiodRegular* regularGpio = dynamic_cast<GpiodRegular*>(gpioConfig.second);
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP): {
+            auto regularGpio = dynamic_cast<GpiodRegularByChip*>(gpioConfig.second);
             if(regularGpio == nullptr) {
                 return GPIO_INVALID_INSTANCE;
             }
-            configureRegularGpio(gpioConfig.first, regularGpio);
+            configureGpioByChip(gpioConfig.first, *regularGpio);
+            break;
+        }
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL):{
+            auto regularGpio = dynamic_cast<GpiodRegularByLabel*>(gpioConfig.second);
+            if(regularGpio == nullptr) {
+                return GPIO_INVALID_INSTANCE;
+            }
+            configureGpioByLabel(gpioConfig.first, *regularGpio);
             break;
         }
         case(gpio::GpioTypes::CALLBACK): {
@@ -70,41 +79,59 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) {
     return RETURN_OK;
 }
 
-ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, GpiodRegular *regularGpio) {
-    std::string chipname;
+ReturnValue_t LinuxLibgpioIF::configureGpioByLabel(gpioId_t gpioId,
+        GpiodRegularByLabel &gpioByLabel) {
+    std::string& label = gpioByLabel.label;
+    struct gpiod_chip* chip = gpiod_chip_open_by_label(label.c_str());
+    if (chip == nullptr) {
+        sif::warning << "LinuxLibgpioIF::configureRegularGpio: Failed to open gpio from gpio "
+                << "group with label " << label << ". Gpio ID: " << gpioId << std::endl;
+        return RETURN_FAILED;
+
+    }
+    std::string failOutput = "label: " + label;
+    return configureRegularGpio(gpioId, gpioByLabel.gpioType, chip, gpioByLabel, failOutput);
+}
+
+ReturnValue_t LinuxLibgpioIF::configureGpioByChip(gpioId_t gpioId,
+        GpiodRegularByChip &gpioByChip) {
+    std::string& chipname = gpioByChip.chipname;
+    struct gpiod_chip* chip = gpiod_chip_open_by_name(chipname.c_str());
+    if (chip == nullptr) {
+        sif::warning << "LinuxLibgpioIF::configureRegularGpio: Failed to open chip "
+                << chipname << ". Gpio ID: " << gpioId << std::endl;
+        return RETURN_FAILED;
+    }
+    std::string failOutput = "chipname: " + chipname;
+    return configureRegularGpio(gpioId, gpioByChip.gpioType, chip, gpioByChip, failOutput);
+}
+
+ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, gpio::GpioTypes gpioType,
+        struct gpiod_chip* chip, GpiodRegularBase& regularGpio, std::string failOutput) {
     unsigned int lineNum;
-    struct gpiod_chip *chip;
     gpio::Direction direction;
     std::string consumer;
     struct gpiod_line *lineHandle;
     int result = 0;
 
-    chipname = regularGpio->chipname;
-    chip = gpiod_chip_open_by_name(chipname.c_str());
-    if (!chip) {
-        sif::warning << "LinuxLibgpioIF::configureRegularGpio: Failed to open chip "
-                << chipname << ". Gpio ID: " << gpioId << std::endl;
-        return RETURN_FAILED;
-    }
-
-    lineNum = regularGpio->lineNum;
+    lineNum = regularGpio.lineNum;
     lineHandle = gpiod_chip_get_line(chip, lineNum);
     if (!lineHandle) {
-        sif::debug << "LinuxLibgpioIF::configureRegularGpio: Failed to open line " << std::endl;
-        sif::debug << "GPIO ID: " << gpioId << ", line number: " << lineNum <<
-                ", chipname: " << chipname << std::endl;
-        sif::debug << "Check if linux GPIO configuration has changed. " << std::endl;
+        sif::warning << "LinuxLibgpioIF::configureRegularGpio: Failed to open line " << std::endl;
+        sif::warning << "GPIO ID: " << gpioId << ", line number: " << lineNum <<
+                ", " << failOutput << std::endl;
+        sif::warning << "Check if Linux GPIO configuration has changed. " << std::endl;
         gpiod_chip_close(chip);
         return RETURN_FAILED;
     }
 
-    direction = regularGpio->direction;
-    consumer = regularGpio->consumer;
+    direction = regularGpio.direction;
+    consumer = regularGpio.consumer;
     /* Configure direction and add a description to the GPIO */
     switch (direction) {
     case(gpio::OUT): {
         result = gpiod_line_request_output(lineHandle, consumer.c_str(),
-                regularGpio->initValue);
+                regularGpio.initValue);
         if (result < 0) {
             sif::error << "LinuxLibgpioIF::configureRegularGpio: Failed to request line " << lineNum <<
                     " from GPIO instance with ID: " << gpioId << std::endl;
@@ -134,7 +161,7 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, GpiodRegular
      * Write line handle to GPIO configuration instance so it can later be used to set or
      * read states of GPIOs.
      */
-    regularGpio->lineHandle = lineHandle;
+    regularGpio.lineHandle = lineHandle;
     return RETURN_OK;
 }
 
@@ -145,8 +172,14 @@ ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) {
         return UNKNOWN_GPIO_ID;
     }
 
-    if(gpioMapIter->second->gpioType == gpio::GpioTypes::GPIO_REGULAR) {
-        return driveGpio(gpioId, dynamic_cast<GpiodRegular*>(gpioMapIter->second), 1);
+    auto gpioType = gpioMapIter->second->gpioType;
+    if(gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP or
+            gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL) {
+        auto regularGpio = dynamic_cast<GpiodRegularBase*>(gpioMapIter->second);
+        if(regularGpio == nullptr) {
+            return GPIO_TYPE_FAILURE;
+        }
+        return driveGpio(gpioId, *regularGpio, gpio::HIGH);
     }
     else {
         auto gpioCallback = dynamic_cast<GpioCallback*>(gpioMapIter->second);
@@ -167,8 +200,14 @@ ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) {
         return UNKNOWN_GPIO_ID;
     }
 
-    if(gpioMapIter->second->gpioType == gpio::GpioTypes::GPIO_REGULAR) {
-        return driveGpio(gpioId, dynamic_cast<GpiodRegular*>(gpioMapIter->second), 0);
+    auto& gpioType = gpioMapIter->second->gpioType;
+    if(gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP or
+            gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL) {
+        auto regularGpio = dynamic_cast<GpiodRegularBase*>(gpioMapIter->second);
+        if(regularGpio == nullptr) {
+            return GPIO_TYPE_FAILURE;
+        }
+        return driveGpio(gpioId, *regularGpio, gpio::LOW);
     }
     else {
         auto gpioCallback = dynamic_cast<GpioCallback*>(gpioMapIter->second);
@@ -183,12 +222,8 @@ ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) {
 }
 
 ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId,
-        GpiodRegular* regularGpio, unsigned int logicLevel) {
-    if(regularGpio == nullptr) {
-        return GPIO_TYPE_FAILURE;
-    }
-
-    int result = gpiod_line_set_value(regularGpio->lineHandle, logicLevel);
+        GpiodRegularBase& regularGpio, gpio::Levels logicLevel) {
+    int result = gpiod_line_set_value(regularGpio.lineHandle, logicLevel);
     if (result < 0) {
         sif::warning << "LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID " << gpioId <<
                 " to logic level " << logicLevel << std::endl;
@@ -204,9 +239,10 @@ ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) {
         sif::warning << "LinuxLibgpioIF::readGpio: Unknown GPIOD ID " << gpioId << std::endl;
         return UNKNOWN_GPIO_ID;
     }
-
-    if(gpioMapIter->second->gpioType == gpio::GpioTypes::GPIO_REGULAR) {
-        GpiodRegular* regularGpio = dynamic_cast<GpiodRegular*>(gpioMapIter->second);
+    auto gpioType = gpioMapIter->second->gpioType;
+    if(gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP or
+            gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL) {
+        auto regularGpio = dynamic_cast<GpiodRegularBase*>(gpioMapIter->second);
         if(regularGpio == nullptr) {
             return GPIO_TYPE_FAILURE;
         }
@@ -225,13 +261,14 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd){
     ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
     for(auto& gpioConfig: mapToAdd) {
         switch(gpioConfig.second->gpioType) {
-        case(gpio::GpioTypes::GPIO_REGULAR): {
-            auto regularGpio = dynamic_cast<GpiodRegular*>(gpioConfig.second);
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP):
+        case(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): {
+            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);
+            result = checkForConflictsRegularGpio(gpioConfig.first, *regularGpio, mapToAdd);
             if(result != HasReturnvaluesIF::RETURN_OK) {
                 status = result;
             }
@@ -259,17 +296,19 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd){
 
 
 ReturnValue_t LinuxLibgpioIF::checkForConflictsRegularGpio(gpioId_t gpioIdToCheck,
-        GpiodRegular* gpioToCheck, GpioMap& mapToAdd) {
+        GpiodRegularBase& gpioToCheck, GpioMap& mapToAdd) {
     /* Cross check with private map */
     gpioMapIter = gpioMap.find(gpioIdToCheck);
     if(gpioMapIter != gpioMap.end()) {
-        if(gpioMapIter->second->gpioType != gpio::GpioTypes::GPIO_REGULAR) {
+        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;
             mapToAdd.erase(gpioIdToCheck);
             return HasReturnvaluesIF::RETURN_OK;
         }
-        auto ownRegularGpio = dynamic_cast<GpiodRegular*>(gpioMapIter->second);
+        auto ownRegularGpio = dynamic_cast<GpiodRegularBase*>(gpioMapIter->second);
         if(ownRegularGpio == nullptr) {
             return GPIO_TYPE_FAILURE;
         }

hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h

@@ -6,6 +6,7 @@
 #include <fsfw/objectmanager/SystemObject.h>
 
 class GpioCookie;
+class GpiodRegularIF;
 
 /**
  * @brief	This class implements the GpioIF for a linux based system. The
@@ -47,9 +48,13 @@ private:
 	 * @param gpioId	The GPIO ID of the GPIO to drive.
 	 * @param logiclevel	The logic level to set. O or 1.
 	 */
-	ReturnValue_t driveGpio(gpioId_t gpioId, GpiodRegular* regularGpio, unsigned int logiclevel);
+	ReturnValue_t driveGpio(gpioId_t gpioId, GpiodRegularBase& regularGpio,
+	        gpio::Levels logicLevel);
 
-	ReturnValue_t configureRegularGpio(gpioId_t gpioId, GpiodRegular* regularGpio);
+	ReturnValue_t configureGpioByLabel(gpioId_t gpioId, GpiodRegularByLabel& gpioByLabel);
+	ReturnValue_t configureGpioByChip(gpioId_t gpioId, GpiodRegularByChip& gpioByChip);
+    ReturnValue_t configureRegularGpio(gpioId_t gpioId, gpio::GpioTypes gpioType,
+            struct gpiod_chip* chip, GpiodRegularBase& regularGpio, std::string failOutput);
 
 	/**
 	 * @brief	This function checks if GPIOs are already registered and whether
@@ -62,7 +67,7 @@ private:
 	 */
 	ReturnValue_t checkForConflicts(GpioMap& mapToAdd);
 
-	ReturnValue_t checkForConflictsRegularGpio(gpioId_t gpiodId, GpiodRegular* regularGpio,
+	ReturnValue_t checkForConflictsRegularGpio(gpioId_t gpiodId, GpiodRegularBase& regularGpio,
 	        GpioMap& mapToAdd);
     ReturnValue_t checkForConflictsCallbackGpio(gpioId_t gpiodId, GpioCallback* regularGpio,
             GpioMap& mapToAdd);

hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp

@@ -12,7 +12,7 @@ ReturnValue_t gpio::createRpiGpioConfig(GpioCookie* cookie, gpioId_t gpioId, int
         return HasReturnvaluesIF::RETURN_FAILED;
     }
 
-    GpiodRegular* config = new GpiodRegular();
+    auto config = new GpiodRegularByChip();
     /* Default chipname for Raspberry Pi. There is still gpiochip1 for expansion, but most users
     will not need this */
     config->chipname = "gpiochip0";

hal/src/fsfw_hal/linux/spi/SpiComIF.cpp

@@ -15,11 +15,6 @@
 #include <cerrno>
 #include <cstring>
 
-/* Can be used for low-level debugging of the SPI bus */
-#ifndef FSFW_HAL_LINUX_SPI_WIRETAPPING
-#define FSFW_HAL_LINUX_SPI_WIRETAPPING                  0
-#endif
-
 SpiComIF::SpiComIF(object_id_t objectId, GpioIF* gpioComIF):
         SystemObject(objectId), gpioComIF(gpioComIF) {
     if(gpioComIF == nullptr) {
@@ -193,7 +188,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie *spiCookie, const
     spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr);
     setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
     spiCookie->assignWriteBuffer(sendData);
-    spiCookie->assignTransferSize(sendLen);
+    spiCookie->setTransferSize(sendLen);
 
     bool fullDuplex = spiCookie->isFullDuplex();
     gpioId_t gpioId = spiCookie->getChipSelectPin();
@@ -202,12 +197,26 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie *spiCookie, const
     if(gpioId != gpio::NO_GPIO) {
         result = spiMutex->lockMutex(timeoutType, timeoutMs);
         if (result != RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
+#else
+            sif::printError("SpiComIF::sendMessage: Failed to lock mutex\n");
+#endif
+#endif
+            return result;
+        }
+        ReturnValue_t result = gpioComIF->pullLow(gpioId);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::warning << "SpiComIF::sendMessage: Pulling low CS pin failed" << std::endl;
+#else
+            sif::printWarning("SpiComIF::sendMessage: Pulling low CS pin failed");
+#endif
 #endif
             return result;
         }
-        gpioComIF->pullLow(gpioId);
     }
 
     /* Execute transfer */
@@ -335,6 +344,7 @@ ReturnValue_t SpiComIF::readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
 
     *buffer = rxBuf;
     *size = spiCookie->getCurrentTransferSize();
+    spiCookie->setTransferSize(0);
     return HasReturnvaluesIF::RETURN_OK;
 }
 

hal/src/fsfw_hal/linux/spi/SpiComIF.h

@@ -1,6 +1,7 @@
 #ifndef LINUX_SPI_SPICOMIF_H_
 #define LINUX_SPI_SPICOMIF_H_
 
+#include "fsfw/FSFW.h"
 #include "spiDefinitions.h"
 #include "returnvalues/classIds.h"
 #include "fsfw_hal/common/gpio/GpioIF.h"

hal/src/fsfw_hal/linux/spi/SpiCookie.cpp

@@ -121,7 +121,7 @@ bool SpiCookie::isFullDuplex() const {
     return not this->halfDuplex;
 }
 
-void SpiCookie::assignTransferSize(size_t transferSize) {
+void SpiCookie::setTransferSize(size_t transferSize) {
     spiTransferStruct.len = transferSize;
 }
 

hal/src/fsfw_hal/linux/spi/SpiCookie.h

@@ -103,10 +103,10 @@ public:
     void assignReadBuffer(uint8_t* rx);
     void assignWriteBuffer(const uint8_t* tx);
     /**
-     * Assign size for the next transfer.
+     * Set size for the next transfer. Set to 0 for no transfer
      * @param transferSize
      */
-    void assignTransferSize(size_t transferSize);
+    void setTransferSize(size_t transferSize);
     size_t getCurrentTransferSize() const;
 
     struct UncommonParameters {
@@ -158,8 +158,6 @@ private:
             std::string spiDev, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed,
             spi::send_callback_function_t callback, void* args);
 
-    size_t currentTransferSize = 0;
-
     address_t spiAddress;
     gpioId_t chipSelectPin;
     std::string spiDevice;

src/fsfw/FSFW.h.in

@@ -16,8 +16,25 @@
 #cmakedefine FSFW_ADD_MONITORING
 #cmakedefine FSFW_ADD_SGP4_PROPAGATOR
 
+#ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED
+#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
+#endif
+
 #ifndef FSFW_HAL_L3GD20_GYRO_DEBUG
-#define FSFW_HAL_L3GD20_GYRO_DEBUG   0
+#define FSFW_HAL_L3GD20_GYRO_DEBUG          0
 #endif /* FSFW_HAL_L3GD20_GYRO_DEBUG */
 
+#ifndef FSFW_HAL_RM3100_MGM_DEBUG
+#define FSFW_HAL_RM3100_MGM_DEBUG           0
+#endif /* FSFW_HAL_RM3100_MGM_DEBUG */
+
+#ifndef FSFW_HAL_LIS3MDL_MGM_DEBUG
+#define FSFW_HAL_LIS3MDL_MGM_DEBUG           0
+#endif /* FSFW_HAL_LIS3MDL_MGM_DEBUG */
+
 #endif /* FSFW_FSFW_H_ */

src/fsfw/devicehandlers/DeviceCommunicationIF.h

@@ -85,9 +85,10 @@ public:
 	 * Called by DHB in the GET_WRITE doGetWrite().
 	 * Get send confirmation that the data in sendMessage() was sent successfully.
 	 * @param cookie
-	 * @return - @c RETURN_OK if data was sent successfull
-	 * 		   - Everything else triggers falure event with
-	 * 		     returnvalue as parameter 1
+	 * @return
+	 *  - @c RETURN_OK if data was sent successfully but a reply is expected
+	 *  - NO_REPLY_EXPECTED if data was sent successfully and no reply is expected
+	 *  - Everything else to indicate failure
 	 */
 	virtual ReturnValue_t getSendSuccess(CookieIF *cookie) = 0;
 

src/fsfw/devicehandlers/DeviceHandlerBase.cpp

@@ -461,7 +461,7 @@ size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId){
         return iter->second.replyLen;
     }else{
         return 0;
-   }
+    }
 }
 
 ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceReply,
@@ -612,15 +612,15 @@ void DeviceHandlerBase::replyToReply(const DeviceCommandId_t command, DeviceRepl
     }
     DeviceCommandInfo* info = &replyInfo.command->second;
     if (info == nullptr){
-    	printWarningOrError(sif::OutputTypes::OUT_ERROR,
-    	                        "replyToReply", HasReturnvaluesIF::RETURN_FAILED,
-    	                        "Command pointer not found");
-    	return;
+        printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                "replyToReply", HasReturnvaluesIF::RETURN_FAILED,
+                "Command pointer not found");
+        return;
     }
 
     if (info->expectedReplies > 0){
-    	// Check before to avoid underflow
-    	info->expectedReplies--;
+        // Check before to avoid underflow
+        info->expectedReplies--;
     }
     // Check if more replies are expected. If so, do nothing.
     if (info->expectedReplies == 0) {
@@ -1355,10 +1355,20 @@ void DeviceHandlerBase::buildInternalCommand(void) {
         DeviceCommandMap::iterator iter = deviceCommandMap.find(
                 deviceCommandId);
         if (iter == deviceCommandMap.end()) {
+#if FSFW_VERBOSE_LEVEL >= 1
+            char output[36];
+            sprintf(output, "Command 0x%08x unknown",
+                    static_cast<unsigned int>(deviceCommandId));
+            // so we can track misconfigurations
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "buildInternalCommand",
+                    COMMAND_NOT_SUPPORTED,
+                    output);
+#endif
             result = COMMAND_NOT_SUPPORTED;
         }
         else if (iter->second.isExecuting) {
-#if FSFW_DISABLE_PRINTOUT == 0
+#if FSFW_VERBOSE_LEVEL >= 1
             char output[36];
             sprintf(output, "Command 0x%08x is executing",
                     static_cast<unsigned int>(deviceCommandId));
@@ -1569,7 +1579,7 @@ LocalDataPoolManager* DeviceHandlerBase::getHkManagerHandle() {
     return &poolManager;
 }
 
-MessageQueueId_t DeviceHandlerBase::getCommanderId(DeviceCommandId_t replyId) const {
+MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyId) const {
     auto commandIter = deviceCommandMap.find(replyId);
     if(commandIter == deviceCommandMap.end()) {
         return MessageQueueIF::NO_QUEUE;

src/fsfw/devicehandlers/DeviceHandlerBase.h

@@ -6,22 +6,22 @@
 #include "DeviceHandlerFailureIsolation.h"
 #include "DeviceHandlerThermalSet.h"
 
-#include "../serviceinterface/ServiceInterface.h"
-#include "../serviceinterface/serviceInterfaceDefintions.h"
-#include "../objectmanager/SystemObject.h"
-#include "../tasks/ExecutableObjectIF.h"
-#include "../returnvalues/HasReturnvaluesIF.h"
-#include "../action/HasActionsIF.h"
-#include "../datapool/PoolVariableIF.h"
-#include "../modes/HasModesIF.h"
-#include "../power/PowerSwitchIF.h"
-#include "../ipc/MessageQueueIF.h"
-#include "../tasks/PeriodicTaskIF.h"
-#include "../action/ActionHelper.h"
-#include "../health/HealthHelper.h"
-#include "../parameters/ParameterHelper.h"
-#include "../datapoollocal/HasLocalDataPoolIF.h"
-#include "../datapoollocal/LocalDataPoolManager.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
+#include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/tasks/ExecutableObjectIF.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+#include "fsfw/action/HasActionsIF.h"
+#include "fsfw/datapool/PoolVariableIF.h"
+#include "fsfw/modes/HasModesIF.h"
+#include "fsfw/power/PowerSwitchIF.h"
+#include "fsfw/ipc/MessageQueueIF.h"
+#include "fsfw/tasks/PeriodicTaskIF.h"
+#include "fsfw/action/ActionHelper.h"
+#include "fsfw/health/HealthHelper.h"
+#include "fsfw/parameters/ParameterHelper.h"
+#include "fsfw/datapoollocal/HasLocalDataPoolIF.h"
+#include "fsfw/datapoollocal/LocalDataPoolManager.h"
 
 #include <map>
 
@@ -399,7 +399,7 @@ protected:
      */
     virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
             const uint8_t *packet) = 0;
-    MessageQueueId_t getCommanderId(DeviceCommandId_t replyId) const;
+    MessageQueueId_t getCommanderQueueId(DeviceCommandId_t replyId) const;
     /**
      * Helper function to get pending command. This is useful for devices
      * like SPI sensors to identify the last sent command.

src/fsfw/devicehandlers/DeviceHandlerIF.h

@@ -120,7 +120,8 @@ public:
 	static const ReturnValue_t WRONG_MODE_FOR_COMMAND = MAKE_RETURN_CODE(0xA5);
 	static const ReturnValue_t TIMEOUT = MAKE_RETURN_CODE(0xA6);
 	static const ReturnValue_t BUSY = MAKE_RETURN_CODE(0xA7);
-	static const ReturnValue_t NO_REPLY_EXPECTED = MAKE_RETURN_CODE(0xA8); //!< Used to indicate that this is a command-only command.
+	//!< Used to indicate that this is a command-only command.
+	static const ReturnValue_t NO_REPLY_EXPECTED = MAKE_RETURN_CODE(0xA8);
 	static const ReturnValue_t NON_OP_TEMPERATURE = MAKE_RETURN_CODE(0xA9);
 	static const ReturnValue_t COMMAND_NOT_IMPLEMENTED = MAKE_RETURN_CODE(0xAA);
 

src/fsfw/globalfunctions/PeriodicOperationDivider.cpp

@@ -2,43 +2,43 @@
 
 
 PeriodicOperationDivider::PeriodicOperationDivider(uint32_t divider,
-		bool resetAutomatically): resetAutomatically(resetAutomatically),
-		counter(divider), divider(divider) {
+        bool resetAutomatically): resetAutomatically(resetAutomatically),
+        counter(divider), divider(divider) {
 }
 
 bool PeriodicOperationDivider::checkAndIncrement() {
-	bool opNecessary = check();
-	if(opNecessary) {
-		if(resetAutomatically) {
-			counter = 0;
-		}
-		return opNecessary;
-	}
-	counter ++;
-	return opNecessary;
+    bool opNecessary = check();
+    if(opNecessary) {
+        if(resetAutomatically) {
+            counter = 1;
+        }
+        return opNecessary;
+    }
+    counter++;
+    return opNecessary;
 }
 
 bool PeriodicOperationDivider::check() {
-	if(counter >= divider) {
-		return true;
-	}
-	return false;
+    if(counter >= divider) {
+        return true;
+    }
+    return false;
 }
 
 
 
 void PeriodicOperationDivider::resetCounter() {
-	counter = 0;
+    counter = 0;
 }
 
 void PeriodicOperationDivider::setDivider(uint32_t newDivider) {
-	divider = newDivider;
+    divider = newDivider;
 }
 
 uint32_t PeriodicOperationDivider::getCounter() const {
-	return counter;
+    return counter;
 }
 
 uint32_t PeriodicOperationDivider::getDivider() const {
-	return divider;
+    return divider;
 }

src/fsfw/globalfunctions/PeriodicOperationDivider.h

@@ -13,51 +13,51 @@
  */
 class PeriodicOperationDivider {
 public:
-	/**
-	 * Initialize with the desired divider and specify whether the internal
-	 * counter will be reset automatically.
-	 * @param divider
-	 * @param resetAutomatically
-	 */
-	PeriodicOperationDivider(uint32_t divider, bool resetAutomatically = true);
+    /**
+     * Initialize with the desired divider and specify whether the internal
+     * counter will be reset automatically.
+     * @param divider
+     * @param resetAutomatically
+     */
+    PeriodicOperationDivider(uint32_t divider, bool resetAutomatically = true);
 
 
-	/**
-	 * Check whether operation is necessary.
-	 * If an operation is necessary and the class has been
-	 * configured to be reset automatically, the counter will be reset.
-	 *
-	 * @return
-	 * -@c true if the counter is larger or equal to the divider
-	 * -@c false otherwise
-	 */
-	bool checkAndIncrement();
+    /**
+     * Check whether operation is necessary.
+     * If an operation is necessary and the class has been
+     * configured to be reset automatically, the counter will be reset.
+     *
+     * @return
+     * -@c true if the counter is larger or equal to the divider
+     * -@c false otherwise
+     */
+    bool checkAndIncrement();
 
-	/**
-	 * Checks whether an operation is necessary.
-	 * This function will not increment the counter!
-	 * @return
-	 * -@c true if the counter is larger or equal to the divider
-	 * -@c false otherwise
-	 */
-	bool check();
+    /**
+     * Checks whether an operation is necessary.
+     * This function will not increment the counter!
+     * @return
+     * -@c true if the counter is larger or equal to the divider
+     * -@c false otherwise
+     */
+    bool check();
 
-	/**
-	 * Can be used to reset the counter to 0 manually.
-	 */
-	void resetCounter();
-	uint32_t getCounter() const;
+    /**
+     * Can be used to reset the counter to 0 manually.
+     */
+    void resetCounter();
+    uint32_t getCounter() const;
 
-	/**
-	 * Can be used to set a new divider value.
-	 * @param newDivider
-	 */
-	void setDivider(uint32_t newDivider);
-	uint32_t getDivider() const;
+    /**
+     * Can be used to set a new divider value.
+     * @param newDivider
+     */
+    void setDivider(uint32_t newDivider);
+    uint32_t getDivider() const;
 private:
-	bool resetAutomatically = true;
-	uint32_t counter = 0;
-	uint32_t divider = 0;
+    bool resetAutomatically = true;
+    uint32_t counter = 0;
+    uint32_t divider = 0;
 };
 
 

src/fsfw/memory/HasFileSystemIF.h

@@ -81,7 +81,7 @@ public:
 	 * @param args Any other arguments which an implementation might require
 	 * @return
 	 */
-	virtual ReturnValue_t deleteFile(const char* repositoryPath,
+	virtual ReturnValue_t removeFile(const char* repositoryPath,
 	        const char* filename, void* args = nullptr) = 0;
 
 	/**

src/fsfw/osal/common/TcpTmTcServer.cpp

@@ -1,8 +1,10 @@
-#include "fsfw/osal/common/TcpTmTcServer.h"
-#include "fsfw/osal/common/TcpTmTcBridge.h"
-#include "fsfw/osal/common/tcpipHelpers.h"
-
 #include "fsfw/platform.h"
+#include "fsfw/FSFW.h"
+
+#include "TcpTmTcServer.h"
+#include "TcpTmTcBridge.h"
+#include "tcpipHelpers.h"
+
 #include "fsfw/container/SharedRingBuffer.h"
 #include "fsfw/ipc/MessageQueueSenderIF.h"
 #include "fsfw/ipc/MutexGuard.h"

src/fsfw/osal/linux/CMakeLists.txt

@@ -16,6 +16,7 @@ target_sources(${LIB_FSFW_NAME}
 		Timer.cpp
 		tcpipHelpers.cpp
 		unixUtility.cpp
+		CommandExecutor.cpp
 )
 
 find_package(Threads REQUIRED)

src/fsfw/osal/linux/CommandExecutor.cpp

@@ -0,0 +1,185 @@
+#include "CommandExecutor.h"
+
+#include "fsfw/serviceinterface.h"
+#include "fsfw/container/SimpleRingBuffer.h"
+#include "fsfw/container/DynamicFIFO.h"
+
+#include <unistd.h>
+
+#include <cstring>
+
+CommandExecutor::CommandExecutor(const size_t maxSize):
+readVec(maxSize) {
+    waiter.events = POLLIN;
+}
+
+ReturnValue_t CommandExecutor::load(std::string command, bool blocking, bool printOutput) {
+    if(state == States::PENDING) {
+        return COMMAND_PENDING;
+    }
+
+    currentCmd = command;
+    this->blocking = blocking;
+    this->printOutput = printOutput;
+    if(state == States::IDLE) {
+        state = States::COMMAND_LOADED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CommandExecutor::execute() {
+    if(state == States::IDLE) {
+        return NO_COMMAND_LOADED_OR_PENDING;
+    }
+    else if(state == States::PENDING) {
+        return COMMAND_PENDING;
+    }
+    currentCmdFile = popen(currentCmd.c_str(), "r");
+    if(currentCmdFile == nullptr) {
+        lastError = errno;
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    if(blocking) {
+        return executeBlocking();
+    }
+    else {
+        currentFd = fileno(currentCmdFile);
+        waiter.fd = currentFd;
+    }
+    state = States::PENDING;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CommandExecutor::close() {
+    if(state == States::PENDING) {
+        // Attempt to close process, irrespective of if it is running or not
+        if(currentCmdFile != nullptr) {
+            pclose(currentCmdFile);
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void CommandExecutor::printLastError(std::string funcName) const {
+    if(lastError != 0) {
+        sif::error << funcName << " pclose failed with code " <<
+                lastError << ": " << strerror(lastError) << std::endl;
+    }
+}
+
+void CommandExecutor::setRingBuffer(SimpleRingBuffer *ringBuffer,
+        DynamicFIFO<uint16_t>* sizesFifo) {
+    this->ringBuffer = ringBuffer;
+    this->sizesFifo = sizesFifo;
+}
+
+ReturnValue_t CommandExecutor::check(bool& bytesRead) {
+    if(blocking) {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    switch(state) {
+    case(States::IDLE):
+    case(States::COMMAND_LOADED): {
+        return NO_COMMAND_LOADED_OR_PENDING;
+    }
+    case(States::PENDING): {
+        break;
+    }
+    }
+
+    int result = poll(&waiter, 1, 0);
+    switch(result) {
+    case(0): {
+        return HasReturnvaluesIF::RETURN_OK;
+        break;
+    }
+    case(1): {
+        if (waiter.revents & POLLIN) {
+            ssize_t readBytes = read(currentFd, readVec.data(), readVec.size());
+            if(readBytes == 0) {
+                // Should not happen
+                sif::warning << "CommandExecutor::check: "
+                        "No bytes read after poll event.." << std::endl;
+                break;
+            }
+            else if(readBytes > 0) {
+                bytesRead = true;
+                if(printOutput) {
+                    // It is assumed the command output is line terminated
+                    sif::info << currentCmd << " | " << readVec.data();
+                }
+                if(ringBuffer != nullptr) {
+                    ringBuffer->writeData(reinterpret_cast<const uint8_t*>(
+                            readVec.data()), readBytes);
+                }
+                if(sizesFifo != nullptr) {
+                    if(not sizesFifo->full()) {
+                        sizesFifo->insert(readBytes);
+                    }
+                }
+                return BYTES_READ;
+            }
+            else {
+                // Should also not happen
+                sif::warning << "CommandExecutor::check: Error " << errno << ": " <<
+                        strerror(errno) << std::endl;
+            }
+        }
+        else if(waiter.revents & POLLERR) {
+            sif::warning << "CommandExecuter::check: Poll error" << std::endl;
+            return COMMAND_ERROR;
+        }
+        else if(waiter.revents & POLLHUP) {
+            int result = pclose(currentCmdFile);
+            if(result != 0) {
+                lastError = result;
+                return HasReturnvaluesIF::RETURN_FAILED;
+            }
+            state = States::IDLE;
+            currentCmdFile = nullptr;
+            currentFd = 0;
+            return EXECUTION_FINISHED;
+        }
+        break;
+    }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void CommandExecutor::reset() {
+    CommandExecutor::close();
+    currentCmdFile = nullptr;
+    currentFd = 0;
+    state = States::IDLE;
+}
+
+int CommandExecutor::getLastError() const {
+    return this->lastError;
+}
+
+CommandExecutor::States CommandExecutor::getCurrentState() const {
+    return state;
+}
+
+ReturnValue_t CommandExecutor::executeBlocking() {
+    while(fgets(readVec.data(), readVec.size(), currentCmdFile) != nullptr) {
+        std::string output(readVec.data());
+        if(printOutput) {
+            sif::info << currentCmd << " | " << output;
+        }
+        if(ringBuffer != nullptr) {
+            ringBuffer->writeData(reinterpret_cast<const uint8_t*>(output.data()), output.size());
+        }
+        if(sizesFifo != nullptr) {
+            if(not sizesFifo->full()) {
+                sizesFifo->insert(output.size());
+            }
+        }
+    }
+    int result = pclose(currentCmdFile);
+    if(result != 0) {
+        lastError = result;
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}

src/fsfw/osal/linux/CommandExecutor.h

@@ -0,0 +1,134 @@
+#ifndef FSFW_SRC_FSFW_OSAL_LINUX_COMMANDEXECUTOR_H_
+#define FSFW_SRC_FSFW_OSAL_LINUX_COMMANDEXECUTOR_H_
+
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+#include "fsfw/returnvalues/FwClassIds.h"
+
+#include <poll.h>
+
+#include <string>
+#include <vector>
+
+class SimpleRingBuffer;
+template <typename T> class DynamicFIFO;
+
+/**
+ * @brief   Helper class to execute shell commands in blocking and non-blocking mode
+ * @details
+ * This class is able to execute processes by using the Linux popen call. It also has the
+ * capability of writing the read output of a process into a provided ring buffer.
+ *
+ * The executor works by first loading the command which should be executed and specifying
+ * whether it should be executed blocking or non-blocking. After that, execution can be started
+ * with the execute command. In blocking mode, the execute command will block until the command
+ * has finished
+ */
+class CommandExecutor {
+public:
+    enum class States {
+        IDLE,
+        COMMAND_LOADED,
+        PENDING
+    };
+
+    static constexpr uint8_t CLASS_ID = CLASS_ID::LINUX_OSAL;
+
+    //! [EXPORT] : [COMMENT] Execution of the current command has finished
+    static constexpr ReturnValue_t EXECUTION_FINISHED =
+            HasReturnvaluesIF::makeReturnCode(CLASS_ID, 0);
+
+    //! [EXPORT] : [COMMENT] Command is pending. This will also be returned if the user tries
+    //! to load another command but a command is still pending
+    static constexpr ReturnValue_t COMMAND_PENDING =
+            HasReturnvaluesIF::makeReturnCode(CLASS_ID, 1);
+    //! [EXPORT] : [COMMENT] Some bytes have been read from the executing process
+    static constexpr ReturnValue_t BYTES_READ =
+            HasReturnvaluesIF::makeReturnCode(CLASS_ID, 2);
+    //! [EXPORT] : [COMMENT] Command execution failed
+    static constexpr ReturnValue_t COMMAND_ERROR =
+            HasReturnvaluesIF::makeReturnCode(CLASS_ID, 3);
+    //! [EXPORT] : [COMMENT]
+    static constexpr ReturnValue_t NO_COMMAND_LOADED_OR_PENDING =
+            HasReturnvaluesIF::makeReturnCode(CLASS_ID, 4);
+    static constexpr ReturnValue_t PCLOSE_CALL_ERROR =
+            HasReturnvaluesIF::makeReturnCode(CLASS_ID, 6);
+
+    /**
+     * Constructor. Is initialized with maximum size of internal buffer to read data from the
+     * executed process.
+     * @param maxSize
+     */
+    CommandExecutor(const size_t maxSize);
+
+    /**
+     * Load a new command which should be executed
+     * @param command
+     * @param blocking
+     * @param printOutput
+     * @return
+     */
+    ReturnValue_t load(std::string command, bool blocking, bool printOutput = true);
+    /**
+     * Execute the loaded command.
+     * @return
+     *  - In blocking mode, it will return RETURN_FAILED if
+     *    the result of the system call was not 0. The error value can be accessed using
+     *    getLastError
+     *  - In non-blocking mode, this call will start
+     *    the execution and then return RETURN_OK
+     */
+    ReturnValue_t execute();
+    /**
+     * Only used in non-blocking mode. Checks the currently running command.
+     * @param bytesRead Will be set to the number of bytes read, if bytes have been read
+     * @return
+     *  - BYTES_READ if bytes have been read from the executing process. It is recommended to call
+     *    check again after this
+     *  - RETURN_OK execution is pending, but no bytes have been read from the executing process
+     *  - RETURN_FAILED if execution has failed, error value can be accessed using getLastError
+     *  - EXECUTION_FINISHED if the process was executed successfully
+     *  - COMMAND_ERROR internal poll error
+     */
+    ReturnValue_t check(bool& bytesRead);
+    /**
+     * Abort the current command. Should normally not be necessary, check can be used to find
+     * out whether command execution was successful
+     * @return RETURN_OK
+     */
+    ReturnValue_t close();
+
+    States getCurrentState() const;
+    int getLastError() const;
+    void printLastError(std::string funcName) const;
+
+    /**
+     * Assign a ring buffer and a FIFO which will be filled by the executor with the output
+     * read from the started process
+     * @param ringBuffer
+     * @param sizesFifo
+     */
+    void setRingBuffer(SimpleRingBuffer* ringBuffer, DynamicFIFO<uint16_t>* sizesFifo);
+
+    /**
+     * Reset the executor. This calls close internally and then reset the state machine so new
+     * commands can be loaded and executed
+     */
+    void reset();
+private:
+    std::string currentCmd;
+    bool blocking = true;
+    FILE* currentCmdFile = nullptr;
+    int currentFd = 0;
+    bool printOutput = true;
+    std::vector<char> readVec;
+    struct pollfd waiter {};
+    SimpleRingBuffer* ringBuffer = nullptr;
+    DynamicFIFO<uint16_t>* sizesFifo = nullptr;
+
+    States state = States::IDLE;
+    int lastError = 0;
+
+    ReturnValue_t executeBlocking();
+};
+
+#endif /* FSFW_SRC_FSFW_OSAL_LINUX_COMMANDEXECUTOR_H_ */

src/fsfw/osal/linux/MessageQueue.cpp

@@ -285,10 +285,10 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 
             utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EBADF");
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::warning << "mq_send to: " << sendTo << " sent from "
-                    << sentFrom << "failed" << std::endl;
+            sif::warning << "mq_send to " << sendTo << " sent from "
+                    << sentFrom << " failed" << std::endl;
 #else
-            sif::printWarning("mq_send to: %d sent from %d failed\n", sendTo, sentFrom);
+            sif::printWarning("mq_send to %d sent from %d failed\n", sendTo, sentFrom);
 #endif
             return DESTINATION_INVALID;
         }

src/fsfw/osal/linux/Timer.cpp

@@ -27,6 +27,7 @@ int Timer::setTimer(uint32_t intervalMs) {
 	timer.it_value.tv_nsec = (intervalMs * 1000000) % (1000000000);
 	timer.it_interval.tv_sec = 0;
 	timer.it_interval.tv_nsec = 0;
+	set = true;
 	return timer_settime(timerId, 0, &timer, NULL);
 }
 
@@ -43,3 +44,14 @@ int Timer::getTimer(uint32_t* remainingTimeMs){
 
 	return status;
 }
+
+bool Timer::isSet() const {
+    return this->set;
+}
+
+void Timer::resetTimer() {
+    if(not this->set) {
+        set = false;
+    }
+    setTimer(0);
+}

src/fsfw/osal/linux/Timer.h

@@ -38,7 +38,11 @@ public:
 	 */
 	int getTimer(uint32_t* remainingTimeMs);
 
+	bool isSet() const;
+	void resetTimer();
+
 private:
+	bool set = true;
 	timer_t timerId;
 };
 

src/fsfw/pus/Service5EventReporting.cpp

@@ -41,8 +41,7 @@ ReturnValue_t Service5EventReporting::performService() {
 	    }
 	}
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	sif::debug <<  "Service5EventReporting::generateEventReport:"
-	        " Too many events" << std::endl;
+	sif::warning <<  "Service5EventReporting::generateEventReport: Too many events" << std::endl;
 #endif
 	return HasReturnvaluesIF::RETURN_OK;
 }
@@ -64,8 +63,11 @@ ReturnValue_t Service5EventReporting::generateEventReport(
 	        requestQueue->getDefaultDestination(),requestQueue->getId());
 	if(result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "Service5EventReporting::generateEventReport:"
-		        " Could not send TM packet" << std::endl;
+		sif::warning << "Service5EventReporting::generateEventReport: "
+		        "Could not send TM packet" << std::endl;
+#else
+		sif::printWarning("Service5EventReporting::generateEventReport: "
+                "Could not send TM packet\n");
 #endif
 	}
 	return result;

src/fsfw/pus/Service8FunctionManagement.cpp

@@ -33,8 +33,8 @@ ReturnValue_t Service8FunctionManagement::getMessageQueueAndObject(
 	if(tcDataLen < sizeof(object_id_t)) {
 		return CommandingServiceBase::INVALID_TC;
 	}
-	SerializeAdapter::deSerialize(objectId, &tcData,
-			&tcDataLen, SerializeIF::Endianness::BIG);
+	// Can't fail, size was checked before
+	SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
 
 	return checkInterfaceAndAcquireMessageQueue(id,objectId);
 }

src/fsfw/returnvalues/FwClassIds.h

@@ -72,6 +72,7 @@ enum: uint8_t {
     PUS_SERVICE_3, //PUS3
     PUS_SERVICE_9, //PUS9
     FILE_SYSTEM, //FILS
+    LINUX_OSAL, //UXOS
     HAL_SPI, //HSPI
     HAL_UART, //HURT
     HAL_I2C, //HI2C

src/fsfw/returnvalues/HasReturnvaluesIF.h

@@ -22,9 +22,9 @@ public:
 	 * @param number
 	 * @return
 	 */
-	static constexpr ReturnValue_t makeReturnCode(uint8_t interfaceId,
+	static constexpr ReturnValue_t makeReturnCode(uint8_t classId,
 			uint8_t number) {
-	    return (static_cast<ReturnValue_t>(interfaceId) << 8) + number;
+	    return (static_cast<ReturnValue_t>(classId) << 8) + number;
 	}
 };
 

src/fsfw/tasks/Typedef.h

@@ -1,5 +1,8 @@
-#ifndef FRAMEWORK_TASKS_TYPEDEF_H_
-#define FRAMEWORK_TASKS_TYPEDEF_H_
+#ifndef FSFW_TASKS_TYPEDEF_H_
+#define FSFW_TASKS_TYPEDEF_H_
+
+#include <cstdint>
+#include <cstddef>
 
 typedef const char* TaskName;
 typedef uint32_t TaskPriority;
@@ -7,4 +10,4 @@ typedef size_t TaskStackSize;
 typedef double TaskPeriod;
 typedef void (*TaskDeadlineMissedFunction)();
 
-#endif /* FRAMEWORK_TASKS_TYPEDEF_H_ */
+#endif /* FSFW_TASKS_TYPEDEF_H_ */

src/fsfw/tmtcpacket/SpacePacketBase.cpp

@@ -3,8 +3,8 @@
 
 #include <cstring>
 
-SpacePacketBase::SpacePacketBase( const uint8_t* set_address ) {
-	this->data = (SpacePacketPointer*) set_address;
+SpacePacketBase::SpacePacketBase(const uint8_t* setAddress) {
+	this->data = reinterpret_cast<SpacePacketPointer*>(const_cast<uint8_t*>(setAddress));
 }
 
 SpacePacketBase::~SpacePacketBase() {
@@ -15,10 +15,21 @@ uint8_t SpacePacketBase::getPacketVersionNumber( void ) {
 	return (this->data->header.packet_id_h & 0b11100000) >> 5;
 }
 
-void SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
+ReturnValue_t SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
 		bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) {
+    if(data == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "SpacePacketBase::initSpacePacketHeader: Data pointer is invalid"
+                << std::endl;
+#else
+        sif::printWarning("SpacePacketBase::initSpacePacketHeader: Data pointer is invalid!\n");
+#endif
+#endif
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 	//reset header to zero:
-	memset(data,0, sizeof(this->data->header) );
+	memset(data, 0, sizeof(this->data->header) );
 	//Set TC/TM bit.
 	data->header.packet_id_h = ((isTelecommand? 1 : 0)) << 4;
 	//Set secondaryHeader bit
@@ -27,7 +38,7 @@ void SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
 	//Always initialize as standalone packets.
 	data->header.sequence_control_h = 0b11000000;
 	setPacketSequenceCount(sequenceCount);
-
+	return HasReturnvaluesIF::RETURN_OK;
 }
 
 bool SpacePacketBase::isTelecommand( void ) {
@@ -54,6 +65,11 @@ void SpacePacketBase::setAPID( uint16_t new_apid ) {
 	this->data->header.packet_id_l = ( new_apid & 0x00FF );
 }
 
+void SpacePacketBase::setSequenceFlags( uint8_t sequenceflags ) {
+    this->data->header.sequence_control_h &= 0x3F;
+    this->data->header.sequence_control_h |= sequenceflags << 6;
+}
+
 uint16_t SpacePacketBase::getPacketSequenceControl( void ) {
 	return ( (this->data->header.sequence_control_h) << 8 )
 		+ this->data->header.sequence_control_l;

src/fsfw/tmtcpacket/SpacePacketBase.h

@@ -2,6 +2,8 @@
 #define FSFW_TMTCPACKET_SPACEPACKETBASE_H_
 
 #include "ccsds_header.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+
 #include <cstddef>
 
 /**
@@ -82,7 +84,7 @@ public:
 	 */
 	bool isTelecommand( void );
 
-	void initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
+	ReturnValue_t initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
 	        uint16_t apid, uint16_t sequenceCount = 0);
 	/**
 	 * The CCSDS header provides a secondary header flag (the fifth-highest bit),
@@ -109,6 +111,13 @@ public:
 	 * 			ignored.
 	 */
 	void setAPID( uint16_t setAPID );
+
+	/**
+     * Sets the sequence flags of a packet, which are bit 17 and 18 in the space packet header.
+     * @param   The sequence flags to set
+     */
+    void setSequenceFlags( uint8_t sequenceflags );
+
 	/**
 	 * Returns the CCSDS packet sequence control field, which are the third and
 	 * the fourth byte of the CCSDS primary header.

src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp

@@ -53,11 +53,14 @@ uint8_t* TmPacketPusC::getPacketTimeRaw() const{
 
 }
 
-void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
+ReturnValue_t TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
         uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId,
         uint8_t timeRefField) {
     //Set primary header:
-    initSpacePacketHeader(false, true, apid);
+    ReturnValue_t result = initSpacePacketHeader(false, true, apid);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
     //Set data Field Header:
     //First, set to zero.
     memset(&tmData->dataField, 0, sizeof(tmData->dataField));
@@ -76,6 +79,7 @@ void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
         timeStamper->addTimeStamp(tmData->dataField.time,
                 sizeof(tmData->dataField.time));
     }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void TmPacketPusC::setSourceDataSize(uint16_t size) {

src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h

@@ -100,7 +100,7 @@ protected:
      * @param subservice PUS Subservice
      * @param packetSubcounter Additional subcounter used.
      */
-    void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
+    ReturnValue_t initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
             uint16_t packetSubcounter, uint16_t destinationId = 0, uint8_t timeRefField = 0);
 
     /**

src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp

@@ -43,27 +43,55 @@ TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
         return;
     }
     size_t sourceDataSize = 0;
-    if (content != NULL) {
+    if (content != nullptr) {
         sourceDataSize += content->getSerializedSize();
     }
-    if (header != NULL) {
+    if (header != nullptr) {
         sourceDataSize += header->getSerializedSize();
     }
-    uint8_t *p_data = NULL;
-    ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
-            (getPacketMinimumSize() + sourceDataSize), &p_data);
+    uint8_t *pData = nullptr;
+    size_t sizeToReserve = getPacketMinimumSize() + sourceDataSize;
+    ReturnValue_t returnValue = store->getFreeElement(&storeAddress, sizeToReserve, &pData);
     if (returnValue != store->RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+        switch(returnValue) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        case(StorageManagerIF::DATA_STORAGE_FULL): {
+            sif::warning << "TmPacketStoredPusC::TmPacketStoredPusC: Store full for packet with "
+                    "size " << sizeToReserve << std::endl;
+            break;
+        }
+        case(StorageManagerIF::DATA_TOO_LARGE): {
+            sif::warning << "TmPacketStoredPusC::TmPacketStoredPusC: Data with size " <<
+                    sizeToReserve << " too large" <<  std::endl;
+            break;
+        }
+#else
+        case(StorageManagerIF::DATA_STORAGE_FULL): {
+            sif::printWarning("TmPacketStoredPusC::TmPacketStoredPusC: Store full for packet with "
+                    "size %d\n", sizeToReserve);
+            break;
+        }
+        case(StorageManagerIF::DATA_TOO_LARGE): {
+            sif::printWarning("TmPacketStoredPusC::TmPacketStoredPusC: Data with size "
+                    "%d too large\n", sizeToReserve);
+            break;
+        }
+#endif
+#endif
+        }
         TmPacketStoredBase::checkAndReportLostTm();
+        return;
     }
-    setData(p_data);
+    setData(pData);
     initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField);
     uint8_t *putDataHere = getSourceData();
     size_t size = 0;
-    if (header != NULL) {
+    if (header != nullptr) {
         header->serialize(&putDataHere, &size, sourceDataSize,
                 SerializeIF::Endianness::BIG);
     }
-    if (content != NULL) {
+    if (content != nullptr) {
         content->serialize(&putDataHere, &size, sourceDataSize,
                 SerializeIF::Endianness::BIG);
     }

src/fsfw/tmtcservices/TmTcBridge.h

@@ -19,7 +19,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
 public:
 	static constexpr uint8_t TMTC_RECEPTION_QUEUE_DEPTH = 20;
 	static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15;
-	static constexpr uint8_t LIMIT_DOWNLINK_PACKETS_STORED = 20;
+	static constexpr uint8_t LIMIT_DOWNLINK_PACKETS_STORED = 200;
 
 	static constexpr uint8_t DEFAULT_STORED_DATA_SENT_PER_CYCLE = 5;
 	static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10;