restructure repository

src/core/action/ActionHelper.cpp

@@ -0,0 +1,167 @@
+#include "ActionHelper.h"
+#include "HasActionsIF.h"
+
+#include "../ipc/MessageQueueSenderIF.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../serviceinterface/ServiceInterface.h"
+
+ActionHelper::ActionHelper(HasActionsIF* setOwner,
+        MessageQueueIF* useThisQueue) :
+        owner(setOwner), queueToUse(useThisQueue) {
+}
+
+ActionHelper::~ActionHelper() {
+}
+
+ReturnValue_t ActionHelper::handleActionMessage(CommandMessage* command) {
+    if (command->getCommand() == ActionMessage::EXECUTE_ACTION) {
+        ActionId_t currentAction = ActionMessage::getActionId(command);
+        prepareExecution(command->getSender(), currentAction,
+                ActionMessage::getStoreId(command));
+        return HasReturnvaluesIF::RETURN_OK;
+    } else {
+        return CommandMessage::UNKNOWN_COMMAND;
+    }
+}
+
+ReturnValue_t ActionHelper::initialize(MessageQueueIF* queueToUse_) {
+    ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+    if (ipcStore == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    if(queueToUse_ != nullptr) {
+        setQueueToUse(queueToUse_);
+    }
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void ActionHelper::step(uint8_t step, MessageQueueId_t reportTo,
+        ActionId_t commandId, ReturnValue_t result) {
+    CommandMessage reply;
+    ActionMessage::setStepReply(&reply, commandId, step + STEP_OFFSET, result);
+    queueToUse->sendMessage(reportTo, &reply);
+}
+
+void ActionHelper::finish(bool success, MessageQueueId_t reportTo, ActionId_t commandId,
+        ReturnValue_t result) {
+    CommandMessage reply;
+    ActionMessage::setCompletionReply(&reply, commandId, success, result);
+    queueToUse->sendMessage(reportTo, &reply);
+}
+
+void ActionHelper::setQueueToUse(MessageQueueIF* queue) {
+    queueToUse = queue;
+}
+
+void ActionHelper::prepareExecution(MessageQueueId_t commandedBy,
+        ActionId_t actionId, store_address_t dataAddress) {
+    const uint8_t* dataPtr = NULL;
+    size_t size = 0;
+    ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        CommandMessage reply;
+        ActionMessage::setStepReply(&reply, actionId, 0, result);
+        queueToUse->sendMessage(commandedBy, &reply);
+        return;
+    }
+    result = owner->executeAction(actionId, commandedBy, dataPtr, size);
+    ipcStore->deleteData(dataAddress);
+    if(result == HasActionsIF::EXECUTION_FINISHED) {
+        CommandMessage reply;
+        ActionMessage::setCompletionReply(&reply, actionId, true, result);
+        queueToUse->sendMessage(commandedBy, &reply);
+    }
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        CommandMessage reply;
+        ActionMessage::setStepReply(&reply, actionId, 0, result);
+        queueToUse->sendMessage(commandedBy, &reply);
+        return;
+    }
+}
+
+ReturnValue_t ActionHelper::reportData(MessageQueueId_t reportTo,
+        ActionId_t replyId, SerializeIF* data, bool hideSender) {
+    CommandMessage reply;
+    store_address_t storeAddress;
+    uint8_t *dataPtr;
+    size_t maxSize = data->getSerializedSize();
+    if (maxSize == 0) {
+        /* No error, there's simply nothing to report. */
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    size_t size = 0;
+    ReturnValue_t result = ipcStore->getFreeElement(&storeAddress, maxSize,
+            &dataPtr);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "ActionHelper::reportData: Getting free element from IPC store failed!" <<
+                std::endl;
+#else
+        sif::printWarning("ActionHelper::reportData: Getting free element from IPC "
+                "store failed!\n");
+#endif
+        return result;
+    }
+    result = data->serialize(&dataPtr, &size, maxSize,
+            SerializeIF::Endianness::BIG);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        ipcStore->deleteData(storeAddress);
+        return result;
+    }
+
+    /* We don't need to report the objectId, as we receive REQUESTED data before the completion
+    success message. True aperiodic replies need to be reported with another dedicated message. */
+    ActionMessage::setDataReply(&reply, replyId, storeAddress);
+
+    /* If the sender needs to be hidden, for example to handle packet
+    as unrequested reply, this will be done here. */
+    if (hideSender) {
+        result = MessageQueueSenderIF::sendMessage(reportTo, &reply);
+    }
+    else {
+        result = queueToUse->sendMessage(reportTo, &reply);
+    }
+
+    if (result != HasReturnvaluesIF::RETURN_OK){
+        ipcStore->deleteData(storeAddress);
+    }
+    return result;
+}
+
+void ActionHelper::resetHelper() {
+}
+
+ReturnValue_t ActionHelper::reportData(MessageQueueId_t reportTo,
+        ActionId_t replyId, const uint8_t *data, size_t dataSize,
+        bool hideSender) {
+    CommandMessage reply;
+    store_address_t storeAddress;
+    ReturnValue_t result = ipcStore->addData(&storeAddress, data, dataSize);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "ActionHelper::reportData: Adding data to IPC store failed!" << std::endl;
+#else
+        sif::printWarning("ActionHelper::reportData: Adding data to IPC store failed!\n");
+#endif
+        return result;
+    }
+
+    /* We don't need to report the objectId, as we receive REQUESTED data before the completion
+    success message. True aperiodic replies need to be reported with another dedicated message. */
+    ActionMessage::setDataReply(&reply, replyId, storeAddress);
+
+    /* If the sender needs to be hidden, for example to handle packet
+    as unrequested reply, this will be done here. */
+    if (hideSender) {
+        result = MessageQueueSenderIF::sendMessage(reportTo, &reply);
+    }
+    else {
+        result = queueToUse->sendMessage(reportTo, &reply);
+    }
+
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        ipcStore->deleteData(storeAddress);
+    }
+    return result;
+}

src/core/action/ActionMessage.cpp

@@ -0,0 +1,82 @@
+#include "ActionMessage.h"
+#include "HasActionsIF.h"
+
+#include "../objectmanager/ObjectManager.h"
+#include "../storagemanager/StorageManagerIF.h"
+
+ActionMessage::ActionMessage() {
+}
+
+ActionMessage::~ActionMessage() {
+}
+
+void ActionMessage::setCommand(CommandMessage* message, ActionId_t fid,
+        store_address_t parameters) {
+    message->setCommand(EXECUTE_ACTION);
+    message->setParameter(fid);
+    message->setParameter2(parameters.raw);
+}
+
+ActionId_t ActionMessage::getActionId(const CommandMessage* message) {
+    return ActionId_t(message->getParameter());
+}
+
+store_address_t ActionMessage::getStoreId(const CommandMessage* message) {
+    store_address_t temp;
+    temp.raw = message->getParameter2();
+    return temp;
+}
+
+void ActionMessage::setStepReply(CommandMessage* message, ActionId_t fid, uint8_t step,
+        ReturnValue_t result) {
+    if (result == HasReturnvaluesIF::RETURN_OK) {
+        message->setCommand(STEP_SUCCESS);
+    } else {
+        message->setCommand(STEP_FAILED);
+    }
+    message->setParameter(fid);
+    message->setParameter2((step << 16) + result);
+}
+
+uint8_t ActionMessage::getStep(const CommandMessage* message) {
+    return uint8_t((message->getParameter2() >> 16) & 0xFF);
+}
+
+ReturnValue_t ActionMessage::getReturnCode(const CommandMessage* message) {
+    return message->getParameter2() & 0xFFFF;
+}
+
+void ActionMessage::setDataReply(CommandMessage* message, ActionId_t actionId,
+        store_address_t data) {
+    message->setCommand(DATA_REPLY);
+    message->setParameter(actionId);
+    message->setParameter2(data.raw);
+}
+
+void ActionMessage::setCompletionReply(CommandMessage* message,
+        ActionId_t fid, bool success, ReturnValue_t result) {
+    if (success) {
+        message->setCommand(COMPLETION_SUCCESS);
+    }
+    else {
+        message->setCommand(COMPLETION_FAILED);
+    }
+    message->setParameter(fid);
+    message->setParameter2(result);
+}
+
+void ActionMessage::clear(CommandMessage* message) {
+    switch(message->getCommand()) {
+    case EXECUTE_ACTION:
+    case DATA_REPLY: {
+        StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
+                objects::IPC_STORE);
+        if (ipcStore != NULL) {
+            ipcStore->deleteData(getStoreId(message));
+        }
+        break;
+    }
+    default:
+        break;
+    }
+}

src/core/action/CMakeLists.txt

@@ -0,0 +1,7 @@
+target_sources(${LIB_FSFW_NAME} 
+    PRIVATE 
+        ActionHelper.cpp
+        ActionMessage.cpp
+        CommandActionHelper.cpp
+        SimpleActionHelper.cpp
+)

src/core/action/CommandActionHelper.cpp

@@ -0,0 +1,128 @@
+#include "ActionMessage.h"
+#include "CommandActionHelper.h"
+#include "CommandsActionsIF.h"
+#include "HasActionsIF.h"
+
+#include "../objectmanager/ObjectManager.h"
+
+CommandActionHelper::CommandActionHelper(CommandsActionsIF *setOwner) :
+        owner(setOwner), queueToUse(NULL), ipcStore(
+        NULL), commandCount(0), lastTarget(0) {
+}
+
+CommandActionHelper::~CommandActionHelper() {
+}
+
+ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo,
+        ActionId_t actionId, SerializeIF *data) {
+    HasActionsIF *receiver = ObjectManager::instance()->get<HasActionsIF>(commandTo);
+    if (receiver == NULL) {
+        return CommandsActionsIF::OBJECT_HAS_NO_FUNCTIONS;
+    }
+    store_address_t storeId;
+    uint8_t *storePointer;
+    size_t maxSize = data->getSerializedSize();
+    ReturnValue_t result = ipcStore->getFreeElement(&storeId, maxSize,
+            &storePointer);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    size_t size = 0;
+    result = data->serialize(&storePointer, &size, maxSize,
+            SerializeIF::Endianness::BIG);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    return sendCommand(receiver->getCommandQueue(), actionId, storeId);
+}
+
+ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo,
+        ActionId_t actionId, const uint8_t *data, uint32_t size) {
+//    if (commandCount != 0) {
+//        return CommandsFunctionsIF::ALREADY_COMMANDING;
+//    }
+    HasActionsIF *receiver = ObjectManager::instance()->get<HasActionsIF>(commandTo);
+    if (receiver == NULL) {
+        return CommandsActionsIF::OBJECT_HAS_NO_FUNCTIONS;
+    }
+    store_address_t storeId;
+    ReturnValue_t result = ipcStore->addData(&storeId, data, size);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    return sendCommand(receiver->getCommandQueue(), actionId, storeId);
+}
+
+ReturnValue_t CommandActionHelper::sendCommand(MessageQueueId_t queueId,
+        ActionId_t actionId, store_address_t storeId) {
+    CommandMessage command;
+    ActionMessage::setCommand(&command, actionId, storeId);
+    ReturnValue_t result = queueToUse->sendMessage(queueId, &command);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        ipcStore->deleteData(storeId);
+    }
+    lastTarget = queueId;
+    commandCount++;
+    return result;
+}
+
+ReturnValue_t CommandActionHelper::initialize() {
+    ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+    if (ipcStore == NULL) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    queueToUse = owner->getCommandQueuePtr();
+    if (queueToUse == NULL) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CommandActionHelper::handleReply(CommandMessage *reply) {
+    if (reply->getSender() != lastTarget) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    switch (reply->getCommand()) {
+    case ActionMessage::COMPLETION_SUCCESS:
+        commandCount--;
+        owner->completionSuccessfulReceived(ActionMessage::getActionId(reply));
+        return HasReturnvaluesIF::RETURN_OK;
+    case ActionMessage::COMPLETION_FAILED:
+        commandCount--;
+        owner->completionFailedReceived(ActionMessage::getActionId(reply),
+                ActionMessage::getReturnCode(reply));
+        return HasReturnvaluesIF::RETURN_OK;
+    case ActionMessage::STEP_SUCCESS:
+        owner->stepSuccessfulReceived(ActionMessage::getActionId(reply),
+                ActionMessage::getStep(reply));
+        return HasReturnvaluesIF::RETURN_OK;
+    case ActionMessage::STEP_FAILED:
+        commandCount--;
+        owner->stepFailedReceived(ActionMessage::getActionId(reply),
+                ActionMessage::getStep(reply),
+                ActionMessage::getReturnCode(reply));
+        return HasReturnvaluesIF::RETURN_OK;
+    case ActionMessage::DATA_REPLY:
+        extractDataForOwner(ActionMessage::getActionId(reply),
+                ActionMessage::getStoreId(reply));
+        return HasReturnvaluesIF::RETURN_OK;
+    default:
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+}
+
+uint8_t CommandActionHelper::getCommandCount() const {
+    return commandCount;
+}
+
+void CommandActionHelper::extractDataForOwner(ActionId_t actionId, store_address_t storeId) {
+    const uint8_t * data = NULL;
+    size_t size = 0;
+    ReturnValue_t result = ipcStore->getData(storeId, &data, &size);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return;
+    }
+    owner->dataReceived(actionId, data, size);
+    ipcStore->deleteData(storeId);
+}

src/core/action/SimpleActionHelper.cpp

@@ -0,0 +1,75 @@
+#include "HasActionsIF.h"
+#include "SimpleActionHelper.h"
+
+SimpleActionHelper::SimpleActionHelper(HasActionsIF* setOwner,
+        MessageQueueIF* useThisQueue) :
+        ActionHelper(setOwner, useThisQueue), isExecuting(false) {
+}
+
+SimpleActionHelper::~SimpleActionHelper() {
+}
+
+void SimpleActionHelper::step(ReturnValue_t result) {
+    // STEP_OFFESET is subtracted to compensate for adding offset in base
+    // method, which is not necessary here.
+    ActionHelper::step(stepCount - STEP_OFFSET, lastCommander, lastAction,
+            result);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        resetHelper();
+    }
+}
+
+void SimpleActionHelper::finish(ReturnValue_t result) {
+    ActionHelper::finish(lastCommander, lastAction, result);
+    resetHelper();
+}
+
+ReturnValue_t SimpleActionHelper::reportData(SerializeIF* data) {
+    return ActionHelper::reportData(lastCommander, lastAction, data);
+}
+
+void SimpleActionHelper::resetHelper() {
+    stepCount = 0;
+    isExecuting = false;
+    lastAction = 0;
+    lastCommander = 0;
+}
+
+void SimpleActionHelper::prepareExecution(MessageQueueId_t commandedBy,
+        ActionId_t actionId, store_address_t dataAddress) {
+    CommandMessage reply;
+    if (isExecuting) {
+        ipcStore->deleteData(dataAddress);
+        ActionMessage::setStepReply(&reply, actionId, 0,
+                HasActionsIF::IS_BUSY);
+        queueToUse->sendMessage(commandedBy, &reply);
+    }
+    const uint8_t* dataPtr = NULL;
+    size_t size = 0;
+    ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        ActionMessage::setStepReply(&reply, actionId, 0, result);
+        queueToUse->sendMessage(commandedBy, &reply);
+        return;
+    }
+    lastCommander = commandedBy;
+    lastAction = actionId;
+    result = owner->executeAction(actionId, commandedBy, dataPtr, size);
+    ipcStore->deleteData(dataAddress);
+    switch (result) {
+    case HasReturnvaluesIF::RETURN_OK:
+        isExecuting = true;
+        stepCount++;
+        break;
+    case HasActionsIF::EXECUTION_FINISHED:
+        ActionMessage::setCompletionReply(&reply, actionId,
+                true, HasReturnvaluesIF::RETURN_OK);
+        queueToUse->sendMessage(commandedBy, &reply);
+        break;
+    default:
+        ActionMessage::setStepReply(&reply, actionId, 0, result);
+        queueToUse->sendMessage(commandedBy, &reply);
+        break;
+    }
+
+}

src/core/container/CMakeLists.txt

@@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} 
+    PRIVATE 
+        SharedRingBuffer.cpp
+        SimpleRingBuffer.cpp
+)

src/core/container/SharedRingBuffer.cpp

@@ -0,0 +1,60 @@
+#include "SharedRingBuffer.h"
+#include "../ipc/MutexFactory.h"
+#include "../ipc/MutexGuard.h"
+
+SharedRingBuffer::SharedRingBuffer(object_id_t objectId, const size_t size,
+        bool overwriteOld, size_t maxExcessBytes):
+        SystemObject(objectId), SimpleRingBuffer(size, overwriteOld,
+        maxExcessBytes) {
+    mutex = MutexFactory::instance()->createMutex();
+}
+
+
+SharedRingBuffer::SharedRingBuffer(object_id_t objectId, uint8_t *buffer,
+        const size_t size, bool overwriteOld, size_t maxExcessBytes):
+        SystemObject(objectId), SimpleRingBuffer(buffer, size, overwriteOld,
+        maxExcessBytes) {
+    mutex = MutexFactory::instance()->createMutex();
+}
+
+SharedRingBuffer::~SharedRingBuffer() {
+    MutexFactory::instance()->deleteMutex(mutex);
+}
+
+void SharedRingBuffer::setToUseReceiveSizeFIFO(size_t fifoDepth) {
+    this->fifoDepth = fifoDepth;
+}
+
+ReturnValue_t SharedRingBuffer::lockRingBufferMutex(
+        MutexIF::TimeoutType timeoutType, dur_millis_t timeout) {
+    return mutex->lockMutex(timeoutType, timeout);
+}
+
+ReturnValue_t SharedRingBuffer::unlockRingBufferMutex() {
+    return mutex->unlockMutex();
+}
+
+
+
+MutexIF* SharedRingBuffer::getMutexHandle() const {
+    return mutex;
+}
+
+ReturnValue_t SharedRingBuffer::initialize() {
+    if(fifoDepth > 0) {
+        receiveSizesFIFO = new DynamicFIFO<size_t>(fifoDepth);
+    }
+    return SystemObject::initialize();
+}
+
+DynamicFIFO<size_t>* SharedRingBuffer::getReceiveSizesFIFO() {
+    if(receiveSizesFIFO == nullptr) {
+        // Configuration error.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "SharedRingBuffer::getReceiveSizesFIFO: Ring buffer"
+                << " was not configured to have sizes FIFO, returning nullptr!"
+                << std::endl;
+#endif
+    }
+    return receiveSizesFIFO;
+}

src/core/container/SimpleRingBuffer.cpp

@@ -0,0 +1,131 @@
+#include "SimpleRingBuffer.h"
+#include <cstring>
+
+SimpleRingBuffer::SimpleRingBuffer(const size_t size, bool overwriteOld,
+        size_t maxExcessBytes) :
+        RingBufferBase<>(0, size, overwriteOld),
+        maxExcessBytes(maxExcessBytes) {
+    if(maxExcessBytes > size) {
+        this->maxExcessBytes = size;
+    }
+    else {
+        this->maxExcessBytes = maxExcessBytes;
+    }
+    buffer = new uint8_t[size + maxExcessBytes];
+}
+
+SimpleRingBuffer::SimpleRingBuffer(uint8_t *buffer, const size_t size,
+        bool overwriteOld, size_t maxExcessBytes):
+        RingBufferBase<>(0, size, overwriteOld), buffer(buffer) {
+    if(maxExcessBytes > size) {
+        this->maxExcessBytes = size;
+    }
+    else {
+        this->maxExcessBytes = maxExcessBytes;
+    }
+}
+
+SimpleRingBuffer::~SimpleRingBuffer() {
+    delete[] buffer;
+}
+
+ReturnValue_t SimpleRingBuffer::getFreeElement(uint8_t **writePointer,
+        size_t amount) {
+    if (availableWriteSpace() >= amount or overwriteOld) {
+        size_t amountTillWrap = writeTillWrap();
+        if (amountTillWrap < amount) {
+            if((amount - amountTillWrap + excessBytes) > maxExcessBytes) {
+                return HasReturnvaluesIF::RETURN_FAILED;
+            }
+            excessBytes = amount - amountTillWrap;
+        }
+        *writePointer = &buffer[write];
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    else {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+}
+
+void SimpleRingBuffer::confirmBytesWritten(size_t amount) {
+    if(getExcessBytes() > 0) {
+        moveExcessBytesToStart();
+    }
+    incrementWrite(amount);
+
+}
+
+ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data,
+        size_t amount) {
+    if (availableWriteSpace() >= amount or overwriteOld) {
+        size_t amountTillWrap = writeTillWrap();
+        if (amountTillWrap >= amount) {
+            // remaining size in buffer is sufficient to fit full amount.
+            memcpy(&buffer[write], data, amount);
+        }
+        else {
+            memcpy(&buffer[write], data, amountTillWrap);
+            memcpy(buffer, data + amountTillWrap, amount - amountTillWrap);
+        }
+        incrementWrite(amount);
+        return HasReturnvaluesIF::RETURN_OK;
+    } else {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+}
+
+ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, size_t amount,
+        bool incrementReadPtr, bool readRemaining, size_t* trueAmount) {
+    size_t availableData = getAvailableReadData(READ_PTR);
+    size_t amountTillWrap = readTillWrap(READ_PTR);
+    if (availableData < amount) {
+        if (readRemaining) {
+            // more data available than amount specified.
+            amount = availableData;
+        } else {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+    }
+    if (trueAmount != nullptr) {
+        *trueAmount = amount;
+    }
+    if (amountTillWrap >= amount) {
+        memcpy(data, &buffer[read[READ_PTR]], amount);
+    } else {
+        memcpy(data, &buffer[read[READ_PTR]], amountTillWrap);
+        memcpy(data + amountTillWrap, buffer, amount - amountTillWrap);
+    }
+
+    if(incrementReadPtr) {
+        deleteData(amount, readRemaining);
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+size_t SimpleRingBuffer::getExcessBytes() const {
+    return excessBytes;
+}
+
+void SimpleRingBuffer::moveExcessBytesToStart() {
+    if(excessBytes > 0) {
+        std::memcpy(buffer, &buffer[size], excessBytes);
+        excessBytes = 0;
+    }
+}
+
+ReturnValue_t SimpleRingBuffer::deleteData(size_t amount,
+        bool deleteRemaining, size_t* trueAmount) {
+    size_t availableData = getAvailableReadData(READ_PTR);
+    if (availableData < amount) {
+        if (deleteRemaining) {
+            amount = availableData;
+        } else {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+    }
+    if (trueAmount != nullptr) {
+        *trueAmount = amount;
+    }
+    incrementRead(amount, READ_PTR);
+    return HasReturnvaluesIF::RETURN_OK;
+}

src/core/controller/CMakeLists.txt

@@ -0,0 +1,4 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    ControllerBase.cpp
+    ExtendedControllerBase.cpp
+)

src/core/controller/ControllerBase.cpp

@@ -0,0 +1,138 @@
+#include "ControllerBase.h"
+
+#include "../subsystem/SubsystemBase.h"
+#include "../ipc/QueueFactory.h"
+#include "../action/HasActionsIF.h"
+#include "../objectmanager/ObjectManager.h"
+
+ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
+        size_t commandQueueDepth) :
+        SystemObject(setObjectId), parentId(parentId), mode(MODE_OFF),
+        submode(SUBMODE_NONE), modeHelper(this),
+        healthHelper(this, setObjectId) {
+    commandQueue = QueueFactory::instance()->createMessageQueue(
+            commandQueueDepth);
+}
+
+ControllerBase::~ControllerBase() {
+    QueueFactory::instance()->deleteMessageQueue(commandQueue);
+}
+
+ReturnValue_t ControllerBase::initialize() {
+    ReturnValue_t result = SystemObject::initialize();
+    if (result != RETURN_OK) {
+        return result;
+    }
+
+    MessageQueueId_t parentQueue = 0;
+    if (parentId != objects::NO_OBJECT) {
+        SubsystemBase *parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
+        if (parent == nullptr) {
+            return RETURN_FAILED;
+        }
+        parentQueue = parent->getCommandQueue();
+
+        parent->registerChild(getObjectId());
+    }
+
+    result = healthHelper.initialize(parentQueue);
+    if (result != RETURN_OK) {
+        return result;
+    }
+
+    result = modeHelper.initialize(parentQueue);
+    if (result != RETURN_OK) {
+        return result;
+    }
+
+    return RETURN_OK;
+}
+
+MessageQueueId_t ControllerBase::getCommandQueue() const {
+    return commandQueue->getId();
+}
+
+void ControllerBase::handleQueue() {
+    CommandMessage command;
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    for (result = commandQueue->receiveMessage(&command);
+            result == RETURN_OK;
+            result = commandQueue->receiveMessage(&command)) {
+
+        result = modeHelper.handleModeCommand(&command);
+        if (result == RETURN_OK) {
+            continue;
+        }
+
+        result = healthHelper.handleHealthCommand(&command);
+        if (result == RETURN_OK) {
+            continue;
+        }
+        result = handleCommandMessage(&command);
+        if (result == RETURN_OK) {
+            continue;
+        }
+        command.setToUnknownCommand();
+        commandQueue->reply(&command);
+    }
+
+}
+
+void ControllerBase::startTransition(Mode_t mode, Submode_t submode) {
+    changeHK(this->mode, this->submode, false);
+    triggerEvent(CHANGING_MODE, mode, submode);
+    this->mode = mode;
+    this->submode = submode;
+    modeHelper.modeChanged(mode, submode);
+    modeChanged(mode, submode);
+    announceMode(false);
+    changeHK(this->mode, this->submode, true);
+}
+
+void ControllerBase::getMode(Mode_t* mode, Submode_t* submode) {
+    *mode = this->mode;
+    *submode = this->submode;
+}
+
+void ControllerBase::setToExternalControl() {
+    healthHelper.setHealth(EXTERNAL_CONTROL);
+}
+
+void ControllerBase::announceMode(bool recursive) {
+    triggerEvent(MODE_INFO, mode, submode);
+}
+
+ReturnValue_t ControllerBase::performOperation(uint8_t opCode) {
+    handleQueue();
+    performControlOperation();
+    return RETURN_OK;
+}
+
+void ControllerBase::modeChanged(Mode_t mode, Submode_t submode) {
+    return;
+}
+
+ReturnValue_t ControllerBase::setHealth(HealthState health) {
+    switch (health) {
+    case HEALTHY:
+    case EXTERNAL_CONTROL:
+        healthHelper.setHealth(health);
+        return RETURN_OK;
+    default:
+        return INVALID_HEALTH_STATE;
+    }
+}
+
+HasHealthIF::HealthState ControllerBase::getHealth() {
+    return healthHelper.getHealth();
+}
+void ControllerBase::setTaskIF(PeriodicTaskIF* task_){
+            executingTask = task_;
+}
+
+void ControllerBase::changeHK(Mode_t mode, Submode_t submode, bool enable) {
+}
+
+ReturnValue_t ControllerBase::initializeAfterTaskCreation() {
+    return HasReturnvaluesIF::RETURN_OK;
+}

src/core/controller/ExtendedControllerBase.cpp

@@ -0,0 +1,104 @@
+#include "ExtendedControllerBase.h"
+
+
+ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId,
+        object_id_t parentId, size_t commandQueueDepth):
+        ControllerBase(objectId, parentId, commandQueueDepth),
+        poolManager(this, commandQueue),
+        actionHelper(this, commandQueue) {
+}
+
+ExtendedControllerBase::~ExtendedControllerBase() {
+}
+
+ReturnValue_t ExtendedControllerBase::executeAction(ActionId_t actionId,
+        MessageQueueId_t commandedBy, const uint8_t *data, size_t size) {
+    /* Needs to be overriden and implemented by child class. */
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+object_id_t ExtendedControllerBase::getObjectId() const {
+    return SystemObject::getObjectId();
+}
+
+uint32_t ExtendedControllerBase::getPeriodicOperationFrequency() const {
+    return this->executingTask->getPeriodMs();
+}
+
+ReturnValue_t ExtendedControllerBase::handleCommandMessage(
+        CommandMessage *message) {
+    ReturnValue_t result = actionHelper.handleActionMessage(message);
+    if(result == HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    return poolManager.handleHousekeepingMessage(message);
+}
+
+void ExtendedControllerBase::handleQueue() {
+    CommandMessage command;
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    for (result = commandQueue->receiveMessage(&command);
+            result == RETURN_OK;
+            result = commandQueue->receiveMessage(&command)) {
+        result = actionHelper.handleActionMessage(&command);
+        if (result == RETURN_OK) {
+            continue;
+        }
+
+        result = modeHelper.handleModeCommand(&command);
+        if (result == RETURN_OK) {
+            continue;
+        }
+
+        result = healthHelper.handleHealthCommand(&command);
+        if (result == RETURN_OK) {
+            continue;
+        }
+
+        result = poolManager.handleHousekeepingMessage(&command);
+        if (result == RETURN_OK) {
+            continue;
+        }
+
+        result = handleCommandMessage(&command);
+        if (result == RETURN_OK) {
+            continue;
+        }
+        command.setToUnknownCommand();
+        commandQueue->reply(&command);
+    }
+}
+
+ReturnValue_t ExtendedControllerBase::initialize() {
+    ReturnValue_t result = ControllerBase::initialize();
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    result = actionHelper.initialize(commandQueue);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    return poolManager.initialize(commandQueue);
+}
+
+ReturnValue_t ExtendedControllerBase::initializeAfterTaskCreation() {
+    return poolManager.initializeAfterTaskCreation();
+}
+
+ReturnValue_t ExtendedControllerBase::performOperation(uint8_t opCode) {
+    handleQueue();
+    performControlOperation();
+    /* We do this after performing control operation because variables will be set changed
+    in this function. */
+    poolManager.performHkOperation();
+    return RETURN_OK;
+}
+
+MessageQueueId_t ExtendedControllerBase::getCommandQueue() const {
+    return commandQueue->getId();
+}
+
+LocalDataPoolManager* ExtendedControllerBase::getHkManagerHandle() {
+    return &poolManager;
+}

src/core/datapool/CMakeLists.txt

@@ -0,0 +1,6 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		HkSwitchHelper.cpp
+		PoolDataSetBase.cpp
+		PoolEntry.cpp
+)

src/core/datapool/HkSwitchHelper.cpp

@@ -0,0 +1,75 @@
+#include "../datapool/HkSwitchHelper.h"
+#include "../ipc/QueueFactory.h"
+
+HkSwitchHelper::HkSwitchHelper(EventReportingProxyIF* eventProxy) :
+		commandActionHelper(this), eventProxy(eventProxy) {
+	actionQueue = QueueFactory::instance()->createMessageQueue();
+}
+
+HkSwitchHelper::~HkSwitchHelper() {
+    QueueFactory::instance()->deleteMessageQueue(actionQueue);
+}
+
+ReturnValue_t HkSwitchHelper::initialize() {
+	ReturnValue_t result = commandActionHelper.initialize();
+
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	return result;
+}
+
+ReturnValue_t HkSwitchHelper::performOperation(uint8_t operationCode) {
+	CommandMessage command;
+	while (actionQueue->receiveMessage(&command) == HasReturnvaluesIF::RETURN_OK) {
+		ReturnValue_t result = commandActionHelper.handleReply(&command);
+		if (result == HasReturnvaluesIF::RETURN_OK) {
+			continue;
+		}
+		command.setToUnknownCommand();
+		actionQueue->reply(&command);
+	}
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void HkSwitchHelper::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) {
+}
+
+void HkSwitchHelper::stepFailedReceived(ActionId_t actionId, uint8_t step,
+		ReturnValue_t returnCode) {
+	eventProxy->forwardEvent(SWITCHING_TM_FAILED, returnCode, actionId);
+}
+
+void HkSwitchHelper::dataReceived(ActionId_t actionId, const uint8_t* data,
+		uint32_t size) {
+}
+
+void HkSwitchHelper::completionSuccessfulReceived(ActionId_t actionId) {
+}
+
+void HkSwitchHelper::completionFailedReceived(ActionId_t actionId,
+		ReturnValue_t returnCode) {
+	eventProxy->forwardEvent(SWITCHING_TM_FAILED, returnCode, actionId);
+}
+
+ReturnValue_t HkSwitchHelper::switchHK(SerializeIF* sids, bool enable) {
+//	ActionId_t action = HKService::DISABLE_HK;
+//	if (enable) {
+//		action = HKService::ENABLE_HK;
+//	}
+//
+//	ReturnValue_t result = commandActionHelper.commandAction(
+//			objects::PUS_HK_SERVICE, action, sids);
+//
+//	if (result != HasReturnvaluesIF::RETURN_OK) {
+//		eventProxy->forwardEvent(SWITCHING_TM_FAILED, result);
+//	}
+//	return result;
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+MessageQueueIF* HkSwitchHelper::getCommandQueuePtr() {
+	return actionQueue;
+}

src/core/datapool/PoolDataSetBase.cpp

@@ -0,0 +1,233 @@
+#include "PoolDataSetBase.h"
+#include "ReadCommitIFAttorney.h"
+
+#include "../serviceinterface/ServiceInterface.h"
+
+#include <cstring>
+
+PoolDataSetBase::PoolDataSetBase(PoolVariableIF** registeredVariablesArray,
+        const size_t maxFillCount):
+        registeredVariables(registeredVariablesArray),
+        maxFillCount(maxFillCount) {}
+
+PoolDataSetBase::~PoolDataSetBase() {}
+
+
+ReturnValue_t PoolDataSetBase::registerVariable(PoolVariableIF *variable) {
+    if(registeredVariables == nullptr) {
+        /* Underlying container invalid */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    if (state != States::STATE_SET_UNINITIALISED) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "DataSet::registerVariable: Call made in wrong position." << std::endl;
+#else
+        sif::printError("DataSet::registerVariable: Call made in wrong position.");
+#endif
+        return DataSetIF::DATA_SET_UNINITIALISED;
+    }
+    if (variable == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "DataSet::registerVariable: Pool variable is nullptr." << std::endl;
+#else
+        sif::printError("DataSet::registerVariable: Pool variable is nullptr.\n");
+#endif
+        return DataSetIF::POOL_VAR_NULL;
+    }
+    if (fillCount >= maxFillCount) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "DataSet::registerVariable: DataSet is full." << std::endl;
+#else
+        sif::printError("DataSet::registerVariable: DataSet is full.\n");
+#endif
+        return DataSetIF::DATA_SET_FULL;
+    }
+    registeredVariables[fillCount] = variable;
+    fillCount++;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t PoolDataSetBase::read(MutexIF::TimeoutType timeoutType,
+        uint32_t lockTimeout) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    ReturnValue_t error = result;
+    if (state == States::STATE_SET_UNINITIALISED) {
+        lockDataPool(timeoutType, lockTimeout);
+        for (uint16_t count = 0; count < fillCount; count++) {
+            result = readVariable(count);
+            if(result != RETURN_OK) {
+                error = result;
+            }
+        }
+        state = States::STATE_SET_WAS_READ;
+        unlockDataPool();
+    }
+    else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "PoolDataSetBase::read: Call made in wrong position. Don't forget to "
+                "commit member datasets!" << std::endl;
+#else
+        sif::printWarning("PoolDataSetBase::read: Call made in wrong position. Don't forget to "
+                "commit member datasets!\n");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+        result = SET_WAS_ALREADY_READ;
+    }
+
+    if(error != HasReturnvaluesIF::RETURN_OK) {
+        result = error;
+    }
+    return result;
+}
+
+uint16_t PoolDataSetBase::getFillCount() const {
+    return fillCount;
+}
+
+ReturnValue_t PoolDataSetBase::readVariable(uint16_t count) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    if(registeredVariables[count] == nullptr) {
+        /* Configuration error. */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    /* These checks are often performed by the respective variable implementation too, but I guess
+    a double check does not hurt. */
+    if (registeredVariables[count]->getReadWriteMode() != PoolVariableIF::VAR_WRITE and
+            registeredVariables[count]->getDataPoolId() != PoolVariableIF::NO_PARAMETER) {
+        if(protectEveryReadCommitCall) {
+            result = registeredVariables[count]->read(timeoutTypeForSingleVars,
+                    mutexTimeoutForSingleVars);
+        }
+        else {
+            /* The readWithoutLock function is protected, so we use the attorney here */
+            result = ReadCommitIFAttorney::readWithoutLock(registeredVariables[count]);
+        }
+
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            result = INVALID_PARAMETER_DEFINITION;
+        }
+    }
+    return result;
+}
+
+ReturnValue_t PoolDataSetBase::commit(MutexIF::TimeoutType timeoutType,
+        uint32_t lockTimeout) {
+    if (state == States::STATE_SET_WAS_READ) {
+        handleAlreadyReadDatasetCommit(timeoutType, lockTimeout);
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    else {
+        return handleUnreadDatasetCommit(timeoutType, lockTimeout);
+    }
+}
+
+void PoolDataSetBase::handleAlreadyReadDatasetCommit(
+        MutexIF::TimeoutType timeoutType, uint32_t lockTimeout) {
+    lockDataPool(timeoutType, lockTimeout);
+    for (uint16_t count = 0; count < fillCount; count++) {
+        if ((registeredVariables[count]->getReadWriteMode() != PoolVariableIF::VAR_READ) and
+                (registeredVariables[count]->getDataPoolId() != PoolVariableIF::NO_PARAMETER)) {
+            if(protectEveryReadCommitCall) {
+                registeredVariables[count]->commit(timeoutTypeForSingleVars,
+                        mutexTimeoutForSingleVars);
+            }
+            else {
+                /* The commitWithoutLock function is protected, so we use the attorney here */
+                ReadCommitIFAttorney::commitWithoutLock(registeredVariables[count]);
+            }
+        }
+    }
+    state = States::STATE_SET_UNINITIALISED;
+    unlockDataPool();
+}
+
+ReturnValue_t PoolDataSetBase::handleUnreadDatasetCommit(
+        MutexIF::TimeoutType timeoutType, uint32_t lockTimeout) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    lockDataPool(timeoutType, lockTimeout);
+    for (uint16_t count = 0; count < fillCount; count++) {
+        if ((registeredVariables[count]->getReadWriteMode() == PoolVariableIF::VAR_WRITE) and
+                (registeredVariables[count]->getDataPoolId() != PoolVariableIF::NO_PARAMETER)) {
+            if(protectEveryReadCommitCall) {
+                result = registeredVariables[count]->commit(timeoutTypeForSingleVars,
+                        mutexTimeoutForSingleVars);
+            }
+            else {
+                /* The commitWithoutLock function is protected, so we use the attorney here */
+                ReadCommitIFAttorney::commitWithoutLock(registeredVariables[count]);
+            }
+
+        } else if (registeredVariables[count]->getDataPoolId()
+                != PoolVariableIF::NO_PARAMETER) {
+            if (result != COMMITING_WITHOUT_READING) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+                sif::error << "DataSet::commit(): commit-without-read call made "
+                        "with non write-only variable." << std::endl;
+#endif
+                result = COMMITING_WITHOUT_READING;
+            }
+        }
+    }
+    state = States::STATE_SET_UNINITIALISED;
+    unlockDataPool();
+    return result;
+}
+
+
+ReturnValue_t PoolDataSetBase::lockDataPool(MutexIF::TimeoutType timeoutType,
+        uint32_t lockTimeout) {
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t PoolDataSetBase::unlockDataPool() {
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t PoolDataSetBase::serialize(uint8_t** buffer, size_t* size,
+        const size_t maxSize, SerializeIF::Endianness streamEndianness) const {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+    for (uint16_t count = 0; count < fillCount; count++) {
+        result = registeredVariables[count]->serialize(buffer, size, maxSize, streamEndianness);
+        if (result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+    }
+    return result;
+}
+
+ReturnValue_t PoolDataSetBase::deSerialize(const uint8_t** buffer, size_t* size,
+        SerializeIF::Endianness streamEndianness) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+    for (uint16_t count = 0; count < fillCount; count++) {
+        result = registeredVariables[count]->deSerialize(buffer, size,
+                streamEndianness);
+        if (result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+    }
+    return result;
+}
+
+size_t PoolDataSetBase::getSerializedSize() const {
+    uint32_t size = 0;
+    for (uint16_t count = 0; count < fillCount; count++) {
+        size += registeredVariables[count]->getSerializedSize();
+    }
+    return size;
+}
+
+void PoolDataSetBase::setContainer(PoolVariableIF **variablesContainer) {
+    this->registeredVariables = variablesContainer;
+}
+
+PoolVariableIF** PoolDataSetBase::getContainer() const {
+    return registeredVariables;
+}
+
+void PoolDataSetBase::setReadCommitProtectionBehaviour(
+        bool protectEveryReadCommit, MutexIF::TimeoutType timeoutType,
+        uint32_t mutexTimeout) {
+    this->protectEveryReadCommitCall = protectEveryReadCommit;
+    this->timeoutTypeForSingleVars = timeoutType;
+    this->mutexTimeoutForSingleVars = mutexTimeout;
+}

src/core/datapool/PoolEntry.cpp

@@ -0,0 +1,101 @@
+#include "PoolEntry.h"
+
+#include "../serviceinterface/ServiceInterface.h"
+#include "../globalfunctions/arrayprinter.h"
+#include <cstring>
+#include <algorithm>
+
+template <typename T>
+PoolEntry<T>::PoolEntry(std::initializer_list<T> initValue, bool setValid ):
+		length(static_cast<uint8_t>(initValue.size())), valid(setValid) {
+	this->address = new T[this->length];
+	if(initValue.size() == 0) {
+		std::memset(this->address, 0, this->getByteSize());
+	}
+	else {
+		std::copy(initValue.begin(), initValue.end(), this->address);
+	}
+}
+
+template <typename T>
+PoolEntry<T>::PoolEntry(T* initValue, uint8_t setLength, bool setValid):
+		length(setLength), valid(setValid) {
+	this->address = new T[this->length];
+	if (initValue != nullptr) {
+		std::memcpy(this->address, initValue, this->getByteSize() );
+	} else {
+		std::memset(this->address, 0, this->getByteSize() );
+	}
+}
+
+//As the data pool is global, this dtor is only be called on program exit.
+//Warning! Never copy pool entries!
+template <typename T>
+PoolEntry<T>::~PoolEntry() {
+	delete[] this->address;
+}
+
+template <typename T>
+uint16_t PoolEntry<T>::getByteSize() {
+	return ( sizeof(T) * this->length );
+}
+
+template <typename T>
+uint8_t PoolEntry<T>::getSize() {
+	return this->length;
+}
+
+template <typename T>
+void* PoolEntry<T>::getRawData() {
+	return this->address;
+}
+
+template <typename T>
+void PoolEntry<T>::setValid(bool isValid) {
+	this->valid = isValid;
+}
+
+template <typename T>
+bool PoolEntry<T>::getValid() {
+	return valid;
+}
+
+template <typename T>
+void PoolEntry<T>::print() {
+	const char* validString = nullptr;
+	if(valid) {
+		validString = "Valid";
+	}
+	else {
+		validString = "Invalid";
+	}
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	 sif::info << "PoolEntry information." << std::endl;
+	 sif::info << "PoolEntry validity: " << validString << std::endl;
+#else
+	 sif::printInfo("PoolEntry information.\n");
+	 sif::printInfo("PoolEntry validity: %s\n", validString);
+#endif
+	arrayprinter::print(reinterpret_cast<uint8_t*>(address), getByteSize());
+}
+
+template<typename T>
+inline T* PoolEntry<T>::getDataPtr() {
+	return this->address;
+}
+
+template<typename T>
+Type PoolEntry<T>::getType() {
+	return PodTypeConversion<T>::type;
+}
+
+template class PoolEntry<uint8_t>;
+template class PoolEntry<uint16_t>;
+template class PoolEntry<uint32_t>;
+template class PoolEntry<uint64_t>;
+template class PoolEntry<int8_t>;
+template class PoolEntry<int16_t>;
+template class PoolEntry<int32_t>;
+template class PoolEntry<int64_t>;
+template class PoolEntry<float>;
+template class PoolEntry<double>;

src/core/datapoollocal/CMakeLists.txt

@@ -0,0 +1,10 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		LocalDataPoolManager.cpp
+		LocalDataSet.cpp
+		LocalPoolDataSetBase.cpp
+		LocalPoolObjectBase.cpp
+		SharedLocalDataSet.cpp
+)
+
+add_subdirectory(internal)

src/core/datapoollocal/LocalDataPoolManager.cpp

@@ -0,0 +1,948 @@
+#include "HasLocalDataPoolIF.h"
+#include "LocalDataPoolManager.h"
+#include "LocalPoolObjectBase.h"
+#include "LocalPoolDataSetBase.h"
+#include "internal/LocalPoolDataSetAttorney.h"
+#include "internal/HasLocalDpIFManagerAttorney.h"
+
+#include "../housekeeping/HousekeepingSetPacket.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../housekeeping/HousekeepingSnapshot.h"
+#include "../housekeeping/AcceptsHkPacketsIF.h"
+#include "../timemanager/CCSDSTime.h"
+#include "../ipc/MutexFactory.h"
+#include "../ipc/MutexGuard.h"
+#include "../ipc/QueueFactory.h"
+
+#include <array>
+#include <cmath>
+
+object_id_t LocalDataPoolManager::defaultHkDestination = objects::PUS_SERVICE_3_HOUSEKEEPING;
+
+LocalDataPoolManager::LocalDataPoolManager(HasLocalDataPoolIF* owner, MessageQueueIF* queueToUse,
+        bool appendValidityBuffer):
+		        appendValidityBuffer(appendValidityBuffer) {
+    if(owner == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "LocalDataPoolManager", HasReturnvaluesIF::RETURN_FAILED,
+                "Invalid supplied owner");
+        return;
+    }
+    this->owner = owner;
+    mutex = MutexFactory::instance()->createMutex();
+    if(mutex == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                "LocalDataPoolManager", HasReturnvaluesIF::RETURN_FAILED,
+                "Could not create mutex");
+    }
+
+    hkQueue = queueToUse;
+}
+
+LocalDataPoolManager::~LocalDataPoolManager() {
+    if(mutex != nullptr) {
+        MutexFactory::instance()->deleteMutex(mutex);
+    }
+}
+
+ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) {
+    if(queueToUse == nullptr) {
+        /* Error, all destinations invalid */
+        printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
+                QUEUE_OR_DESTINATION_INVALID);
+    }
+    hkQueue = queueToUse;
+
+    ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+    if(ipcStore == nullptr) {
+        /* Error, all destinations invalid */
+        printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                "initialize", HasReturnvaluesIF::RETURN_FAILED,
+                "Could not set IPC store.");
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+
+    if(defaultHkDestination != objects::NO_OBJECT) {
+        AcceptsHkPacketsIF* hkPacketReceiver = ObjectManager::instance()->
+                get<AcceptsHkPacketsIF>(defaultHkDestination);
+        if(hkPacketReceiver != nullptr) {
+            hkDestinationId = hkPacketReceiver->getHkQueue();
+        }
+        else {
+            printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                    "initialize", QUEUE_OR_DESTINATION_INVALID);
+            return QUEUE_OR_DESTINATION_INVALID;
+        }
+    }
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::initializeAfterTaskCreation(
+        uint8_t nonDiagInvlFactor) {
+    setNonDiagnosticIntervalFactor(nonDiagInvlFactor);
+    return initializeHousekeepingPoolEntriesOnce();
+}
+
+ReturnValue_t LocalDataPoolManager::initializeHousekeepingPoolEntriesOnce() {
+    if(not mapInitialized) {
+        ReturnValue_t result = owner->initializeLocalDataPool(localPoolMap,
+                *this);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            mapInitialized = true;
+        }
+        return result;
+    }
+
+    printWarningOrError(sif::OutputTypes::OUT_WARNING,
+            "initialize", HasReturnvaluesIF::RETURN_FAILED,
+            "The map should only be initialized once");
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::performHkOperation() {
+    ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
+    for(auto& receiver: hkReceivers) {
+        switch(receiver.reportingType) {
+        case(ReportingType::PERIODIC): {
+            if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
+                /* Periodic packets shall only be generated from datasets */
+                continue;
+            }
+            performPeriodicHkGeneration(receiver);
+            break;
+        }
+        case(ReportingType::UPDATE_HK): {
+            handleHkUpdate(receiver, status);
+            break;
+        }
+        case(ReportingType::UPDATE_NOTIFICATION): {
+            handleNotificationUpdate(receiver, status);
+            break;
+        }
+        case(ReportingType::UPDATE_SNAPSHOT): {
+            handleNotificationSnapshot(receiver, status);
+            break;
+        }
+        default:
+            // This should never happen.
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+    }
+    resetHkUpdateResetHelper();
+    return status;
+}
+
+ReturnValue_t LocalDataPoolManager::handleHkUpdate(HkReceiver& receiver,
+        ReturnValue_t& status) {
+    if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
+        /* Update packets shall only be generated from datasets. */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
+            receiver.dataId.sid);
+    if(dataSet == nullptr) {
+        return DATASET_NOT_FOUND;
+    }
+    if(dataSet->hasChanged()) {
+        /* Prepare and send update notification */
+        ReturnValue_t result = generateHousekeepingPacket(
+                receiver.dataId.sid, dataSet, true);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            status = result;
+        }
+    }
+    handleChangeResetLogic(receiver.dataType, receiver.dataId,
+            dataSet);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::handleNotificationUpdate(HkReceiver& receiver,
+        ReturnValue_t& status) {
+    MarkChangedIF* toReset = nullptr;
+    if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
+        LocalPoolObjectBase* poolObj = HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
+                receiver.dataId.localPoolId);
+        if(poolObj == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "handleNotificationUpdate", POOLOBJECT_NOT_FOUND);
+            return POOLOBJECT_NOT_FOUND;
+        }
+        if(poolObj->hasChanged()) {
+            /* Prepare and send update notification. */
+            CommandMessage notification;
+            HousekeepingMessage::setUpdateNotificationVariableCommand(&notification,
+                    gp_id_t(owner->getObjectId(), receiver.dataId.localPoolId));
+            ReturnValue_t result = hkQueue->sendMessage(receiver.destinationQueue, &notification);
+            if(result != HasReturnvaluesIF::RETURN_OK) {
+                status = result;
+            }
+            toReset = poolObj;
+        }
+
+    }
+    else {
+        LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
+                receiver.dataId.sid);
+        if(dataSet == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "handleNotificationUpdate", DATASET_NOT_FOUND);
+            return DATASET_NOT_FOUND;
+        }
+        if(dataSet->hasChanged()) {
+            /* Prepare and send update notification */
+            CommandMessage notification;
+            HousekeepingMessage::setUpdateNotificationSetCommand(&notification,
+                    receiver.dataId.sid);
+            ReturnValue_t result = hkQueue->sendMessage(
+                    receiver.destinationQueue, &notification);
+            if(result != HasReturnvaluesIF::RETURN_OK) {
+                status = result;
+            }
+            toReset = dataSet;
+        }
+    }
+    if(toReset != nullptr) {
+        handleChangeResetLogic(receiver.dataType, receiver.dataId, toReset);
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(
+        HkReceiver& receiver, ReturnValue_t& status) {
+    MarkChangedIF* toReset = nullptr;
+    /* Check whether data has changed and send messages in case it has */
+    if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
+        LocalPoolObjectBase* poolObj = HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
+                receiver.dataId.localPoolId);
+        if(poolObj == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "handleNotificationSnapshot", POOLOBJECT_NOT_FOUND);
+            return POOLOBJECT_NOT_FOUND;
+        }
+
+        if (not poolObj->hasChanged()) {
+            return HasReturnvaluesIF::RETURN_OK;
+        }
+
+        /* Prepare and send update snapshot */
+        timeval now;
+        Clock::getClock_timeval(&now);
+        CCSDSTime::CDS_short cds;
+        CCSDSTime::convertToCcsds(&cds, &now);
+        HousekeepingSnapshot updatePacket(reinterpret_cast<uint8_t*>(&cds), sizeof(cds),
+                HasLocalDpIFManagerAttorney::getPoolObjectHandle(
+                        owner,receiver.dataId.localPoolId));
+
+        store_address_t storeId;
+        ReturnValue_t result = addUpdateToStore(updatePacket, storeId);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+
+        CommandMessage notification;
+        HousekeepingMessage::setUpdateSnapshotVariableCommand(&notification,
+                gp_id_t(owner->getObjectId(), receiver.dataId.localPoolId), storeId);
+        result = hkQueue->sendMessage(receiver.destinationQueue,
+                &notification);
+        if (result != HasReturnvaluesIF::RETURN_OK) {
+            status = result;
+        }
+        toReset = poolObj;
+    }
+    else {
+        LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
+                receiver.dataId.sid);
+        if(dataSet == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "handleNotificationSnapshot", DATASET_NOT_FOUND);
+            return DATASET_NOT_FOUND;
+        }
+
+        if(not dataSet->hasChanged()) {
+            return HasReturnvaluesIF::RETURN_OK;
+        }
+
+        /* Prepare and send update snapshot */
+        timeval now;
+        Clock::getClock_timeval(&now);
+        CCSDSTime::CDS_short cds;
+        CCSDSTime::convertToCcsds(&cds, &now);
+        HousekeepingSnapshot updatePacket(reinterpret_cast<uint8_t*>(&cds),
+                sizeof(cds), HasLocalDpIFManagerAttorney::getDataSetHandle(owner,
+                        receiver.dataId.sid));
+
+        store_address_t storeId;
+        ReturnValue_t result = addUpdateToStore(updatePacket, storeId);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+
+        CommandMessage notification;
+        HousekeepingMessage::setUpdateSnapshotSetCommand(
+                &notification, receiver.dataId.sid, storeId);
+        result = hkQueue->sendMessage(receiver.destinationQueue, &notification);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            status = result;
+        }
+        toReset = dataSet;
+
+    }
+    if(toReset != nullptr) {
+        handleChangeResetLogic(receiver.dataType,
+                receiver.dataId, toReset);
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::addUpdateToStore(
+        HousekeepingSnapshot& updatePacket, store_address_t& storeId) {
+    size_t updatePacketSize = updatePacket.getSerializedSize();
+    uint8_t *storePtr = nullptr;
+    ReturnValue_t result = ipcStore->getFreeElement(&storeId,
+            updatePacket.getSerializedSize(), &storePtr);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    size_t serializedSize = 0;
+    result = updatePacket.serialize(&storePtr, &serializedSize,
+            updatePacketSize, SerializeIF::Endianness::MACHINE);
+    return result;;
+}
+
+void LocalDataPoolManager::handleChangeResetLogic(
+        DataType type, DataId dataId, MarkChangedIF* toReset) {
+    if(hkUpdateResetList == nullptr) {
+        /* Config error */
+        return;
+    }
+    HkUpdateResetList& listRef = *hkUpdateResetList;
+    for(auto& changeInfo: listRef) {
+        if(changeInfo.dataType != type) {
+            continue;
+        }
+        if((changeInfo.dataType == DataType::DATA_SET) and
+                (changeInfo.dataId.sid != dataId.sid)) {
+            continue;
+        }
+        if((changeInfo.dataType == DataType::LOCAL_POOL_VARIABLE) and
+                (changeInfo.dataId.localPoolId != dataId.localPoolId)) {
+            continue;
+        }
+
+        /* Only one update recipient, we can reset changes status immediately */
+        if(changeInfo.updateCounter <= 1) {
+            toReset->setChanged(false);
+        }
+        /* All recipients have been notified, reset the changed flag */
+        else if(changeInfo.currentUpdateCounter <= 1) {
+            toReset->setChanged(false);
+            changeInfo.currentUpdateCounter = 0;
+        }
+        /* Not all recipiens have been notified yet, decrement */
+        else {
+            changeInfo.currentUpdateCounter--;
+        }
+        return;
+    }
+}
+
+void LocalDataPoolManager::resetHkUpdateResetHelper() {
+    if(hkUpdateResetList == nullptr) {
+        return;
+    }
+
+    for(auto& changeInfo: *hkUpdateResetList) {
+        changeInfo.currentUpdateCounter = changeInfo.updateCounter;
+    }
+}
+
+ReturnValue_t LocalDataPoolManager::subscribeForPeriodicPacket(sid_t sid,
+        bool enableReporting, float collectionInterval, bool isDiagnostics,
+        object_id_t packetDestination) {
+    AcceptsHkPacketsIF* hkReceiverObject = ObjectManager::instance()->
+            get<AcceptsHkPacketsIF>(packetDestination);
+    if(hkReceiverObject == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
+    }
+
+    struct HkReceiver hkReceiver;
+    hkReceiver.dataId.sid = sid;
+    hkReceiver.reportingType = ReportingType::PERIODIC;
+    hkReceiver.dataType = DataType::DATA_SET;
+    hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
+
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet != nullptr) {
+        LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enableReporting);
+        LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
+        LocalPoolDataSetAttorney::initializePeriodicHelper(*dataSet, collectionInterval,
+                owner->getPeriodicOperationFrequency());
+    }
+
+    hkReceivers.push_back(hkReceiver);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t LocalDataPoolManager::subscribeForUpdatePacket(sid_t sid,
+        bool isDiagnostics, bool reportingEnabled,
+        object_id_t packetDestination) {
+    AcceptsHkPacketsIF* hkReceiverObject =
+            ObjectManager::instance()->get<AcceptsHkPacketsIF>(packetDestination);
+    if(hkReceiverObject == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
+    }
+
+    struct HkReceiver hkReceiver;
+    hkReceiver.dataId.sid = sid;
+    hkReceiver.reportingType = ReportingType::UPDATE_HK;
+    hkReceiver.dataType = DataType::DATA_SET;
+    hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
+
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet != nullptr) {
+        LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, true);
+        LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
+    }
+
+    hkReceivers.push_back(hkReceiver);
+
+    handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::subscribeForSetUpdateMessage(
+        const uint32_t setId, object_id_t destinationObject,
+        MessageQueueId_t targetQueueId, bool generateSnapshot) {
+    struct HkReceiver hkReceiver;
+    hkReceiver.dataType = DataType::DATA_SET;
+    hkReceiver.dataId.sid = sid_t(owner->getObjectId(), setId);
+    hkReceiver.destinationQueue = targetQueueId;
+    hkReceiver.objectId = destinationObject;
+    if(generateSnapshot) {
+        hkReceiver.reportingType = ReportingType::UPDATE_SNAPSHOT;
+    }
+    else {
+        hkReceiver.reportingType = ReportingType::UPDATE_NOTIFICATION;
+    }
+
+    hkReceivers.push_back(hkReceiver);
+
+    handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::subscribeForVariableUpdateMessage(
+        const lp_id_t localPoolId, object_id_t destinationObject,
+        MessageQueueId_t targetQueueId, bool generateSnapshot) {
+    struct HkReceiver hkReceiver;
+    hkReceiver.dataType = DataType::LOCAL_POOL_VARIABLE;
+    hkReceiver.dataId.localPoolId = localPoolId;
+    hkReceiver.destinationQueue = targetQueueId;
+    hkReceiver.objectId = destinationObject;
+    if(generateSnapshot) {
+        hkReceiver.reportingType = ReportingType::UPDATE_SNAPSHOT;
+    }
+    else {
+        hkReceiver.reportingType = ReportingType::UPDATE_NOTIFICATION;
+    }
+
+    hkReceivers.push_back(hkReceiver);
+
+    handleHkUpdateResetListInsertion(hkReceiver.dataType, hkReceiver.dataId);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void LocalDataPoolManager::handleHkUpdateResetListInsertion(DataType dataType,
+        DataId dataId) {
+    if(hkUpdateResetList == nullptr) {
+        hkUpdateResetList = new std::vector<struct HkUpdateResetHelper>();
+    }
+
+    for(auto& updateResetStruct: *hkUpdateResetList) {
+        if(dataType == DataType::DATA_SET) {
+            if(updateResetStruct.dataId.sid == dataId.sid) {
+                updateResetStruct.updateCounter++;
+                updateResetStruct.currentUpdateCounter++;
+                return;
+            }
+        }
+        else {
+            if(updateResetStruct.dataId.localPoolId == dataId.localPoolId) {
+                updateResetStruct.updateCounter++;
+                updateResetStruct.currentUpdateCounter++;
+                return;
+            }
+        }
+
+    }
+    HkUpdateResetHelper hkUpdateResetHelper;
+    hkUpdateResetHelper.currentUpdateCounter = 1;
+    hkUpdateResetHelper.updateCounter = 1;
+    hkUpdateResetHelper.dataType = dataType;
+    if(dataType == DataType::DATA_SET) {
+        hkUpdateResetHelper.dataId.sid = dataId.sid;
+    }
+    else {
+        hkUpdateResetHelper.dataId.localPoolId = dataId.localPoolId;
+    }
+    hkUpdateResetList->push_back(hkUpdateResetHelper);
+}
+
+ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(
+        CommandMessage* message) {
+    Command_t command = message->getCommand();
+    sid_t sid = HousekeepingMessage::getSid(message);
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    switch(command) {
+    // Houskeeping interface handling.
+    case(HousekeepingMessage::ENABLE_PERIODIC_DIAGNOSTICS_GENERATION): {
+        result = togglePeriodicGeneration(sid, true, true);
+        break;
+    }
+
+    case(HousekeepingMessage::DISABLE_PERIODIC_DIAGNOSTICS_GENERATION): {
+        result = togglePeriodicGeneration(sid, false, true);
+        break;
+    }
+
+    case(HousekeepingMessage::ENABLE_PERIODIC_HK_REPORT_GENERATION): {
+        result = togglePeriodicGeneration(sid, true, false);
+        break;
+    }
+
+    case(HousekeepingMessage::DISABLE_PERIODIC_HK_REPORT_GENERATION): {
+        result = togglePeriodicGeneration(sid, false, false);
+        break;
+    }
+
+    case(HousekeepingMessage::REPORT_DIAGNOSTICS_REPORT_STRUCTURES): {
+        result = generateSetStructurePacket(sid, true);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+        break;
+    }
+
+    case(HousekeepingMessage::REPORT_HK_REPORT_STRUCTURES): {
+        result = generateSetStructurePacket(sid, false);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+        break;
+    }
+    case(HousekeepingMessage::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL):
+    case(HousekeepingMessage::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL): {
+        float newCollIntvl = 0;
+        HousekeepingMessage::getCollectionIntervalModificationCommand(message,
+                &newCollIntvl);
+        if(command == HousekeepingMessage::
+                MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL) {
+            result = changeCollectionInterval(sid, newCollIntvl, true);
+        }
+        else {
+            result = changeCollectionInterval(sid, newCollIntvl, false);
+        }
+        break;
+    }
+
+    case(HousekeepingMessage::GENERATE_ONE_PARAMETER_REPORT):
+    case(HousekeepingMessage::GENERATE_ONE_DIAGNOSTICS_REPORT): {
+        LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+        if(dataSet == nullptr) {
+            printWarningOrError(sif::OutputTypes::OUT_WARNING, "handleHousekeepingMessage",
+                    DATASET_NOT_FOUND);
+            return DATASET_NOT_FOUND;
+        }
+        if(command == HousekeepingMessage::GENERATE_ONE_PARAMETER_REPORT
+                and LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
+            result = WRONG_HK_PACKET_TYPE;
+            break;
+        }
+        else if(command == HousekeepingMessage::GENERATE_ONE_DIAGNOSTICS_REPORT
+                and not LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
+            result = WRONG_HK_PACKET_TYPE;
+            break;
+        }
+        return generateHousekeepingPacket(HousekeepingMessage::getSid(message),
+                dataSet, true);
+    }
+
+    /* Notification handling */
+    case(HousekeepingMessage::UPDATE_NOTIFICATION_SET): {
+        owner->handleChangedDataset(sid);
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(HousekeepingMessage::UPDATE_NOTIFICATION_VARIABLE): {
+        gp_id_t globPoolId = HousekeepingMessage::getUpdateNotificationVariableCommand(message);
+        owner->handleChangedPoolVariable(globPoolId);
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(HousekeepingMessage::UPDATE_SNAPSHOT_SET): {
+        store_address_t storeId;
+        HousekeepingMessage::getUpdateSnapshotSetCommand(message, &storeId);
+        bool clearMessage = true;
+        owner->handleChangedDataset(sid, storeId, &clearMessage);
+        if(clearMessage) {
+            message->clear();
+        }
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(HousekeepingMessage::UPDATE_SNAPSHOT_VARIABLE): {
+        store_address_t storeId;
+        gp_id_t globPoolId = HousekeepingMessage::getUpdateSnapshotVariableCommand(message,
+                &storeId);
+        bool clearMessage = true;
+        owner->handleChangedPoolVariable(globPoolId, storeId, &clearMessage);
+        if(clearMessage) {
+            message->clear();
+        }
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+
+    default:
+        return CommandMessageIF::UNKNOWN_COMMAND;
+    }
+
+    CommandMessage reply;
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        HousekeepingMessage::setHkRequestFailureReply(&reply, sid, result);
+    }
+    else {
+        HousekeepingMessage::setHkRequestSuccessReply(&reply, sid);
+    }
+    hkQueue->sendMessage(hkDestinationId, &reply);
+    return result;
+}
+
+ReturnValue_t LocalDataPoolManager::printPoolEntry(
+        lp_id_t localPoolId) {
+    auto poolIter = localPoolMap.find(localPoolId);
+    if (poolIter == localPoolMap.end()) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING, "printPoolEntry",
+                localpool::POOL_ENTRY_NOT_FOUND);
+        return localpool::POOL_ENTRY_NOT_FOUND;
+    }
+    poolIter->second->print();
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+MutexIF* LocalDataPoolManager::getMutexHandle() {
+    return mutex;
+}
+
+HasLocalDataPoolIF* LocalDataPoolManager::getOwner() {
+    return owner;
+}
+
+ReturnValue_t LocalDataPoolManager::generateHousekeepingPacket(sid_t sid,
+        LocalPoolDataSetBase* dataSet, bool forDownlink,
+        MessageQueueId_t destination) {
+    if(dataSet == nullptr) {
+        /* Configuration error. */
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "generateHousekeepingPacket",
+                DATASET_NOT_FOUND);
+        return DATASET_NOT_FOUND;
+    }
+
+    store_address_t storeId;
+    HousekeepingPacketDownlink hkPacket(sid, dataSet);
+    size_t serializedSize = 0;
+    ReturnValue_t result = serializeHkPacketIntoStore(hkPacket, storeId,
+            forDownlink, &serializedSize);
+    if(result != HasReturnvaluesIF::RETURN_OK or serializedSize == 0) {
+        return result;
+    }
+
+    /* Now we set a HK message and send it the HK packet destination. */
+    CommandMessage hkMessage;
+    if(LocalPoolDataSetAttorney::isDiagnostics(*dataSet)) {
+        HousekeepingMessage::setHkDiagnosticsReply(&hkMessage, sid, storeId);
+    }
+    else {
+        HousekeepingMessage::setHkReportReply(&hkMessage, sid, storeId);
+    }
+
+    if(hkQueue == nullptr) {
+        /* Error, no queue available to send packet with. */
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "generateHousekeepingPacket",
+                QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
+    }
+    if(destination == MessageQueueIF::NO_QUEUE) {
+        if(hkDestinationId == MessageQueueIF::NO_QUEUE) {
+            /* Error, all destinations invalid */
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "generateHousekeepingPacket",
+                    QUEUE_OR_DESTINATION_INVALID);
+        }
+        destination = hkDestinationId;
+    }
+
+    return hkQueue->sendMessage(destination, &hkMessage);
+}
+
+ReturnValue_t LocalDataPoolManager::serializeHkPacketIntoStore(
+        HousekeepingPacketDownlink& hkPacket,
+        store_address_t& storeId, bool forDownlink,
+        size_t* serializedSize) {
+    uint8_t* dataPtr = nullptr;
+    const size_t maxSize = hkPacket.getSerializedSize();
+    ReturnValue_t result = ipcStore->getFreeElement(&storeId,
+            maxSize, &dataPtr);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    if(forDownlink) {
+        return hkPacket.serialize(&dataPtr, serializedSize, maxSize,
+                SerializeIF::Endianness::BIG);
+    }
+    return hkPacket.serialize(&dataPtr, serializedSize, maxSize,
+            SerializeIF::Endianness::MACHINE);
+}
+
+void LocalDataPoolManager::setNonDiagnosticIntervalFactor(
+        uint8_t nonDiagInvlFactor) {
+    this->nonDiagnosticIntervalFactor = nonDiagInvlFactor;
+}
+
+void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
+    sid_t sid = receiver.dataId.sid;
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "performPeriodicHkGeneration",
+                DATASET_NOT_FOUND);
+        return;
+    }
+
+    if(not LocalPoolDataSetAttorney::getReportingEnabled(*dataSet)) {
+        return;
+    }
+
+    PeriodicHousekeepingHelper* periodicHelper =
+            LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet);
+
+    if(periodicHelper == nullptr) {
+        /* Configuration error */
+        return;
+    }
+
+    if(not periodicHelper->checkOpNecessary()) {
+        return;
+    }
+
+    ReturnValue_t result = generateHousekeepingPacket(
+            sid, dataSet, true);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        /* Configuration error */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "LocalDataPoolManager::performHkOperation: HK generation failed." <<
+                std::endl;
+#else
+        sif::printWarning("LocalDataPoolManager::performHkOperation: HK generation failed.\n");
+#endif
+    }
+}
+
+
+ReturnValue_t LocalDataPoolManager::togglePeriodicGeneration(sid_t sid,
+        bool enable, bool isDiagnostics) {
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING, "togglePeriodicGeneration",
+                DATASET_NOT_FOUND);
+        return DATASET_NOT_FOUND;
+    }
+
+    if((LocalPoolDataSetAttorney::isDiagnostics(*dataSet) and not isDiagnostics) or
+            (not LocalPoolDataSetAttorney::isDiagnostics(*dataSet) and isDiagnostics)) {
+        return WRONG_HK_PACKET_TYPE;
+    }
+
+    if((LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and enable) or
+            (not LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and not enable)) {
+        return REPORTING_STATUS_UNCHANGED;
+    }
+
+    LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enable);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::changeCollectionInterval(sid_t sid,
+        float newCollectionInterval, bool isDiagnostics) {
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING, "changeCollectionInterval",
+                DATASET_NOT_FOUND);
+        return DATASET_NOT_FOUND;
+    }
+
+    bool targetIsDiagnostics = LocalPoolDataSetAttorney::isDiagnostics(*dataSet);
+    if((targetIsDiagnostics and not isDiagnostics) or
+            (not targetIsDiagnostics and isDiagnostics)) {
+        return WRONG_HK_PACKET_TYPE;
+    }
+
+    PeriodicHousekeepingHelper* periodicHelper =
+            LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet);
+
+    if(periodicHelper == nullptr) {
+        /* Configuration error, set might not have a corresponding pool manager */
+        return PERIODIC_HELPER_INVALID;
+    }
+
+    periodicHelper->changeCollectionInterval(newCollectionInterval);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalDataPoolManager::generateSetStructurePacket(sid_t sid,
+        bool isDiagnostics) {
+    /* Get and check dataset first. */
+    LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+    if(dataSet == nullptr) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "performPeriodicHkGeneration", DATASET_NOT_FOUND);
+        return DATASET_NOT_FOUND;
+    }
+
+
+    bool targetIsDiagnostics = LocalPoolDataSetAttorney::isDiagnostics(*dataSet);
+    if((targetIsDiagnostics and not isDiagnostics) or
+            (not targetIsDiagnostics and isDiagnostics)) {
+        return WRONG_HK_PACKET_TYPE;
+    }
+
+    bool valid = dataSet->isValid();
+    bool reportingEnabled = LocalPoolDataSetAttorney::getReportingEnabled(*dataSet);
+    float collectionInterval = LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet)->
+            getCollectionIntervalInSeconds();
+
+    // Generate set packet which can be serialized.
+    HousekeepingSetPacket setPacket(sid,
+            reportingEnabled, valid, collectionInterval, dataSet);
+    size_t expectedSize = setPacket.getSerializedSize();
+    uint8_t* storePtr = nullptr;
+    store_address_t storeId;
+    ReturnValue_t result = ipcStore->getFreeElement(&storeId,
+            expectedSize,&storePtr);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        printWarningOrError(sif::OutputTypes::OUT_ERROR,
+                "generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED,
+                "Could not get free element from IPC store.");
+        return result;
+    }
+
+    // Serialize set packet into store.
+    size_t size = 0;
+    result = setPacket.serialize(&storePtr, &size, expectedSize,
+            SerializeIF::Endianness::BIG);
+    if(expectedSize != size) {
+        printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                "generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED,
+                "Expected size is not equal to serialized size");
+    }
+
+    // Send structure reporting reply.
+    CommandMessage reply;
+    if(isDiagnostics) {
+        HousekeepingMessage::setDiagnosticsStuctureReportReply(&reply,
+                sid, storeId);
+    }
+    else {
+        HousekeepingMessage::setHkStuctureReportReply(&reply,
+                sid, storeId);
+    }
+
+    hkQueue->reply(&reply);
+    return result;
+}
+
+void LocalDataPoolManager::clearReceiversList() {
+    /* Clear the vector completely and releases allocated memory. */
+    HkReceivers().swap(hkReceivers);
+    /* Also clear the reset helper if it exists */
+    if(hkUpdateResetList != nullptr) {
+        HkUpdateResetList().swap(*hkUpdateResetList);
+    }
+}
+
+MutexIF* LocalDataPoolManager::getLocalPoolMutex() {
+    return this->mutex;
+}
+
+object_id_t LocalDataPoolManager::getCreatorObjectId() const {
+    return owner->getObjectId();
+}
+
+void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
+        const char* functionName, ReturnValue_t error, const char* errorPrint) {
+#if FSFW_VERBOSE_LEVEL >= 1
+    if(errorPrint == nullptr) {
+        if(error == DATASET_NOT_FOUND) {
+            errorPrint = "Dataset not found";
+        }
+        else if(error == POOLOBJECT_NOT_FOUND) {
+            errorPrint = "Pool Object not found";
+        }
+        else if(error == HasReturnvaluesIF::RETURN_FAILED) {
+            if(outputType == sif::OutputTypes::OUT_WARNING) {
+                errorPrint = "Generic Warning";
+            }
+            else {
+                errorPrint = "Generic error";
+            }
+        }
+        else if(error == QUEUE_OR_DESTINATION_INVALID) {
+            errorPrint = "Queue or destination not set";
+        }
+        else if(error == localpool::POOL_ENTRY_TYPE_CONFLICT) {
+            errorPrint = "Pool entry type conflict";
+        }
+        else if(error == localpool::POOL_ENTRY_NOT_FOUND) {
+            errorPrint = "Pool entry not found";
+        }
+        else {
+            errorPrint = "Unknown error";
+        }
+    }
+    object_id_t objectId = 0xffffffff;
+    if(owner != nullptr) {
+        objectId = owner->getObjectId();
+    }
+
+    if(outputType == sif::OutputTypes::OUT_WARNING) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+                std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+                std::dec << std::setfill(' ') << std::endl;
+#else
+        sif::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
+                functionName, objectId, errorPrint);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+    }
+    else if(outputType == sif::OutputTypes::OUT_ERROR) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+                std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+                std::dec << std::setfill(' ') << std::endl;
+#else
+        sif::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
+                functionName, objectId, errorPrint);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+    }
+#endif /* #if FSFW_VERBOSE_LEVEL >= 1 */
+}
+
+LocalDataPoolManager* LocalDataPoolManager::getPoolManagerHandle() {
+    return this;
+}

src/core/datapoollocal/LocalDataSet.cpp

@@ -0,0 +1,22 @@
+#include "LocalDataSet.h"
+#include "../datapoollocal/LocalDataPoolManager.h"
+#include "../serialize/SerializeAdapter.h"
+
+#include <cmath>
+#include <cstring>
+
+LocalDataSet::LocalDataSet(HasLocalDataPoolIF *hkOwner, uint32_t setId,
+        const size_t maxNumberOfVariables):
+        LocalPoolDataSetBase(hkOwner, setId, nullptr, maxNumberOfVariables),
+        poolVarList(maxNumberOfVariables) {
+    this->setContainer(poolVarList.data());
+}
+
+LocalDataSet::LocalDataSet(sid_t sid, const size_t maxNumberOfVariables):
+        LocalPoolDataSetBase(sid, nullptr, maxNumberOfVariables),
+        poolVarList(maxNumberOfVariables) {
+    this->setContainer(poolVarList.data());
+}
+
+LocalDataSet::~LocalDataSet() {}
+

src/core/datapoollocal/LocalPoolDataSetBase.cpp

@@ -0,0 +1,324 @@
+#include "LocalPoolDataSetBase.h"
+#include "HasLocalDataPoolIF.h"
+#include "internal/HasLocalDpIFUserAttorney.h"
+
+#include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../globalfunctions/bitutility.h"
+#include "../datapoollocal/LocalDataPoolManager.h"
+#include "../housekeeping/PeriodicHousekeepingHelper.h"
+#include "../serialize/SerializeAdapter.h"
+
+#include <cmath>
+#include <cstring>
+
+LocalPoolDataSetBase::LocalPoolDataSetBase(HasLocalDataPoolIF *hkOwner,
+        uint32_t setId, PoolVariableIF** registeredVariablesArray,
+        const size_t maxNumberOfVariables, bool periodicHandling):
+        PoolDataSetBase(registeredVariablesArray, maxNumberOfVariables) {
+    if(hkOwner == nullptr) {
+        // Configuration error.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "LocalPoolDataSetBase::LocalPoolDataSetBase: Owner "
+                << "invalid!" << std::endl;
+#else
+        sif::printError("LocalPoolDataSetBase::LocalPoolDataSetBase: Owner "
+                "invalid!\n\r");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+        return;
+    }
+    AccessPoolManagerIF* accessor = HasLocalDpIFUserAttorney::getAccessorHandle(hkOwner);
+
+    if(accessor != nullptr) {
+        poolManager = accessor->getPoolManagerHandle();
+        mutexIfSingleDataCreator = accessor->getLocalPoolMutex();
+    }
+
+    this->sid.objectId = hkOwner->getObjectId();
+    this->sid.ownerSetId = setId;
+
+    /* Data creators get a periodic helper for periodic HK data generation. */
+    if(periodicHandling) {
+        periodicHelper = new PeriodicHousekeepingHelper(this);
+    }
+}
+
+LocalPoolDataSetBase::LocalPoolDataSetBase(sid_t sid, PoolVariableIF** registeredVariablesArray,
+        const size_t maxNumberOfVariables):
+        PoolDataSetBase(registeredVariablesArray, maxNumberOfVariables)  {
+    HasLocalDataPoolIF* hkOwner = ObjectManager::instance()->get<HasLocalDataPoolIF>(
+            sid.objectId);
+    if(hkOwner != nullptr) {
+        AccessPoolManagerIF* accessor = HasLocalDpIFUserAttorney::getAccessorHandle(hkOwner);
+        if(accessor != nullptr) {
+            mutexIfSingleDataCreator = accessor->getLocalPoolMutex();
+            poolManager = accessor->getPoolManagerHandle();
+        }
+    }
+
+    this->sid = sid;
+}
+
+LocalPoolDataSetBase::LocalPoolDataSetBase(PoolVariableIF **registeredVariablesArray,
+        const size_t maxNumberOfVariables, bool protectEveryReadCommitCall):
+		        PoolDataSetBase(registeredVariablesArray, maxNumberOfVariables) {
+    this->setReadCommitProtectionBehaviour(protectEveryReadCommitCall);
+}
+
+
+LocalPoolDataSetBase::~LocalPoolDataSetBase() {
+    /* We only delete objects which were created in the class constructor */
+    if(periodicHelper != nullptr) {
+        delete periodicHelper;
+    }
+    /* In case set was read but not comitted, we commit all variables with an invalid state */
+    if(state == States::STATE_SET_WAS_READ) {
+        for (uint16_t count = 0; count < fillCount; count++) {
+            if(registeredVariables[count] != nullptr) {
+                registeredVariables[count]->setValid(false);
+                registeredVariables[count]->commit(MutexIF::TimeoutType::WAITING, 20);
+            }
+        }
+    }
+}
+
+ReturnValue_t LocalPoolDataSetBase::lockDataPool(
+        MutexIF::TimeoutType timeoutType,
+        uint32_t timeoutMs) {
+    if(mutexIfSingleDataCreator != nullptr) {
+        return mutexIfSingleDataCreator->lockMutex(timeoutType, timeoutMs);
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalPoolDataSetBase::serializeWithValidityBuffer(uint8_t **buffer,
+        size_t *size, size_t maxSize,
+        SerializeIF::Endianness streamEndianness) const {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    const uint8_t validityMaskSize = std::ceil(static_cast<float>(fillCount)/8.0);
+    uint8_t* validityPtr = nullptr;
+#ifdef _MSC_VER
+    /* Use a std::vector here because MSVC will (rightly) not create a fixed size array
+    with a non constant size specifier */
+    std::vector<uint8_t> validityMask(validityMaskSize);
+    validityPtr = validityMask.data();
+#else
+    uint8_t validityMask[validityMaskSize] = {0};
+    validityPtr = validityMask;
+#endif
+    uint8_t validBufferIndex = 0;
+    uint8_t validBufferIndexBit = 0;
+    for (uint16_t count = 0; count < fillCount; count++) {
+        if(registeredVariables[count]->isValid()) {
+            /* Set bit at correct position */
+            bitutil::bitSet(validityPtr + validBufferIndex, validBufferIndexBit);
+        }
+        if(validBufferIndexBit == 7) {
+            validBufferIndex ++;
+            validBufferIndexBit = 0;
+        }
+        else {
+            validBufferIndexBit ++;
+        }
+
+        result = registeredVariables[count]->serialize(buffer, size, maxSize,
+                streamEndianness);
+        if (result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+    }
+
+    if(*size + validityMaskSize > maxSize) {
+        return SerializeIF::BUFFER_TOO_SHORT;
+    }
+    // copy validity buffer to end
+    std::memcpy(*buffer, validityPtr, validityMaskSize);
+    *size += validityMaskSize;
+    return result;
+}
+
+ReturnValue_t LocalPoolDataSetBase::deSerializeWithValidityBuffer(
+        const uint8_t **buffer, size_t *size,
+        SerializeIF::Endianness streamEndianness) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+    for (uint16_t count = 0; count < fillCount; count++) {
+        result = registeredVariables[count]->deSerialize(buffer, size,
+                streamEndianness);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+    }
+
+    if(*size < std::ceil(static_cast<float>(fillCount) / 8.0)) {
+        return SerializeIF::STREAM_TOO_SHORT;
+    }
+
+    uint8_t validBufferIndex = 0;
+    uint8_t validBufferIndexBit = 0;
+    for (uint16_t count = 0; count < fillCount; count++) {
+        // set validity buffer here.
+        bool nextVarValid = bitutil::bitGet(*buffer +
+                validBufferIndex, validBufferIndexBit);
+        registeredVariables[count]->setValid(nextVarValid);
+
+        if(validBufferIndexBit == 7) {
+            validBufferIndex ++;
+            validBufferIndexBit = 0;
+        }
+        else {
+            validBufferIndexBit ++;
+        }
+    }
+    return result;
+}
+
+ReturnValue_t LocalPoolDataSetBase::unlockDataPool() {
+    if(mutexIfSingleDataCreator != nullptr) {
+        return mutexIfSingleDataCreator->unlockMutex();
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t LocalPoolDataSetBase::serializeLocalPoolIds(uint8_t** buffer,
+        size_t* size, size_t maxSize,SerializeIF::Endianness streamEndianness,
+        bool serializeFillCount) const {
+    /* Serialize fill count as uint8_t */
+    uint8_t fillCount = this->fillCount;
+    if(serializeFillCount) {
+        SerializeAdapter::serialize(&fillCount, buffer, size, maxSize,
+                streamEndianness);
+    }
+    for (uint16_t count = 0; count < fillCount; count++) {
+        lp_id_t currentPoolId = registeredVariables[count]->getDataPoolId();
+        auto result = SerializeAdapter::serialize(&currentPoolId, buffer,
+                size, maxSize, streamEndianness);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::warning << "LocalPoolDataSetBase::serializeLocalPoolIds: "
+                    << "Serialization error!" << std::endl;
+#else
+            sif::printWarning("LocalPoolDataSetBase::serializeLocalPoolIds: "
+                    "Serialization error!\n\r");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+            return result;
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+uint8_t LocalPoolDataSetBase::getLocalPoolIdsSerializedSize(
+        bool serializeFillCount) const {
+    if(serializeFillCount) {
+        return fillCount * sizeof(lp_id_t) + sizeof(uint8_t);
+    }
+    else {
+        return fillCount * sizeof(lp_id_t);
+    }
+}
+
+size_t LocalPoolDataSetBase::getSerializedSize() const {
+    if(withValidityBuffer) {
+        uint8_t validityMaskSize = std::ceil(static_cast<float>(fillCount)/8.0);
+        return validityMaskSize + PoolDataSetBase::getSerializedSize();
+    }
+    else {
+        return PoolDataSetBase::getSerializedSize();
+    }
+}
+
+void LocalPoolDataSetBase::setValidityBufferGeneration(
+        bool withValidityBuffer) {
+    this->withValidityBuffer = withValidityBuffer;
+}
+
+ReturnValue_t LocalPoolDataSetBase::deSerialize(const uint8_t **buffer,
+        size_t *size, SerializeIF::Endianness streamEndianness) {
+    if(withValidityBuffer) {
+        return this->deSerializeWithValidityBuffer(buffer, size,
+                streamEndianness);
+    }
+    else {
+        return PoolDataSetBase::deSerialize(buffer, size, streamEndianness);
+    }
+}
+
+ReturnValue_t LocalPoolDataSetBase::serialize(uint8_t **buffer, size_t *size,
+        size_t maxSize, SerializeIF::Endianness streamEndianness) const {
+    if(withValidityBuffer) {
+        return this->serializeWithValidityBuffer(buffer, size,
+                maxSize, streamEndianness);
+    }
+    else {
+        return PoolDataSetBase::serialize(buffer, size, maxSize,
+                streamEndianness);
+    }
+}
+
+void LocalPoolDataSetBase::setDiagnostic(bool isDiagnostics) {
+    this->diagnostic = isDiagnostics;
+}
+
+bool LocalPoolDataSetBase::isDiagnostics() const {
+    return diagnostic;
+}
+
+void LocalPoolDataSetBase::setReportingEnabled(bool reportingEnabled) {
+    this->reportingEnabled = reportingEnabled;
+}
+
+bool LocalPoolDataSetBase::getReportingEnabled() const {
+    return reportingEnabled;
+}
+
+void LocalPoolDataSetBase::initializePeriodicHelper(float collectionInterval,
+        dur_millis_t minimumPeriodicInterval, uint8_t nonDiagIntervalFactor) {
+    periodicHelper->initialize(collectionInterval, minimumPeriodicInterval, nonDiagIntervalFactor);
+}
+
+void LocalPoolDataSetBase::setChanged(bool changed) {
+    this->changed = changed;
+}
+
+bool LocalPoolDataSetBase::hasChanged() const {
+    return changed;
+}
+
+sid_t LocalPoolDataSetBase::getSid() const {
+    return sid;
+}
+
+bool LocalPoolDataSetBase::isValid() const {
+    return this->valid;
+}
+
+void LocalPoolDataSetBase::setValidity(bool valid, bool setEntriesRecursively) {
+    if(setEntriesRecursively) {
+        for(size_t idx = 0; idx < this->getFillCount(); idx++) {
+            registeredVariables[idx]->setValid(valid);
+        }
+    }
+    this->valid = valid;
+}
+
+object_id_t LocalPoolDataSetBase::getCreatorObjectId() {
+    if(poolManager != nullptr) {
+        return poolManager->getCreatorObjectId();
+    }
+    return objects::NO_OBJECT;
+}
+
+void LocalPoolDataSetBase::setAllVariablesReadOnly() {
+    for(size_t idx = 0; idx < this->getFillCount(); idx++) {
+        registeredVariables[idx]->setReadWriteMode(pool_rwm_t::VAR_READ);
+    }
+}
+
+float LocalPoolDataSetBase::getCollectionInterval() const {
+    if(periodicHelper != nullptr) {
+        return periodicHelper->getCollectionIntervalInSeconds();
+    }
+    else {
+        return 0.0;
+    }
+}

src/core/datapoollocal/LocalPoolObjectBase.cpp

@@ -0,0 +1,141 @@
+#include "LocalPoolObjectBase.h"
+#include "LocalDataPoolManager.h"
+#include "AccessLocalPoolF.h"
+#include "HasLocalDataPoolIF.h"
+#include "internal/HasLocalDpIFUserAttorney.h"
+
+#include "../objectmanager/ObjectManager.h"
+
+
+LocalPoolObjectBase::LocalPoolObjectBase(lp_id_t poolId, HasLocalDataPoolIF* hkOwner,
+        DataSetIF* dataSet, pool_rwm_t setReadWriteMode):
+        localPoolId(poolId), readWriteMode(setReadWriteMode) {
+    if(poolId == PoolVariableIF::NO_PARAMETER) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "LocalPoolVar<T>::LocalPoolVar: 0 passed as pool ID, "
+                << "which is the NO_PARAMETER value!" << std::endl;
+#endif
+    }
+    if(hkOwner == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "LocalPoolVar<T>::LocalPoolVar: The supplied pool "
+                << "owner is a invalid!" << std::endl;
+#endif
+        return;
+    }
+    AccessPoolManagerIF* poolManAccessor = HasLocalDpIFUserAttorney::getAccessorHandle(hkOwner);
+    hkManager = poolManAccessor->getPoolManagerHandle();
+
+    if (dataSet != nullptr) {
+        dataSet->registerVariable(this);
+    }
+}
+
+LocalPoolObjectBase::LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId, DataSetIF *dataSet,
+        pool_rwm_t setReadWriteMode):
+		                localPoolId(poolId), readWriteMode(setReadWriteMode) {
+    if(poolId == PoolVariableIF::NO_PARAMETER) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "LocalPoolVar<T>::LocalPoolVar: 0 passed as pool ID, "
+                "which is the NO_PARAMETER value!" << std::endl;
+#else
+        sif::printWarning("LocalPoolVar<T>::LocalPoolVar: 0 passed as pool ID, "
+                "which is the NO_PARAMETER value!\n");
+#endif
+    }
+    HasLocalDataPoolIF* hkOwner = ObjectManager::instance()->get<HasLocalDataPoolIF>(poolOwner);
+    if(hkOwner == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "LocalPoolVariable: The supplied pool owner did not implement the correct "
+                "interface HasLocalDataPoolIF!" << std::endl;
+#else
+        sif::printError( "LocalPoolVariable: The supplied pool owner did not implement the correct "
+                "interface HasLocalDataPoolIF!\n");
+#endif
+        return;
+    }
+
+    AccessPoolManagerIF* accessor = HasLocalDpIFUserAttorney::getAccessorHandle(hkOwner);
+    if(accessor != nullptr) {
+        hkManager = accessor->getPoolManagerHandle();
+    }
+
+    if(dataSet != nullptr) {
+        dataSet->registerVariable(this);
+    }
+}
+
+pool_rwm_t LocalPoolObjectBase::getReadWriteMode() const {
+    return readWriteMode;
+}
+
+bool LocalPoolObjectBase::isValid() const {
+    return valid;
+}
+
+void LocalPoolObjectBase::setValid(bool valid) {
+    this->valid = valid;
+}
+
+lp_id_t LocalPoolObjectBase::getDataPoolId() const {
+    return localPoolId;
+}
+
+void LocalPoolObjectBase::setDataPoolId(lp_id_t poolId) {
+    this->localPoolId = poolId;
+}
+
+void LocalPoolObjectBase::setChanged(bool changed) {
+    this->changed = changed;
+}
+
+bool LocalPoolObjectBase::hasChanged() const {
+    return changed;
+}
+
+void LocalPoolObjectBase::setReadWriteMode(pool_rwm_t newReadWriteMode) {
+    this->readWriteMode = newReadWriteMode;
+}
+
+void LocalPoolObjectBase::reportReadCommitError(const char* variableType,
+        ReturnValue_t error, bool read, object_id_t objectId, lp_id_t lpId) {
+#if FSFW_DISABLE_PRINTOUT == 0
+    const char* variablePrintout = variableType;
+    if(variablePrintout == nullptr) {
+        variablePrintout = "Unknown Type";
+    }
+    const char* type = nullptr;
+    if(read) {
+        type = "read";
+    }
+    else {
+        type = "commit";
+    }
+
+    const char* errMsg = nullptr;
+    if(error == localpool::POOL_ENTRY_NOT_FOUND) {
+        errMsg = "Pool entry not found";
+    }
+    else if(error == localpool::POOL_ENTRY_TYPE_CONFLICT) {
+        errMsg = "Pool entry type conflict";
+    }
+    else if(error == PoolVariableIF::INVALID_READ_WRITE_MODE) {
+        errMsg = "Pool variable wrong read-write mode";
+    }
+    else if(error == PoolVariableIF::INVALID_POOL_ENTRY) {
+        errMsg = "Pool entry invalid";
+    }
+    else {
+        errMsg = "Unknown error code";
+    }
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << variablePrintout << ": " << type << " call | " << errMsg << " | Owner: 0x"
+            << std::hex << std::setw(8) << std::setfill('0') << objectId << std::dec
+            << " LPID: " << lpId << std::endl;
+#else
+    sif::printWarning("%s: %s call | %s | Owner: 0x%08x LPID: %lu\n",
+            variablePrintout, type, errMsg, objectId, lpId);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_DISABLE_PRINTOUT == 0 */
+}

src/core/datapoollocal/SharedLocalDataSet.cpp

@@ -0,0 +1,37 @@
+#include "SharedLocalDataSet.h"
+
+
+SharedLocalDataSet::SharedLocalDataSet(object_id_t objectId, sid_t sid,
+        const size_t maxSize): SystemObject(objectId),
+        LocalPoolDataSetBase(sid, nullptr, maxSize), poolVarVector(maxSize) {
+    this->setContainer(poolVarVector.data());
+    datasetLock = MutexFactory::instance()->createMutex();
+}
+
+SharedLocalDataSet::SharedLocalDataSet(object_id_t objectId,
+        HasLocalDataPoolIF *owner, uint32_t setId,
+        const size_t maxSize): SystemObject(objectId),
+        LocalPoolDataSetBase(owner, setId, nullptr, maxSize), poolVarVector(maxSize)  {
+    this->setContainer(poolVarVector.data());
+    datasetLock = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t SharedLocalDataSet::lockDataset(MutexIF::TimeoutType timeoutType,
+        dur_millis_t mutexTimeout) {
+    if(datasetLock != nullptr) {
+        return datasetLock->lockMutex(timeoutType, mutexTimeout);
+    }
+    return HasReturnvaluesIF::RETURN_FAILED;
+}
+
+
+SharedLocalDataSet::~SharedLocalDataSet() {
+    MutexFactory::instance()->deleteMutex(datasetLock);
+}
+
+ReturnValue_t SharedLocalDataSet::unlockDataset() {
+    if(datasetLock != nullptr) {
+        return datasetLock->unlockMutex();
+    }
+    return HasReturnvaluesIF::RETURN_FAILED;
+}

src/core/datapoollocal/internal/CMakeLists.txt

@@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		HasLocalDpIFUserAttorney.cpp
+		HasLocalDpIFManagerAttorney.cpp
+)

src/core/datapoollocal/internal/HasLocalDpIFManagerAttorney.cpp

@@ -0,0 +1,18 @@
+#include "HasLocalDpIFManagerAttorney.h"
+#include "../LocalPoolObjectBase.h"
+#include "../LocalPoolDataSetBase.h"
+#include "../HasLocalDataPoolIF.h"
+
+LocalPoolDataSetBase* HasLocalDpIFManagerAttorney::getDataSetHandle(HasLocalDataPoolIF* clientIF,
+        sid_t sid) {
+    return clientIF->getDataSetHandle(sid);
+}
+
+LocalPoolObjectBase* HasLocalDpIFManagerAttorney::getPoolObjectHandle(HasLocalDataPoolIF* clientIF,
+        lp_id_t localPoolId) {
+    return clientIF->getPoolObjectHandle(localPoolId);
+}
+
+object_id_t HasLocalDpIFManagerAttorney::getObjectId(HasLocalDataPoolIF* clientIF) {
+    return clientIF->getObjectId();
+}

src/core/datapoollocal/internal/HasLocalDpIFManagerAttorney.h

@@ -0,0 +1,22 @@
+#ifndef FSFW_DATAPOOLLOCAL_HASLOCALDPIFMANAGERATTORNEY_H_
+#define FSFW_DATAPOOLLOCAL_HASLOCALDPIFMANAGERATTORNEY_H_
+
+#include "../localPoolDefinitions.h"
+
+class HasLocalDataPoolIF;
+class LocalPoolDataSetBase;
+class LocalPoolObjectBase;
+
+class HasLocalDpIFManagerAttorney {
+
+    static LocalPoolDataSetBase* getDataSetHandle(HasLocalDataPoolIF* clientIF, sid_t sid);
+
+    static LocalPoolObjectBase* getPoolObjectHandle(HasLocalDataPoolIF* clientIF,
+            lp_id_t localPoolId);
+
+    static object_id_t getObjectId(HasLocalDataPoolIF* clientIF);
+
+    friend class LocalDataPoolManager;
+};
+
+#endif /* FSFW_DATAPOOLLOCAL_HASLOCALDPIFMANAGERATTORNEY_H_ */

src/core/datapoollocal/internal/HasLocalDpIFUserAttorney.cpp

@@ -0,0 +1,7 @@
+#include "HasLocalDpIFUserAttorney.h"
+#include "../AccessLocalPoolF.h"
+#include "../HasLocalDataPoolIF.h"
+
+AccessPoolManagerIF* HasLocalDpIFUserAttorney::getAccessorHandle(HasLocalDataPoolIF *clientIF) {
+	return clientIF->getAccessorHandle();
+}

src/core/datapoollocal/internal/HasLocalDpIFUserAttorney.h

@@ -0,0 +1,18 @@
+#ifndef FSFW_DATAPOOLLOCAL_HASLOCALDPIFUSERATTORNEY_H_
+#define FSFW_DATAPOOLLOCAL_HASLOCALDPIFUSERATTORNEY_H_
+
+class HasLocalDataPoolIF;
+class AccessPoolManagerIF;
+
+class HasLocalDpIFUserAttorney {
+private:
+
+    static AccessPoolManagerIF* getAccessorHandle(HasLocalDataPoolIF* clientIF);
+
+    friend class LocalPoolObjectBase;
+    friend class LocalPoolDataSetBase;
+
+};
+
+
+#endif /* FSFW_DATAPOOLLOCAL_HASLOCALDPIFUSERATTORNEY_H_ */

src/core/datapoollocal/internal/LocalDpManagerAttorney.h

@@ -0,0 +1,31 @@
+#ifndef FSFW_DATAPOOLLOCAL_LOCALDPMANAGERATTORNEY_H_
+#define FSFW_DATAPOOLLOCAL_LOCALDPMANAGERATTORNEY_H_
+
+#include "../LocalDataPoolManager.h"
+
+/**
+ * @brief	This is a helper class implements the Attorney-Client idiom for access to
+ * 			LocalDataPoolManager internals
+ * @details
+ * This helper class provides better control over which classes are allowed to access
+ * LocalDataPoolManager internals in a granular and encapsulated way when compared to
+ * other methods like direct friend declarations. It allows these classes to use
+ * an explicit subset of the pool manager private/protected functions.
+ * See: https://en.wikibooks.org/wiki/More_C%2B%2B_Idioms/Friendship_and_the_Attorney-Client
+ */
+class LocalDpManagerAttorney {
+private:
+    template<typename T> static ReturnValue_t fetchPoolEntry(LocalDataPoolManager& manager,
+            lp_id_t localPoolId, PoolEntry<T> **poolEntry) {
+        return manager.fetchPoolEntry(localPoolId, poolEntry);
+    }
+
+    static MutexIF* getMutexHandle(LocalDataPoolManager& manager) {
+        return manager.getMutexHandle();
+    }
+
+    template<typename T> friend class LocalPoolVariable;
+    template<typename T, uint16_t vecSize> friend class LocalPoolVector;
+};
+
+#endif /* FSFW_DATAPOOLLOCAL_LOCALDPMANAGERATTORNEY_H_ */

src/core/datapoollocal/internal/LocalPoolDataSetAttorney.h

@@ -0,0 +1,38 @@
+#ifndef FSFW_DATAPOOLLOCAL_LOCALPOOLDATASETATTORNEY_H_
+#define FSFW_DATAPOOLLOCAL_LOCALPOOLDATASETATTORNEY_H_
+
+#include "../LocalPoolDataSetBase.h"
+
+class LocalPoolDataSetAttorney {
+private:
+    static void setDiagnostic(LocalPoolDataSetBase& set, bool diagnostics) {
+        set.setDiagnostic(diagnostics);
+    }
+
+    static bool isDiagnostics(LocalPoolDataSetBase& set) {
+        return set.isDiagnostics();
+    }
+
+    static void initializePeriodicHelper(LocalPoolDataSetBase& set, float collectionInterval,
+            uint32_t minimumPeriodicIntervalMs, uint8_t nonDiagIntervalFactor = 5) {
+        set.initializePeriodicHelper(collectionInterval, minimumPeriodicIntervalMs,
+                nonDiagIntervalFactor);
+    }
+
+    static void setReportingEnabled(LocalPoolDataSetBase& set, bool enabled) {
+        set.setReportingEnabled(enabled);
+    }
+
+    static bool getReportingEnabled(LocalPoolDataSetBase& set) {
+        return set.getReportingEnabled();
+    }
+
+    static PeriodicHousekeepingHelper* getPeriodicHelper(LocalPoolDataSetBase& set) {
+        return set.periodicHelper;
+    }
+
+    friend class LocalDataPoolManager;
+};
+
+
+#endif /* FSFW_DATAPOOLLOCAL_LOCALPOOLDATASETATTORNEY_H_ */

src/core/devicehandlers/AssemblyBase.cpp

@@ -0,0 +1,273 @@
+#include "AssemblyBase.h"
+
+AssemblyBase::AssemblyBase(object_id_t objectId, object_id_t parentId,
+		uint16_t commandQueueDepth) :
+		SubsystemBase(objectId, parentId, MODE_OFF, commandQueueDepth),
+		internalState(STATE_NONE), recoveryState(RECOVERY_IDLE),
+		recoveringDevice(childrenMap.end()), targetMode(MODE_OFF),
+		targetSubmode(SUBMODE_NONE) {
+	recoveryOffTimer.setTimeout(POWER_OFF_TIME_MS);
+}
+
+AssemblyBase::~AssemblyBase() {
+}
+
+ReturnValue_t AssemblyBase::handleCommandMessage(CommandMessage* message) {
+	return handleHealthReply(message);
+}
+
+void AssemblyBase::performChildOperation() {
+	if (isInTransition()) {
+		handleChildrenTransition();
+	} else {
+		handleChildrenChanged();
+	}
+}
+
+void AssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
+	doStartTransition(mode, submode);
+	if (modeHelper.isForced()) {
+		triggerEvent(FORCING_MODE, mode, submode);
+	} else {
+		triggerEvent(CHANGING_MODE, mode, submode);
+	}
+}
+
+void AssemblyBase::doStartTransition(Mode_t mode, Submode_t submode) {
+	targetMode = mode;
+	targetSubmode = submode;
+	internalState = STATE_SINGLE_STEP;
+	ReturnValue_t result = commandChildren(mode, submode);
+	if (result == NEED_SECOND_STEP) {
+		internalState = STATE_NEED_SECOND_STEP;
+	}
+}
+
+bool AssemblyBase::isInTransition() {
+	return (internalState != STATE_NONE) || (recoveryState != RECOVERY_IDLE);
+}
+
+bool AssemblyBase::handleChildrenChanged() {
+	if (childrenChangedMode) {
+		ReturnValue_t result = checkChildrenState();
+		if (result != RETURN_OK) {
+			handleChildrenLostMode(result);
+		}
+		return true;
+	} else {
+		return handleChildrenChangedHealth();
+	}
+}
+
+void AssemblyBase::handleChildrenLostMode(ReturnValue_t result) {
+	triggerEvent(CANT_KEEP_MODE, mode, submode);
+	startTransition(MODE_OFF, SUBMODE_NONE);
+}
+
+bool AssemblyBase::handleChildrenChangedHealth() {
+	auto iter = childrenMap.begin();
+	for (; iter != childrenMap.end(); iter++) {
+		if (iter->second.healthChanged) {
+			iter->second.healthChanged = false;
+			break;
+		}
+	}
+	if (iter == childrenMap.end()) {
+		return false;
+	}
+	HealthState healthState = healthHelper.healthTable->getHealth(iter->first);
+	if (healthState == HasHealthIF::NEEDS_RECOVERY) {
+		triggerEvent(TRYING_RECOVERY);
+		recoveryState = RECOVERY_STARTED;
+		recoveringDevice = iter;
+		doStartTransition(targetMode, targetSubmode);
+	} else {
+		triggerEvent(CHILD_CHANGED_HEALTH);
+		doStartTransition(mode, submode);
+	}
+	if (modeHelper.isForced()) {
+		triggerEvent(FORCING_MODE, targetMode, targetSubmode);
+	}
+	return true;
+}
+
+void AssemblyBase::handleChildrenTransition() {
+	if (commandsOutstanding <= 0) {
+		switch (internalState) {
+		case STATE_NEED_SECOND_STEP:
+			internalState = STATE_SECOND_STEP;
+			commandChildren(targetMode, targetSubmode);
+			return;
+		case STATE_OVERWRITE_HEALTH: {
+			internalState = STATE_SINGLE_STEP;
+			ReturnValue_t result = commandChildren(mode, submode);
+			if (result == NEED_SECOND_STEP) {
+				internalState = STATE_NEED_SECOND_STEP;
+			}
+			return;
+		}
+		case STATE_NONE:
+			//Valid state, used in recovery.
+		case STATE_SINGLE_STEP:
+		case STATE_SECOND_STEP:
+			if (checkAndHandleRecovery()) {
+				return;
+			}
+			break;
+		}
+		ReturnValue_t result = checkChildrenState();
+		if (result == RETURN_OK) {
+			handleModeReached();
+		} else {
+			handleModeTransitionFailed(result);
+		}
+	}
+}
+
+void AssemblyBase::handleModeReached() {
+	internalState = STATE_NONE;
+	setMode(targetMode, targetSubmode);
+}
+
+void AssemblyBase::handleModeTransitionFailed(ReturnValue_t result) {
+//always accept transition to OFF, there is nothing we can do except sending an info event
+//In theory this should never happen, but we would risk an infinite loop otherwise
+	if (targetMode == MODE_OFF) {
+		triggerEvent(CHILD_PROBLEMS, result);
+		internalState = STATE_NONE;
+		setMode(targetMode, targetSubmode);
+	} else {
+		if (handleChildrenChangedHealth()) {
+			//If any health change is pending, handle that first.
+			return;
+		}
+		triggerEvent(MODE_TRANSITION_FAILED, result);
+		startTransition(MODE_OFF, SUBMODE_NONE);
+	}
+}
+
+void AssemblyBase::sendHealthCommand(MessageQueueId_t sendTo,
+		HealthState health) {
+	CommandMessage command;
+	HealthMessage::setHealthMessage(&command, HealthMessage::HEALTH_SET,
+			health);
+	if (commandQueue->sendMessage(sendTo, &command) == RETURN_OK) {
+		commandsOutstanding++;
+	}
+}
+
+ReturnValue_t AssemblyBase::checkChildrenState() {
+	if (targetMode == MODE_OFF) {
+		return checkChildrenStateOff();
+	} else {
+		return checkChildrenStateOn(targetMode, targetSubmode);
+	}
+}
+
+ReturnValue_t AssemblyBase::checkChildrenStateOff() {
+	for (const auto& childIter: childrenMap) {
+		if (checkChildOff(childIter.first) != RETURN_OK) {
+			return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+		}
+	}
+	return RETURN_OK;
+}
+
+ReturnValue_t AssemblyBase::checkChildOff(uint32_t objectId) {
+	ChildInfo childInfo = childrenMap.find(objectId)->second;
+	if (healthHelper.healthTable->isCommandable(objectId)) {
+		if (childInfo.submode != SUBMODE_NONE) {
+			return RETURN_FAILED;
+		} else {
+			if ((childInfo.mode != MODE_OFF)
+					&& (childInfo.mode != DeviceHandlerIF::MODE_ERROR_ON)) {
+				return RETURN_FAILED;
+			}
+		}
+	}
+	return RETURN_OK;
+}
+
+ReturnValue_t AssemblyBase::checkModeCommand(Mode_t mode, Submode_t submode,
+		uint32_t* msToReachTheMode) {
+
+//always accept transition to OFF
+	if (mode == MODE_OFF) {
+		if (submode != SUBMODE_NONE) {
+			return INVALID_SUBMODE;
+		}
+		return RETURN_OK;
+	}
+
+	if ((mode != MODE_ON) && (mode != DeviceHandlerIF::MODE_NORMAL)) {
+		return INVALID_MODE;
+	}
+
+	if (internalState != STATE_NONE) {
+		return IN_TRANSITION;
+	}
+
+	return isModeCombinationValid(mode, submode);
+}
+
+ReturnValue_t AssemblyBase::handleHealthReply(CommandMessage* message) {
+	if (message->getCommand() == HealthMessage::HEALTH_INFO) {
+		HealthState health = HealthMessage::getHealth(message);
+		if (health != EXTERNAL_CONTROL) {
+			updateChildChangedHealth(message->getSender(), true);
+		}
+		return RETURN_OK;
+	}
+	if (message->getCommand() == HealthMessage::REPLY_HEALTH_SET
+			|| (message->getCommand() == CommandMessage::REPLY_REJECTED
+					&& message->getParameter2() == HealthMessage::HEALTH_SET)) {
+		if (isInTransition()) {
+			commandsOutstanding--;
+		}
+		return RETURN_OK;
+	}
+	return RETURN_FAILED;
+}
+
+bool AssemblyBase::checkAndHandleRecovery() {
+	switch (recoveryState) {
+	case RECOVERY_STARTED:
+		recoveryState = RECOVERY_WAIT;
+		recoveryOffTimer.resetTimer();
+		return true;
+	case RECOVERY_WAIT:
+		if (recoveryOffTimer.isBusy()) {
+			return true;
+		}
+		triggerEvent(RECOVERY_STEP, 0);
+		sendHealthCommand(recoveringDevice->second.commandQueue, HEALTHY);
+		internalState = STATE_NONE;
+		recoveryState = RECOVERY_ONGOING;
+		//Don't check state!
+		return true;
+	case RECOVERY_ONGOING:
+		triggerEvent(RECOVERY_STEP, 1);
+		recoveryState = RECOVERY_ONGOING_2;
+		recoveringDevice->second.healthChanged = false;
+		//Device should be healthy again, so restart a transition.
+		//Might be including second step, but that's already handled.
+		doStartTransition(targetMode, targetSubmode);
+		return true;
+	case RECOVERY_ONGOING_2:
+		triggerEvent(RECOVERY_DONE);
+		//Now we're through, but not sure if it was successful.
+		recoveryState = RECOVERY_IDLE;
+		return false;
+	case RECOVERY_IDLE:
+	default:
+		return false;
+	}
+}
+
+void AssemblyBase::overwriteDeviceHealth(object_id_t objectId,
+		HasHealthIF::HealthState oldHealth) {
+	triggerEvent(OVERWRITING_HEALTH, objectId, oldHealth);
+	internalState = STATE_OVERWRITE_HEALTH;
+	modeHelper.setForced(true);
+	sendHealthCommand(childrenMap[objectId].commandQueue, EXTERNAL_CONTROL);
+}

src/core/devicehandlers/ChildHandlerBase.cpp

@@ -0,0 +1,46 @@
+#include "ChildHandlerBase.h"
+#include "../subsystem/SubsystemBase.h"
+
+ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId,
+		object_id_t deviceCommunication, CookieIF * cookie,
+		object_id_t hkDestination, uint32_t thermalStatePoolId,
+		uint32_t thermalRequestPoolId,
+		object_id_t parent,
+		FailureIsolationBase* customFdir, size_t cmdQueueSize) :
+		DeviceHandlerBase(setObjectId, deviceCommunication, cookie,
+			(customFdir == nullptr? &childHandlerFdir : customFdir),
+			cmdQueueSize),
+		parentId(parent), childHandlerFdir(setObjectId) {
+	this->setHkDestination(hkDestination);
+	this->setThermalStateRequestPoolIds(thermalStatePoolId,
+			thermalRequestPoolId);
+
+}
+
+ChildHandlerBase::~ChildHandlerBase() {
+}
+
+ReturnValue_t ChildHandlerBase::initialize() {
+	ReturnValue_t result = DeviceHandlerBase::initialize();
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	MessageQueueId_t parentQueue = 0;
+
+	if (parentId != objects::NO_OBJECT) {
+		SubsystemBase *parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
+		if (parent == NULL) {
+			return RETURN_FAILED;
+		}
+		parentQueue = parent->getCommandQueue();
+
+		parent->registerChild(getObjectId());
+	}
+
+	healthHelper.setParentQueue(parentQueue);
+
+	modeHelper.setParentQueue(parentQueue);
+
+	return RETURN_OK;
+}

src/core/devicehandlers/ChildHandlerFDIR.cpp

@@ -0,0 +1,11 @@
+#include "ChildHandlerFDIR.h"
+
+ChildHandlerFDIR::ChildHandlerFDIR(object_id_t owner,
+        object_id_t faultTreeParent, uint32_t recoveryCount) :
+		DeviceHandlerFailureIsolation(owner, faultTreeParent) {
+	recoveryCounter.setFailureThreshold(recoveryCount);
+}
+
+ChildHandlerFDIR::~ChildHandlerFDIR() {
+}
+

src/core/devicehandlers/DeviceHandlerFailureIsolation.cpp

@@ -0,0 +1,269 @@
+#include "DeviceHandlerFailureIsolation.h"
+
+#include "../devicehandlers/DeviceHandlerIF.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../modes/HasModesIF.h"
+#include "../health/HealthTableIF.h"
+#include "../power/Fuse.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../thermal/ThermalComponentIF.h"
+
+object_id_t DeviceHandlerFailureIsolation::powerConfirmationId =
+        objects::NO_OBJECT;
+
+DeviceHandlerFailureIsolation::DeviceHandlerFailureIsolation(object_id_t owner,
+		object_id_t parent) :
+		FailureIsolationBase(owner, parent),
+		strangeReplyCount(DEFAULT_MAX_STRANGE_REPLIES,
+		        DEFAULT_STRANGE_REPLIES_TIME_MS,
+				parameterDomainBase++),
+		missedReplyCount( DEFAULT_MAX_MISSED_REPLY_COUNT,
+		        DEFAULT_MISSED_REPLY_TIME_MS,
+				parameterDomainBase++),
+		recoveryCounter(DEFAULT_MAX_REBOOT, DEFAULT_REBOOT_TIME_MS,
+		        parameterDomainBase++),
+		fdirState(NONE) {
+}
+
+DeviceHandlerFailureIsolation::~DeviceHandlerFailureIsolation() {
+}
+
+ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event) {
+	if(isFdirInActionOrAreWeFaulty(event)) {
+		return RETURN_OK;
+	}
+	ReturnValue_t result = RETURN_FAILED;
+	switch (event->getEvent()) {
+	case HasModesIF::MODE_TRANSITION_FAILED:
+	case HasModesIF::OBJECT_IN_INVALID_MODE:
+		//We'll try a recovery as long as defined in MAX_REBOOT.
+		//Might cause some AssemblyBase cycles, so keep number low.
+		handleRecovery(event->getEvent());
+		break;
+	case DeviceHandlerIF::DEVICE_INTERPRETING_REPLY_FAILED:
+	case DeviceHandlerIF::DEVICE_READING_REPLY_FAILED:
+	case DeviceHandlerIF::DEVICE_UNREQUESTED_REPLY:
+	case DeviceHandlerIF::DEVICE_UNKNOWN_REPLY: //Some DH's generate generic reply-ids.
+	case DeviceHandlerIF::DEVICE_BUILDING_COMMAND_FAILED:
+		//These faults all mean that there were stupid replies from a device.
+		if (strangeReplyCount.incrementAndCheck()) {
+			handleRecovery(event->getEvent());
+		}
+		break;
+	case DeviceHandlerIF::DEVICE_SENDING_COMMAND_FAILED:
+	case DeviceHandlerIF::DEVICE_REQUESTING_REPLY_FAILED:
+		//The two above should never be confirmed.
+	case DeviceHandlerIF::DEVICE_MISSED_REPLY:
+		result = sendConfirmationRequest(event);
+		if (result == HasReturnvaluesIF::RETURN_OK) {
+			break;
+		}
+		//else
+		if (missedReplyCount.incrementAndCheck()) {
+			handleRecovery(event->getEvent());
+		}
+		break;
+	case StorageManagerIF::GET_DATA_FAILED:
+	case StorageManagerIF::STORE_DATA_FAILED:
+		//Rather strange bugs, occur in RAW mode only. Ignore.
+		break;
+	case DeviceHandlerIF::INVALID_DEVICE_COMMAND:
+		//Ignore, is bad configuration. We can't do anything in flight.
+		break;
+	case HasHealthIF::HEALTH_INFO:
+	case HasModesIF::MODE_INFO:
+	case HasModesIF::CHANGING_MODE:
+		//Do nothing, but mark as handled.
+		break;
+		//****Power*****
+	case PowerSwitchIF::SWITCH_WENT_OFF:
+		if(powerConfirmation != MessageQueueIF::NO_QUEUE) {
+			result = sendConfirmationRequest(event, powerConfirmation);
+			if (result == RETURN_OK) {
+				setFdirState(DEVICE_MIGHT_BE_OFF);
+			}
+		}
+		break;
+	case Fuse::FUSE_WENT_OFF:
+		//Not so good, because PCDU reacted.
+	case Fuse::POWER_ABOVE_HIGH_LIMIT:
+		//Better, because software detected over-current.
+		setFaulty(event->getEvent());
+		break;
+	case Fuse::POWER_BELOW_LOW_LIMIT:
+		//Device might got stuck during boot, retry.
+		handleRecovery(event->getEvent());
+		break;
+		//****Thermal*****
+	case ThermalComponentIF::COMPONENT_TEMP_LOW:
+	case ThermalComponentIF::COMPONENT_TEMP_HIGH:
+	case ThermalComponentIF::COMPONENT_TEMP_OOL_LOW:
+	case ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH:
+		//Well, the device is not really faulty, but it is required to stay off as long as possible.
+		setFaulty(event->getEvent());
+		break;
+	case ThermalComponentIF::TEMP_NOT_IN_OP_RANGE:
+		//Ignore, is information only.
+		break;
+		//*******Default monitoring variables. Are currently not used.*****
+//	case DeviceHandlerIF::MONITORING_LIMIT_EXCEEDED:
+//		setFaulty(event->getEvent());
+//		break;
+//	case DeviceHandlerIF::MONITORING_AMBIGUOUS:
+//		break;
+	default:
+		//We don't know the event, someone else should handle it.
+		return RETURN_FAILED;
+	}
+	return RETURN_OK;
+}
+
+void DeviceHandlerFailureIsolation::eventConfirmed(EventMessage* event) {
+	switch (event->getEvent()) {
+	case DeviceHandlerIF::DEVICE_SENDING_COMMAND_FAILED:
+	case DeviceHandlerIF::DEVICE_REQUESTING_REPLY_FAILED:
+	case DeviceHandlerIF::DEVICE_MISSED_REPLY:
+		if (missedReplyCount.incrementAndCheck()) {
+			handleRecovery(event->getEvent());
+		}
+		break;
+	case PowerSwitchIF::SWITCH_WENT_OFF:
+		//This means the switch went off only for one device.
+		handleRecovery(event->getEvent());
+		break;
+	default:
+		break;
+	}
+}
+
+void DeviceHandlerFailureIsolation::decrementFaultCounters() {
+	strangeReplyCount.checkForDecrement();
+	missedReplyCount.checkForDecrement();
+	recoveryCounter.checkForDecrement();
+}
+
+void DeviceHandlerFailureIsolation::handleRecovery(Event reason) {
+	clearFaultCounters();
+	if (not recoveryCounter.incrementAndCheck()) {
+		startRecovery(reason);
+	} else {
+		setFaulty(reason);
+	}
+}
+
+void DeviceHandlerFailureIsolation::wasParentsFault(EventMessage* event) {
+	//We'll better ignore the SWITCH_WENT_OFF event and await a system-wide reset.
+	//This means, no fault message will come through until a MODE_ or
+	//HEALTH_INFO message comes through -> Is that ok?
+	//Same issue in TxFailureIsolation!
+//	if ((event->getEvent() == PowerSwitchIF::SWITCH_WENT_OFF)
+//			&& (fdirState != RECOVERY_ONGOING)) {
+//		setFdirState(NONE);
+//	}
+}
+
+void DeviceHandlerFailureIsolation::clearFaultCounters() {
+	strangeReplyCount.clear();
+	missedReplyCount.clear();
+}
+
+ReturnValue_t DeviceHandlerFailureIsolation::initialize() {
+	ReturnValue_t result = FailureIsolationBase::initialize();
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "DeviceHandlerFailureIsolation::initialize: Could not"
+				" initialize FailureIsolationBase." << std::endl;
+#endif
+		return result;
+	}
+	ConfirmsFailuresIF* power = ObjectManager::instance()->get<ConfirmsFailuresIF>(
+			powerConfirmationId);
+	if (power != nullptr) {
+		powerConfirmation = power->getEventReceptionQueue();
+	}
+
+	return RETURN_OK;
+}
+
+void DeviceHandlerFailureIsolation::setFdirState(FDIRState state) {
+	FailureIsolationBase::throwFdirEvent(FDIR_CHANGED_STATE, state, fdirState);
+	fdirState = state;
+}
+
+void DeviceHandlerFailureIsolation::triggerEvent(Event event, uint32_t parameter1,
+		uint32_t parameter2) {
+	//Do not throw error events if fdirState != none.
+	//This will still forward MODE and HEALTH INFO events in any case.
+	if (fdirState == NONE || event::getSeverity(event) == severity::INFO) {
+		FailureIsolationBase::triggerEvent(event, parameter1, parameter2);
+	}
+}
+
+bool DeviceHandlerFailureIsolation::isFdirActionInProgress() {
+	return (fdirState != NONE);
+}
+
+void DeviceHandlerFailureIsolation::startRecovery(Event reason) {
+	throwFdirEvent(FDIR_STARTS_RECOVERY, event::getEventId(reason));
+	setOwnerHealth(HasHealthIF::NEEDS_RECOVERY);
+	setFdirState(RECOVERY_ONGOING);
+}
+
+ReturnValue_t DeviceHandlerFailureIsolation::getParameter(uint8_t domainId,
+		uint8_t uniqueId, ParameterWrapper* parameterWrapper,
+		const ParameterWrapper* newValues, uint16_t startAtIndex) {
+	ReturnValue_t result = strangeReplyCount.getParameter(domainId, uniqueId,
+			parameterWrapper, newValues, startAtIndex);
+	if (result != INVALID_DOMAIN_ID) {
+		return result;
+	}
+	result = missedReplyCount.getParameter(domainId, uniqueId, parameterWrapper, newValues,
+	        startAtIndex);
+	if (result != INVALID_DOMAIN_ID) {
+		return result;
+	}
+	result = recoveryCounter.getParameter(domainId, uniqueId, parameterWrapper, newValues,
+	        startAtIndex);
+	if (result != INVALID_DOMAIN_ID) {
+		return result;
+	}
+	return INVALID_DOMAIN_ID;
+}
+
+void DeviceHandlerFailureIsolation::setFaulty(Event reason) {
+	throwFdirEvent(FDIR_TURNS_OFF_DEVICE, event::getEventId(reason));
+	setOwnerHealth(HasHealthIF::FAULTY);
+	setFdirState(AWAIT_SHUTDOWN);
+}
+
+bool DeviceHandlerFailureIsolation::isFdirInActionOrAreWeFaulty(
+		EventMessage* event) {
+	if (fdirState != NONE) {
+		//Only wait for those events, ignore all others.
+		if (event->getParameter1() == HasHealthIF::HEALTHY
+				&& event->getEvent() == HasHealthIF::HEALTH_INFO) {
+			setFdirState(NONE);
+		}
+		if (event->getEvent() == HasModesIF::MODE_INFO
+				&& fdirState != RECOVERY_ONGOING) {
+			setFdirState(NONE);
+		}
+		return true;
+	}
+
+	if (owner == nullptr) {
+	    // Configuration error.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::error << "DeviceHandlerFailureIsolation::"
+	            << "isFdirInActionOrAreWeFaulty: Owner not set!" << std::endl;
+#endif
+	    return false;
+	}
+
+	if (owner->getHealth() == HasHealthIF::FAULTY
+			|| owner->getHealth() == HasHealthIF::PERMANENT_FAULTY) {
+		//Ignore all events in case device is already faulty.
+		return true;
+	}
+	return false;
+}

src/core/devicehandlers/DeviceHandlerMessage.cpp

@@ -0,0 +1,88 @@
+#include "DeviceHandlerMessage.h"
+#include "../objectmanager/ObjectManager.h"
+
+store_address_t DeviceHandlerMessage::getStoreAddress(
+		const CommandMessage* message) {
+	return store_address_t(message->getParameter2());
+}
+
+uint32_t DeviceHandlerMessage::getDeviceCommandId(
+		const CommandMessage* message) {
+	return message->getParameter();
+}
+
+object_id_t DeviceHandlerMessage::getIoBoardObjectId(
+		const CommandMessage* message) {
+	return message->getParameter();
+}
+
+uint8_t DeviceHandlerMessage::getWiretappingMode(
+		const CommandMessage* message) {
+	return message->getParameter();
+}
+
+void DeviceHandlerMessage::setDeviceHandlerRawCommandMessage(
+		CommandMessage* message, store_address_t rawPacketStoreId) {
+	message->setCommand(CMD_RAW);
+	message->setParameter2(rawPacketStoreId.raw);
+}
+
+void DeviceHandlerMessage::setDeviceHandlerWiretappingMessage(
+		CommandMessage* message, uint8_t wiretappingMode) {
+	message->setCommand(CMD_WIRETAPPING);
+	message->setParameter(wiretappingMode);
+}
+
+void DeviceHandlerMessage::setDeviceHandlerSwitchIoBoardMessage(
+		CommandMessage* message, uint32_t ioBoardIdentifier) {
+	message->setCommand(CMD_SWITCH_ADDRESS);
+	message->setParameter(ioBoardIdentifier);
+}
+
+object_id_t DeviceHandlerMessage::getDeviceObjectId(
+		const CommandMessage* message) {
+	return message->getParameter();
+}
+
+void DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(
+		CommandMessage* message, object_id_t deviceObjectid,
+		store_address_t rawPacketStoreId, bool isCommand) {
+	if (isCommand) {
+		message->setCommand(REPLY_RAW_COMMAND);
+	} else {
+		message->setCommand(REPLY_RAW_REPLY);
+	}
+	message->setParameter(deviceObjectid);
+	message->setParameter2(rawPacketStoreId.raw);
+}
+
+void DeviceHandlerMessage::setDeviceHandlerDirectCommandReply(
+		CommandMessage* message, object_id_t deviceObjectid,
+		store_address_t commandParametersStoreId) {
+	message->setCommand(REPLY_DIRECT_COMMAND_DATA);
+	message->setParameter(deviceObjectid);
+	message->setParameter2(commandParametersStoreId.raw);
+}
+
+void DeviceHandlerMessage::clear(CommandMessage* message) {
+	switch (message->getCommand()) {
+	case CMD_RAW:
+	case REPLY_RAW_COMMAND:
+	case REPLY_RAW_REPLY:
+	case REPLY_DIRECT_COMMAND_DATA: {
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
+				objects::IPC_STORE);
+		if (ipcStore != nullptr) {
+			ipcStore->deleteData(getStoreAddress(message));
+		}
+	}
+		/* NO BREAK falls through*/
+	case CMD_SWITCH_ADDRESS:
+	case CMD_WIRETAPPING:
+		message->setCommand(CommandMessage::CMD_NONE);
+		message->setParameter(0);
+		message->setParameter2(0);
+		break;
+	}
+
+}

src/core/devicehandlers/DeviceTmReportingWrapper.cpp

@@ -0,0 +1,45 @@
+#include "DeviceTmReportingWrapper.h"
+#include "../serialize/SerializeAdapter.h"
+
+DeviceTmReportingWrapper::DeviceTmReportingWrapper(object_id_t objectId,
+		ActionId_t actionId, SerializeIF* data) :
+		objectId(objectId), actionId(actionId), data(data) {
+}
+
+DeviceTmReportingWrapper::~DeviceTmReportingWrapper() {
+
+}
+
+ReturnValue_t DeviceTmReportingWrapper::serialize(uint8_t** buffer,
+		size_t* size, size_t maxSize, Endianness streamEndianness) const {
+	ReturnValue_t result = SerializeAdapter::serialize(&objectId,
+			buffer, size, maxSize, streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	result = SerializeAdapter::serialize(&actionId, buffer,
+			size, maxSize, streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	return data->serialize(buffer, size, maxSize, streamEndianness);
+}
+
+size_t DeviceTmReportingWrapper::getSerializedSize() const {
+	return sizeof(objectId) + sizeof(ActionId_t) + data->getSerializedSize();
+}
+
+ReturnValue_t DeviceTmReportingWrapper::deSerialize(const uint8_t** buffer,
+		size_t* size, Endianness streamEndianness) {
+	ReturnValue_t result = SerializeAdapter::deSerialize(&objectId,
+			buffer, size, streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	result = SerializeAdapter::deSerialize(&actionId, buffer,
+			size, streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	return data->deSerialize(buffer, size, streamEndianness);
+}

src/core/devicehandlers/HealthDevice.cpp

@@ -0,0 +1,59 @@
+#include "HealthDevice.h"
+#include "../ipc/QueueFactory.h"
+
+HealthDevice::HealthDevice(object_id_t setObjectId,
+		MessageQueueId_t parentQueue) :
+		SystemObject(setObjectId), lastHealth(HEALTHY), parentQueue(
+				parentQueue), commandQueue(), healthHelper(this, setObjectId) {
+	commandQueue = QueueFactory::instance()->createMessageQueue(3);
+}
+
+HealthDevice::~HealthDevice() {
+	QueueFactory::instance()->deleteMessageQueue(commandQueue);
+}
+
+ReturnValue_t HealthDevice::performOperation(uint8_t opCode) {
+	CommandMessage command;
+	ReturnValue_t result = commandQueue->receiveMessage(&command);
+	if (result == HasReturnvaluesIF::RETURN_OK) {
+		result = healthHelper.handleHealthCommand(&command);
+	}
+	return result;
+}
+
+ReturnValue_t HealthDevice::initialize() {
+	ReturnValue_t result = SystemObject::initialize();
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	if (parentQueue != 0) {
+		return healthHelper.initialize(parentQueue);
+	} else {
+		return healthHelper.initialize();
+	}
+}
+
+MessageQueueId_t HealthDevice::getCommandQueue() const {
+	return commandQueue->getId();
+}
+
+void HealthDevice::setParentQueue(MessageQueueId_t parentQueue) {
+	healthHelper.setParentQueue(parentQueue);
+}
+
+bool HealthDevice::hasHealthChanged() {
+	bool changed;
+	HealthState currentHealth = healthHelper.getHealth();
+	changed = currentHealth != lastHealth;
+	lastHealth = currentHealth;
+	return changed;
+}
+
+ReturnValue_t HealthDevice::setHealth(HealthState health) {
+	healthHelper.setHealth(health);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+HasHealthIF::HealthState HealthDevice::getHealth() {
+	return healthHelper.getHealth();
+}

src/core/events/CMakeLists.txt

@@ -0,0 +1,7 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		EventManager.cpp
+		EventMessage.cpp
+)
+
+add_subdirectory(eventmatching)

src/core/events/EventManager.cpp

@@ -0,0 +1,210 @@
+#include "EventManager.h"
+#include "EventMessage.h"
+
+#include "../ipc/QueueFactory.h"
+#include "../ipc/MutexFactory.h"
+
+MessageQueueId_t EventManagerIF::eventmanagerQueue = MessageQueueIF::NO_QUEUE;
+
+// If one checks registerListener calls, there are around 40 (to max 50)
+// objects registering for certain events.
+// Each listener requires 1 or 2 EventIdMatcher and 1 or 2 ReportRangeMatcher.
+// So a good guess is 75 to a max of 100 pools required for each, which fits well.
+const LocalPool::LocalPoolConfig EventManager::poolConfig = {
+        {fsfwconfig::FSFW_EVENTMGMR_MATCHTREE_NODES,
+        		sizeof(EventMatchTree::Node)},
+        {fsfwconfig::FSFW_EVENTMGMT_EVENTIDMATCHERS,
+        		sizeof(EventIdRangeMatcher)},
+        {fsfwconfig::FSFW_EVENTMGMR_RANGEMATCHERS,
+        		sizeof(ReporterRangeMatcher)}
+};
+
+EventManager::EventManager(object_id_t setObjectId) :
+		SystemObject(setObjectId),
+		factoryBackend(0, poolConfig, false, true) {
+	mutex = MutexFactory::instance()->createMutex();
+	eventReportQueue = QueueFactory::instance()->createMessageQueue(
+			MAX_EVENTS_PER_CYCLE, EventMessage::EVENT_MESSAGE_SIZE);
+}
+
+EventManager::~EventManager() {
+	QueueFactory::instance()->deleteMessageQueue(eventReportQueue);
+	MutexFactory::instance()->deleteMutex(mutex);
+}
+
+MessageQueueId_t EventManager::getEventReportQueue() {
+	return eventReportQueue->getId();
+}
+
+ReturnValue_t EventManager::performOperation(uint8_t opCode) {
+	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+	while (result == HasReturnvaluesIF::RETURN_OK) {
+		EventMessage message;
+		result = eventReportQueue->receiveMessage(&message);
+		if (result == HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_OBJ_EVENT_TRANSLATION == 1
+			printEvent(&message);
+#endif
+			notifyListeners(&message);
+		}
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void EventManager::notifyListeners(EventMessage* message) {
+	lockMutex();
+	for (auto iter = listenerList.begin(); iter != listenerList.end(); ++iter) {
+		if (iter->second.match(message)) {
+			MessageQueueSenderIF::sendMessage(iter->first, message,
+					message->getSender());
+		}
+	}
+	unlockMutex();
+}
+
+ReturnValue_t EventManager::registerListener(MessageQueueId_t listener,
+bool forwardAllButSelected) {
+	auto result = listenerList.insert(
+			std::pair<MessageQueueId_t, EventMatchTree>(listener,
+					EventMatchTree(&factoryBackend, forwardAllButSelected)));
+	if (!result.second) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t EventManager::subscribeToEvent(MessageQueueId_t listener,
+		EventId_t event) {
+	return subscribeToEventRange(listener, event);
+}
+
+ReturnValue_t EventManager::subscribeToAllEventsFrom(MessageQueueId_t listener,
+		object_id_t object) {
+	return subscribeToEventRange(listener, 0, 0, true, object);
+}
+
+ReturnValue_t EventManager::subscribeToEventRange(MessageQueueId_t listener,
+		EventId_t idFrom, EventId_t idTo, bool idInverted,
+		object_id_t reporterFrom, object_id_t reporterTo,
+		bool reporterInverted) {
+	auto iter = listenerList.find(listener);
+	if (iter == listenerList.end()) {
+		return LISTENER_NOT_FOUND;
+	}
+	lockMutex();
+	ReturnValue_t result = iter->second.addMatch(idFrom, idTo, idInverted,
+			reporterFrom, reporterTo, reporterInverted);
+	unlockMutex();
+	return result;
+}
+
+ReturnValue_t EventManager::unsubscribeFromEventRange(MessageQueueId_t listener,
+		EventId_t idFrom, EventId_t idTo, bool idInverted,
+		object_id_t reporterFrom, object_id_t reporterTo,
+		bool reporterInverted) {
+	auto iter = listenerList.find(listener);
+	if (iter == listenerList.end()) {
+		return LISTENER_NOT_FOUND;
+	}
+	lockMutex();
+	ReturnValue_t result = iter->second.removeMatch(idFrom, idTo, idInverted,
+			reporterFrom, reporterTo, reporterInverted);
+	unlockMutex();
+	return result;
+}
+
+void EventManager::lockMutex() {
+	mutex->lockMutex(timeoutType, timeoutMs);
+}
+
+void EventManager::unlockMutex() {
+	mutex->unlockMutex();
+}
+
+void EventManager::setMutexTimeout(MutexIF::TimeoutType timeoutType,
+		uint32_t timeoutMs) {
+	this->timeoutType = timeoutType;
+	this->timeoutMs = timeoutMs;
+}
+
+#if FSFW_OBJ_EVENT_TRANSLATION == 1
+
+void EventManager::printEvent(EventMessage* message) {
+    switch (message->getSeverity()) {
+    case severity::INFO: {
+#if FSFW_DEBUG_INFO == 1
+        printUtility(sif::OutputTypes::OUT_INFO, message);
+#endif /* DEBUG_INFO_EVENT == 1 */
+        break;
+    }
+    default:
+        printUtility(sif::OutputTypes::OUT_DEBUG, message);
+        break;
+    }
+}
+
+void EventManager::printUtility(sif::OutputTypes printType, EventMessage *message) {
+    const char *string = 0;
+    if(printType == sif::OutputTypes::OUT_INFO) {
+        string = translateObject(message->getReporter());
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::info << "EventManager: ";
+        if (string != 0) {
+            sif::info << string;
+        }
+        else {
+            sif::info << "0x" << std::hex << std::setw(8) << std::setfill('0') <<
+                    message->getReporter() << std::setfill(' ') << std::dec;
+        }
+        sif::info << " reported event with ID " << message->getEventId() << std::endl;
+        sif::debug << translateEvents(message->getEvent()) << " | " <<std::hex << "P1 Hex: 0x" <<
+                message->getParameter1() << " | P1 Dec: " << std::dec << message->getParameter1() <<
+                std::hex << " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<
+                std::dec << message->getParameter2() << std::endl;
+#else
+        if (string != 0) {
+            sif::printInfo("Event Manager: %s reported event with ID %d\n", string,
+                    message->getEventId());
+        }
+        else {
+            sif::printInfo("Event Manager: Reporter ID 0x%08x reported event with ID %d\n",
+                    message->getReporter(), message->getEventId());
+        }
+        sif::printInfo("P1 Hex: 0x%x | P1 Dec: %d | P2 Hex: 0x%x | P2 Dec: %d\n",
+                message->getParameter1(), message->getParameter1(),
+                message->getParameter2(), message->getParameter2());
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
+    }
+    else {
+        string = translateObject(message->getReporter());
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::debug << "EventManager: ";
+        if (string != 0) {
+            sif::debug << string;
+        }
+        else {
+            sif::debug << "0x" << std::hex << std::setw(8) << std::setfill('0') <<
+                    message->getReporter() << std::setfill(' ') << std::dec;
+        }
+        sif::debug << " reported event with ID " << message->getEventId() << std::endl;
+        sif::debug << translateEvents(message->getEvent()) << " | " <<std::hex << "P1 Hex: 0x" <<
+                message->getParameter1() << " | P1 Dec: " << std::dec << message->getParameter1() <<
+                std::hex << " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " <<
+                std::dec << message->getParameter2() << std::endl;
+#else
+        if (string != 0) {
+            sif::printDebug("Event Manager: %s reported event with ID %d\n", string,
+                    message->getEventId());
+        }
+        else {
+            sif::printDebug("Event Manager: Reporter ID 0x%08x reported event with ID %d\n",
+                    message->getReporter(), message->getEventId());
+        }
+        sif::printDebug("P1 Hex: 0x%x | P1 Dec: %d | P2 Hex: 0x%x | P2 Dec: %d\n",
+                message->getParameter1(), message->getParameter1(),
+                message->getParameter2(), message->getParameter2());
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */
+    }
+}
+
+#endif /* FSFW_OBJ_EVENT_TRANSLATION == 1 */

src/core/events/EventMessage.cpp

@@ -0,0 +1,114 @@
+#include "EventMessage.h"
+#include <cstring>
+
+EventMessage::EventMessage() {
+	messageSize = EVENT_MESSAGE_SIZE;
+	clearEventMessage();
+}
+
+EventMessage::EventMessage(Event event, object_id_t reporter,
+		uint32_t parameter1, uint32_t parameter2) {
+	messageSize = EVENT_MESSAGE_SIZE;
+	setMessageId(EVENT_MESSAGE);
+	setEvent(event);
+	setReporter(reporter);
+	setParameter1(parameter1);
+	setParameter2(parameter2);
+}
+
+EventMessage::~EventMessage() {
+}
+
+Event EventMessage::getEvent() {
+	Event event;
+	memcpy(&event, getData(), sizeof(Event));
+	return (event & 0xFFFFFF);
+}
+
+void EventMessage::setEvent(Event event) {
+	Event tempEvent;
+	memcpy(&tempEvent, getData(), sizeof(Event));
+	tempEvent = (tempEvent & 0xFF000000) + (event & 0xFFFFFF);
+	memcpy(getData(), &tempEvent, sizeof(Event));
+}
+
+uint8_t EventMessage::getMessageId() {
+	Event event;
+	memcpy(&event, getData(), sizeof(Event));
+	return (event & 0xFF000000) >> 24;
+}
+
+void EventMessage::setMessageId(uint8_t id) {
+	Event event;
+	memcpy(&event, getData(), sizeof(Event));
+	event = (event & 0x00FFFFFF) + (id << 24);
+	memcpy(getData(), &event, sizeof(Event));
+}
+
+EventSeverity_t EventMessage::getSeverity() {
+	Event event;
+	memcpy(&event, getData(), sizeof(Event));
+	return event::getSeverity(event);
+}
+
+void EventMessage::setSeverity(EventSeverity_t severity) {
+	Event event;
+	memcpy(&event, getData(), sizeof(Event));
+	event = (event & 0xFF00FFFF) + (severity << 16);
+	memcpy(getData(), &event, sizeof(Event));
+}
+
+EventId_t EventMessage::getEventId() {
+	Event event;
+	memcpy(&event, getData(), sizeof(Event));
+	return event::getEventId(event);
+}
+
+void EventMessage::setEventId(EventId_t eventId) {
+	Event event;
+	memcpy(&event, getData(), sizeof(Event));
+	event = (event & 0xFFFF0000) + eventId;
+	memcpy(getData(), &event, sizeof(Event));
+}
+
+object_id_t EventMessage::getReporter() {
+	object_id_t parameter;
+	memcpy(&parameter, getData() + sizeof(Event), sizeof(object_id_t));
+	return parameter;
+}
+
+void EventMessage::setReporter(object_id_t reporter) {
+	memcpy(getData() + sizeof(Event), &reporter, sizeof(object_id_t));
+}
+
+uint32_t EventMessage::getParameter1() {
+	uint32_t parameter;
+	memcpy(&parameter, getData() + sizeof(Event) + sizeof(object_id_t), 4);
+	return parameter;
+}
+
+void EventMessage::setParameter1(uint32_t parameter) {
+	memcpy(getData() + sizeof(Event) + sizeof(object_id_t), &parameter, 4);
+}
+
+uint32_t EventMessage::getParameter2() {
+	uint32_t parameter;
+	memcpy(&parameter, getData() + sizeof(Event) + sizeof(object_id_t) + 4, 4);
+	return parameter;
+}
+
+void EventMessage::setParameter2(uint32_t parameter) {
+	memcpy(getData() + sizeof(Event) + sizeof(object_id_t) + 4, &parameter, 4);
+}
+
+void EventMessage::clearEventMessage() {
+	setEvent(INVALID_EVENT);
+}
+
+bool EventMessage::isClearedEventMessage() {
+	return getEvent() == INVALID_EVENT;
+}
+
+size_t EventMessage::getMinimumMessageSize() const {
+	return EVENT_MESSAGE_SIZE;
+}

src/core/fdir/CMakeLists.txt

@@ -0,0 +1,6 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		EventCorrelation.cpp
+		FailureIsolationBase.cpp
+		FaultCounter.cpp
+)

src/core/fdir/EventCorrelation.cpp

@@ -0,0 +1,44 @@
+#include "EventCorrelation.h"
+
+EventCorrelation::EventCorrelation(uint32_t timeout) :
+		eventPending(false) {
+	correlationTimer.setTimeout(timeout);
+}
+
+EventCorrelation::~EventCorrelation() {
+}
+
+EventCorrelation::State EventCorrelation::doesEventCorrelate() {
+	if (correlationTimer.isBusy()) {
+		eventPending = false;
+		return CORRELATED;
+	} else {
+		if (eventPending) {
+			return ALREADY_STARTED;
+		} else {
+			eventPending = true;
+			correlationTimer.resetTimer();
+			return CORRELATION_STARTED;
+		}
+	}
+}
+
+bool EventCorrelation::isEventPending() {
+	if (eventPending) {
+		eventPending = false;
+		return true;
+	} else {
+		correlationTimer.resetTimer();
+		return false;
+	}
+}
+
+bool EventCorrelation::hasPendingEventTimedOut() {
+	if (correlationTimer.hasTimedOut()) {
+		bool temp = eventPending;
+		eventPending = false;
+		return temp;
+	} else {
+		return false;
+	}
+}

src/core/fdir/FailureIsolationBase.cpp

@@ -0,0 +1,172 @@
+#include "../events/EventManagerIF.h"
+#include "FailureIsolationBase.h"
+#include "../health/HasHealthIF.h"
+#include "../health/HealthMessage.h"
+#include "../ipc/QueueFactory.h"
+#include "../objectmanager/ObjectManager.h"
+
+FailureIsolationBase::FailureIsolationBase(object_id_t owner,
+		object_id_t parent, uint8_t messageDepth, uint8_t parameterDomainBase) :
+		ownerId(owner), faultTreeParent(parent),
+		parameterDomainBase(parameterDomainBase) {
+	eventQueue = QueueFactory::instance()->createMessageQueue(messageDepth,
+			EventMessage::EVENT_MESSAGE_SIZE);
+}
+
+FailureIsolationBase::~FailureIsolationBase() {
+	QueueFactory::instance()->deleteMessageQueue(eventQueue);
+}
+
+ReturnValue_t FailureIsolationBase::initialize() {
+	EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(
+			objects::EVENT_MANAGER);
+	if (manager == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "FailureIsolationBase::initialize: Event Manager has not"
+				" been initialized!" << std::endl;
+#endif
+		return RETURN_FAILED;
+	}
+	ReturnValue_t result = manager->registerListener(eventQueue->getId());
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	if (ownerId != objects::NO_OBJECT) {
+		result = manager->subscribeToAllEventsFrom(eventQueue->getId(), ownerId);
+		if (result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+		owner = ObjectManager::instance()->get<HasHealthIF>(ownerId);
+		if (owner == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::error << "FailureIsolationBase::intialize: Owner object "
+					"invalid. Make sure it implements HasHealthIF" << std::endl;
+#endif
+			return ObjectManagerIF::CHILD_INIT_FAILED;
+		}
+	}
+	if (faultTreeParent != objects::NO_OBJECT) {
+		ConfirmsFailuresIF* parentIF = ObjectManager::instance()->get<ConfirmsFailuresIF>(
+				faultTreeParent);
+		if (parentIF == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::error << "FailureIsolationBase::intialize: Parent object"
+					<< "invalid." << std::endl;
+#endif
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::error << "Make sure it implements ConfirmsFailuresIF."
+					<< std::endl;
+#endif
+			return ObjectManagerIF::CHILD_INIT_FAILED;
+			return RETURN_FAILED;
+		}
+		eventQueue->setDefaultDestination(parentIF->getEventReceptionQueue());
+	}
+	return RETURN_OK;
+}
+
+void FailureIsolationBase::checkForFailures() {
+	EventMessage event;
+	for (ReturnValue_t result = eventQueue->receiveMessage(&event);
+			result == RETURN_OK; result = eventQueue->receiveMessage(&event)) {
+		if (event.getSender() == eventQueue->getId()) {
+			//We already got this event, because we sent it.
+			continue;
+		}
+		switch (event.getMessageId()) {
+		case EventMessage::EVENT_MESSAGE:
+			if (isFdirDisabledForSeverity(event.getSeverity())) {
+				//We do not handle events when disabled.
+				continue;
+			}
+			eventReceived(&event);
+			break;
+		case EventMessage::CONFIRMATION_REQUEST:
+			doConfirmFault(&event);
+			break;
+		case EventMessage::YOUR_FAULT:
+			eventConfirmed(&event);
+			break;
+		case EventMessage::MY_FAULT:
+			wasParentsFault(&event);
+			break;
+		default:
+			break;
+		}
+	}
+	decrementFaultCounters();
+}
+
+void FailureIsolationBase::setOwnerHealth(HasHealthIF::HealthState health) {
+	if (owner != NULL) {
+		owner->setHealth(health);
+	}
+	//else: owner has no health.
+
+}
+
+MessageQueueId_t FailureIsolationBase::getEventReceptionQueue() {
+	return eventQueue->getId();
+}
+
+ReturnValue_t FailureIsolationBase::sendConfirmationRequest(EventMessage* event,
+		MessageQueueId_t destination) {
+	event->setMessageId(EventMessage::CONFIRMATION_REQUEST);
+	if (destination != MessageQueueIF::NO_QUEUE) {
+		return eventQueue->sendMessage(destination, event);
+	} else if (faultTreeParent != objects::NO_OBJECT) {
+		return eventQueue->sendToDefault(event);
+	}
+	return RETURN_FAILED;
+}
+
+void FailureIsolationBase::eventConfirmed(EventMessage* event) {
+}
+
+void FailureIsolationBase::wasParentsFault(EventMessage* event) {
+}
+
+void FailureIsolationBase::doConfirmFault(EventMessage* event) {
+	ReturnValue_t result = confirmFault(event);
+	if (result == YOUR_FAULT) {
+		event->setMessageId(EventMessage::YOUR_FAULT);
+		eventQueue->reply(event);
+	} else if (result == MY_FAULT) {
+		event->setMessageId(EventMessage::MY_FAULT);
+		eventQueue->reply(event);
+	} else {
+
+	}
+}
+
+ReturnValue_t FailureIsolationBase::confirmFault(EventMessage* event) {
+	return YOUR_FAULT;
+}
+
+void FailureIsolationBase::triggerEvent(Event event, uint32_t parameter1,
+		uint32_t parameter2) {
+	//With this mechanism, all events are disabled for a certain device.
+	//That's not so good for visibility.
+	if (isFdirDisabledForSeverity(event::getSeverity(event))) {
+		return;
+	}
+	EventMessage message(event, ownerId, parameter1, parameter2);
+	EventManagerIF::triggerEvent(&message, eventQueue->getId());
+	eventReceived(&message);
+}
+
+bool FailureIsolationBase::isFdirDisabledForSeverity(EventSeverity_t severity) {
+	if ((owner != NULL) && (severity != severity::INFO)) {
+		if (owner->getHealth() == HasHealthIF::EXTERNAL_CONTROL) {
+			//External control disables handling of fault messages.
+			return true;
+		}
+	}
+	return false;
+}
+
+void FailureIsolationBase::throwFdirEvent(Event event, uint32_t parameter1,
+		uint32_t parameter2) {
+	EventMessage message(event, ownerId, parameter1, parameter2);
+	EventManagerIF::triggerEvent(&message, eventQueue->getId());
+}

src/core/fdir/FaultCounter.cpp

@@ -0,0 +1,86 @@
+#include "FaultCounter.h"
+
+FaultCounter::FaultCounter(uint32_t failureThreshold, uint32_t decrementAfterMs,
+		uint8_t setParameterDomain) :
+		parameterDomain(setParameterDomain), timer(), faultCount(0), failureThreshold(
+				failureThreshold) {
+	timer.setTimeout(decrementAfterMs);
+}
+
+FaultCounter::~FaultCounter() {
+}
+
+void FaultCounter::increment(uint32_t amount) {
+	if (faultCount == 0) {
+		timer.resetTimer();
+	}
+	faultCount += amount;
+}
+
+bool FaultCounter::checkForDecrement() {
+	if (timer.hasTimedOut()) {
+		timer.resetTimer();
+		if (faultCount > 0) {
+			faultCount--;
+			return true;
+		}
+	}
+	return false;
+}
+
+bool FaultCounter::incrementAndCheck(uint32_t amount) {
+	increment(amount);
+	return aboveThreshold();
+}
+
+bool FaultCounter::aboveThreshold() {
+	if (faultCount > failureThreshold) {
+		faultCount = 0;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+void FaultCounter::clear() {
+	faultCount = 0;
+}
+
+void FaultCounter::setFailureThreshold(uint32_t failureThreshold) {
+	this->failureThreshold = failureThreshold;
+}
+
+void FaultCounter::setFaultDecrementTimeMs(uint32_t timeMs) {
+	timer.setTimeout(timeMs);
+}
+
+FaultCounter::FaultCounter() :
+		parameterDomain(0), timer(), faultCount(0), failureThreshold(0) {
+}
+
+ReturnValue_t FaultCounter::getParameter(uint8_t domainId, uint8_t uniqueId,
+		ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
+		uint16_t startAtIndex) {
+	if (domainId != parameterDomain) {
+		return INVALID_DOMAIN_ID;
+	}
+
+	switch (uniqueId) {
+	case 0:
+		parameterWrapper->set(failureThreshold);
+		break;
+	case 1:
+		parameterWrapper->set(faultCount);
+		break;
+	case 2:
+		parameterWrapper->set(timer.timeout);
+		break;
+	default:
+		return INVALID_IDENTIFIER_ID;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void FaultCounter::setParameterDomain(uint8_t domain) {
+	parameterDomain = domain;
+}

src/core/globalfunctions/AsciiConverter.cpp

@@ -0,0 +1,230 @@
+#include "AsciiConverter.h"
+#include <limits>
+#include <cmath>
+
+template<typename T>
+ReturnValue_t AsciiConverter::scanAsciiDecimalNumber(const uint8_t** dataPtr,
+		uint8_t len, T* value) {
+	if (len > std::numeric_limits<T>().digits10) {
+		return TOO_LONG_FOR_TARGET_TYPE;
+	}
+
+	double temp;
+
+	ReturnValue_t result = scanAsciiDecimalNumber_(dataPtr, len, &temp);
+
+	*value = temp;
+
+	return result;
+}
+
+ReturnValue_t AsciiConverter::scanAsciiHexByte(const uint8_t** dataPtr,
+		uint8_t* value) {
+	int8_t tmp;
+
+	tmp = convertHexChar(*dataPtr);
+	(*dataPtr)++;
+	if (tmp == -1) {
+		return INVALID_CHARACTERS;
+	}
+	if (tmp == -2) {
+		tmp = 0;
+	}
+
+	*value = tmp << 4;
+
+	tmp = convertHexChar(*dataPtr);
+	(*dataPtr)++;
+	if (tmp == -1) {
+		return INVALID_CHARACTERS;
+	}
+	if (tmp != -2) {
+		*value += tmp;
+	} else {
+		*value = *value >> 4;
+	}
+
+	return RETURN_OK;
+}
+
+ReturnValue_t AsciiConverter::scanAsciiDecimalNumber_(uint8_t const ** dataPtr,
+		uint8_t len, double* value) {
+
+	uint8_t const *ptr = *dataPtr;
+	int8_t sign = 1;
+	float decimal = 0;
+	bool abort = false;
+
+	*value = 0;
+
+	//ignore leading space
+	ptr = clearSpace(ptr, len);
+
+	while ((ptr - *dataPtr < len) && !abort) {
+		switch (*ptr) {
+		case '+':
+			sign = 1;
+			break;
+		case '-':
+			sign = -1;
+			break;
+		case '.':
+			decimal = 1;
+			break;
+		case ' ':
+		case 0x0d:
+		case 0x0a:
+			//ignore trailing space
+			ptr = clearSpace(ptr, len - (ptr - *dataPtr)) - 1; //before aborting the loop, ptr will be incremented
+			abort = true;
+			break;
+		default:
+			if ((*ptr < 0x30) || (*ptr > 0x39)) {
+				return INVALID_CHARACTERS;
+			}
+			*value = *value * 10 + (*ptr - 0x30);
+			if (decimal > 0) {
+				decimal *= 10;
+			}
+			break;
+		}
+		ptr++;
+	}
+
+	if (decimal == 0) {
+		decimal = 1;
+	}
+
+	*value = *value / (decimal) * sign;
+
+	*dataPtr = ptr;
+
+	return RETURN_OK;
+}
+
+ReturnValue_t AsciiConverter::printFloat(uint8_t* buffer, uint32_t bufferLength,
+		float value, uint8_t decimalPlaces, uint32_t *printedSize) {
+	*printedSize = 0;
+	uint32_t streamposition = 0, integerSize;
+	bool negative = (value < 0);
+	int32_t digits = bufferLength - decimalPlaces - 1;
+	if (digits <= 0) {
+		return BUFFER_TOO_SMALL;
+	}
+	if (negative) {
+		digits -= 1;
+		buffer[streamposition++] = '-';
+		value = -value;
+	}
+	float maximumNumber = pow(10, digits);
+	if (value >= maximumNumber) {
+		return BUFFER_TOO_SMALL;
+	}
+	//print the numbers before the decimal point;
+	ReturnValue_t result = printInteger(buffer + streamposition,
+			bufferLength - streamposition - decimalPlaces - 1, value,
+			&integerSize);
+	if (result != RETURN_OK) {
+		return result;
+	}
+	streamposition += integerSize;
+	//The decimal Point
+	buffer[streamposition++] = '.';
+
+	//Print the decimals
+	uint32_t integerValue = value;
+	value -= integerValue;
+	value = value * pow(10, decimalPlaces);
+	result = printInteger(buffer + streamposition, decimalPlaces, round(value),
+			&integerSize, true);
+	*printedSize = integerSize + streamposition;
+	return result;
+}
+
+ReturnValue_t AsciiConverter::printInteger(uint8_t* buffer,
+		uint32_t bufferLength, uint32_t value, uint32_t *printedSize,
+		bool leadingZeros) {
+	*printedSize = 0;
+	if (bufferLength == 0) {
+		return BUFFER_TOO_SMALL;
+	}
+	uint32_t maximumNumber = -1;
+	if (bufferLength < 10) {
+		maximumNumber = pow(10, bufferLength);
+		if (value >= maximumNumber) {
+			return BUFFER_TOO_SMALL;
+		}
+		maximumNumber /= 10;
+	} else {
+		if (!(value <= maximumNumber)) {
+			return BUFFER_TOO_SMALL;
+		}
+		maximumNumber = 1000000000;
+	}
+	if (!leadingZeros && (value == 0)) {
+		buffer[(*printedSize)++] = '0';
+		return RETURN_OK;
+	}
+	while (maximumNumber >= 1) {
+		uint8_t number = value / maximumNumber;
+		value = value - (number * maximumNumber);
+		if (!leadingZeros && number == 0) {
+			maximumNumber /= 10;
+		} else {
+			leadingZeros = true;
+			buffer[(*printedSize)++] = '0' + number;
+			maximumNumber /= 10;
+		}
+	}
+	return RETURN_OK;
+}
+
+ReturnValue_t AsciiConverter::printSignedInteger(uint8_t* buffer,
+		uint32_t bufferLength, int32_t value, uint32_t *printedSize) {
+	bool negative = false;
+	if ((bufferLength > 0) && (value < 0)) {
+		*buffer++ = '-';
+		bufferLength--;
+		value = -value;
+		negative = true;
+	}
+	ReturnValue_t result = printInteger(buffer, bufferLength, value,
+			printedSize);
+	if (negative) {
+		(*printedSize)++;
+	}
+	return result;
+}
+
+int8_t AsciiConverter::convertHexChar(const uint8_t* character) {
+	if ((*character > 0x60) && (*character < 0x67)) {
+		return *character - 0x61 + 10;
+	} else if ((*character > 0x40) && (*character < 0x47)) {
+		return *character - 0x41 + 10;
+	} else if ((*character > 0x2F) && (*character < 0x3A)) {
+		return *character - 0x30;
+	} else if (*character == ' ') {
+		return -2;
+	}
+	return -1;
+}
+
+template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber<float>(
+		const uint8_t** dataPtr, uint8_t len, float* value);
+template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber<uint8_t>(
+		const uint8_t** dataPtr, uint8_t len, uint8_t* value);
+template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber<uint16_t>(
+		const uint8_t** dataPtr, uint8_t len, uint16_t* value);
+template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber<double>(
+		const uint8_t** dataPtr, uint8_t len, double* value);
+
+const uint8_t* AsciiConverter::clearSpace(const uint8_t* data, uint8_t len) {
+	while (len > 0) {
+		if ((*data != ' ') && (*data != 0x0a) && (*data != 0x0d)) {
+			return data;
+		}
+		data++;
+		len--;
+	}
+	return data;
+}

src/core/globalfunctions/CMakeLists.txt

@@ -0,0 +1,13 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		arrayprinter.cpp
+		AsciiConverter.cpp
+		CRC.cpp
+		DleEncoder.cpp
+		PeriodicOperationDivider.cpp
+		timevalOperations.cpp
+		Type.cpp
+		bitutility.cpp
+)
+
+add_subdirectory(math)

src/core/globalfunctions/CRC.cpp

@@ -0,0 +1,139 @@
+#include "CRC.h"
+#include <math.h>
+
+const uint16_t CRC::crc16ccitt_table[256] = {
+    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
+    0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
+    0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+    0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
+    0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
+    0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+    0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
+    0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
+    0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+    0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
+    0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
+    0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+    0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
+    0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
+    0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+    0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
+    0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
+    0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+    0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
+    0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
+    0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+    0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+    0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
+    0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+    0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
+    0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
+    0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+    0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
+    0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
+    0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+    0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
+    0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
+};
+
+
+// CRC implementation
+uint16_t CRC::crc16ccitt(uint8_t const input[], uint32_t length, uint16_t startingCrc)
+{
+	uint8_t *data = (uint8_t *)input;
+	unsigned int tbl_idx;
+
+    while (length--) {
+        tbl_idx = ((startingCrc >> 8) ^ *data) & 0xff;
+        startingCrc = (crc16ccitt_table[tbl_idx] ^ (startingCrc << 8)) & 0xffff;
+
+        data++;
+    }
+    return startingCrc & 0xffff;
+
+    //The part below is not used!
+//	bool temr[16];
+//	bool xor_out[16];
+//	bool r[16];
+//	bool d[8];
+//	uint16_t crc_value = 0;
+//
+//
+//	for (int i=0; i<16 ;i++) {
+//		temr[i] = false;
+//		xor_out[i] = false;
+//	}
+//
+//
+//	for (int i=0; i<16 ;i++)
+//			r[i] = true;    // initialize with 0xFFFF
+//
+//
+//
+//	for (int j=0; j<length ;j++)
+//	{
+//
+//	for (int i=0; i<8 ;i++)
+//		if ((input[j] & 1<<i) == 1<<i)
+//			d[7-i]=true;			// reverse   input data
+//		else
+//			d[7-i]=false;			// reverse input data
+//
+//
+//
+//		  temr[0] = d[4] ^ d[0];
+//		  temr[1] = d[5] ^ d[1];
+//		  temr[2] = d[6] ^ d[2];
+//		  temr[3] = d[7] ^ d[3];
+//		  temr[4] = r[12] ^ r[8];
+//		  temr[5] = r[13] ^ r[9];
+//		  temr[6] = r[14] ^ r[10];
+//		  temr[7] = r[15] ^ r[11];
+//		  temr[8] = d[4] ^ r[12];
+//		  temr[9] = d[5] ^ r[13];
+//		  temr[10] = d[6] ^ r[14];
+//		  temr[11] = d[7] ^ r[15];
+//		  temr[12] = temr[0] ^ temr[4];
+//		  temr[13] = temr[1] ^ temr[5];
+//		  temr[14] = temr[2] ^ temr[6];
+//		  temr[15] = temr[3] ^ temr[7];
+//
+//
+//		  xor_out[0] = temr[12];
+//		  xor_out[1] = temr[13];
+//		  xor_out[2] = temr[14];
+//		  xor_out[3] = temr[15];
+//		  xor_out[4] = temr[8];
+//		  xor_out[5] = temr[9] ^ temr[12];
+//		  xor_out[6] = temr[10] ^ temr[13];
+//		  xor_out[7] = temr[11] ^ temr[14];
+//		  xor_out[8] = temr[15] ^ r[0];
+//		  xor_out[9] = temr[8] ^ r[1];
+//		  xor_out[10] = temr[9] ^ r[2];
+//		  xor_out[11] = temr[10] ^ r[3];
+//		  xor_out[12] = temr[11] ^ temr[12] ^ r[4];
+//		  xor_out[13] = temr[13] ^ r[5];
+//		  xor_out[14] = temr[14] ^ r[6];
+//		  xor_out[15] = temr[15] ^ r[7];
+//
+//	 for (int i=0; i<16 ;i++)
+//	 {
+//		  r[i]= xor_out[i] ;
+//	 }
+//
+//	}
+//
+//
+//
+//	for (int i=0; i<16 ;i++)
+//	{
+//		 if  (xor_out[i] == true)
+//			 crc_value = crc_value + pow(2,(15 -i));   // reverse CrC result before Final XOR
+//	}
+//
+//	crc_value = 0;// for debug mode
+//	return (crc_value);
+
+}   /* Calculate_CRC() */
+
+

src/core/globalfunctions/DleEncoder.cpp

@@ -0,0 +1,124 @@
+#include "../globalfunctions/DleEncoder.h"
+
+DleEncoder::DleEncoder() {}
+
+DleEncoder::~DleEncoder() {}
+
+ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
+		size_t sourceLen, uint8_t* destStream, size_t maxDestLen,
+		size_t* encodedLen, bool addStxEtx) {
+	if (maxDestLen < 2) {
+		return STREAM_TOO_SHORT;
+	}
+	size_t encodedIndex = 0, sourceIndex = 0;
+	uint8_t nextByte;
+	if (addStxEtx) {
+		destStream[0] = STX_CHAR;
+		++encodedIndex;
+	}
+
+	while (encodedIndex < maxDestLen and sourceIndex < sourceLen)
+	{
+		nextByte = sourceStream[sourceIndex];
+		// STX, ETX and CR characters in the stream need to be escaped with DLE
+		if (nextByte == STX_CHAR or nextByte == ETX_CHAR or nextByte == CARRIAGE_RETURN) {
+			if (encodedIndex + 1 >= maxDestLen) {
+				return STREAM_TOO_SHORT;
+			}
+			else {
+				destStream[encodedIndex] = DLE_CHAR;
+				++encodedIndex;
+				/* Escaped byte will be actual byte + 0x40. This prevents
+				 * STX, ETX, and carriage return characters from appearing
+				 * in the encoded data stream at all, so when polling an
+				 * encoded stream, the transmission can be stopped at ETX.
+				 * 0x40 was chosen at random with special requirements:
+				 *  - Prevent going from one control char to another
+				 *  - Prevent overflow for common characters */
+				destStream[encodedIndex] = nextByte + 0x40;
+			}
+		}
+		// DLE characters are simply escaped with DLE.
+		else if (nextByte == DLE_CHAR) {
+			if (encodedIndex + 1 >= maxDestLen) {
+			    return STREAM_TOO_SHORT;
+			}
+			else {
+				destStream[encodedIndex] = DLE_CHAR;
+				++encodedIndex;
+				destStream[encodedIndex] = DLE_CHAR;
+			}
+		}
+		else {
+			destStream[encodedIndex] = nextByte;
+		}
+		++encodedIndex;
+		++sourceIndex;
+	}
+
+	if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
+		if (addStxEtx) {
+			destStream[encodedIndex] = ETX_CHAR;
+			++encodedIndex;
+		}
+		*encodedLen = encodedIndex;
+		return RETURN_OK;
+	}
+	else {
+		return STREAM_TOO_SHORT;
+	}
+}
+
+ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
+		size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
+		size_t maxDestStreamlen, size_t *decodedLen) {
+	size_t encodedIndex = 0, decodedIndex = 0;
+	uint8_t nextByte;
+	if (*sourceStream != STX_CHAR) {
+		return DECODING_ERROR;
+	}
+	++encodedIndex;
+
+	while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)
+			&& (sourceStream[encodedIndex] != ETX_CHAR)
+			&& (sourceStream[encodedIndex] != STX_CHAR)) {
+		if (sourceStream[encodedIndex] == DLE_CHAR) {
+			nextByte = sourceStream[encodedIndex + 1];
+			// The next byte is a DLE character that was escaped by another
+			// DLE character, so we can write it to the destination stream.
+			if (nextByte == DLE_CHAR) {
+				destStream[decodedIndex] = nextByte;
+			}
+			else {
+			    /* The next byte is a STX, DTX or 0x0D character which
+			     * was escaped by a DLE character. The actual byte was
+				 * also encoded by adding + 0x40 to prevent having control chars,
+				 * in the stream at all, so we convert it back. */
+				if (nextByte == 0x42 or nextByte == 0x43 or nextByte == 0x4D) {
+					destStream[decodedIndex] = nextByte - 0x40;
+				}
+				else {
+				    return DECODING_ERROR;
+				}
+			}
+			++encodedIndex;
+		}
+		else {
+			destStream[decodedIndex] = sourceStream[encodedIndex];
+		}
+
+		++encodedIndex;
+		++decodedIndex;
+	}
+
+	if (sourceStream[encodedIndex] != ETX_CHAR) {
+		*readLen = ++encodedIndex;
+		return DECODING_ERROR;
+	}
+	else {
+		*readLen = ++encodedIndex;
+		*decodedLen = decodedIndex;
+		return RETURN_OK;
+	}
+}
+

src/core/globalfunctions/PeriodicOperationDivider.cpp

@@ -0,0 +1,44 @@
+#include "PeriodicOperationDivider.h"
+
+
+PeriodicOperationDivider::PeriodicOperationDivider(uint32_t 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 PeriodicOperationDivider::check() {
+	if(counter >= divider) {
+		return true;
+	}
+	return false;
+}
+
+
+
+void PeriodicOperationDivider::resetCounter() {
+	counter = 0;
+}
+
+void PeriodicOperationDivider::setDivider(uint32_t newDivider) {
+	divider = newDivider;
+}
+
+uint32_t PeriodicOperationDivider::getCounter() const {
+	return counter;
+}
+
+uint32_t PeriodicOperationDivider::getDivider() const {
+	return divider;
+}

src/core/globalfunctions/Type.cpp

@@ -0,0 +1,181 @@
+#include "Type.h"
+#include "../serialize/SerializeAdapter.h"
+
+Type::Type() :
+		actualType(UNKNOWN_TYPE) {
+}
+
+Type::Type(ActualType_t actualType) :
+		actualType(actualType) {
+}
+
+Type::Type(const Type& type) :
+		actualType(type.actualType) {
+}
+
+Type& Type::operator =(Type rhs) {
+	this->actualType = rhs.actualType;
+	return *this;
+}
+
+Type& Type::operator =(ActualType_t actualType) {
+	this->actualType = actualType;
+	return *this;
+}
+
+Type::operator Type::ActualType_t() const {
+	return actualType;
+}
+
+bool Type::operator ==(const Type& rhs) {
+	return this->actualType == rhs.actualType;
+}
+
+bool Type::operator !=(const Type& rhs) {
+	return !operator==(rhs);
+}
+
+uint8_t Type::getSize() const {
+	switch (actualType) {
+	case UINT8_T:
+		return sizeof(uint8_t);
+	case INT8_T:
+		return sizeof(int8_t);
+	case UINT16_T:
+		return sizeof(uint16_t);
+	case INT16_T:
+		return sizeof(int16_t);
+	case UINT32_T:
+		return sizeof(uint32_t);
+	case INT32_T:
+		return sizeof(int32_t);
+	case FLOAT:
+		return sizeof(float);
+	case DOUBLE:
+		return sizeof(double);
+	default:
+		return 0;
+	}
+}
+
+ReturnValue_t Type::serialize(uint8_t** buffer, size_t* size,
+		size_t maxSize, Endianness streamEndianness) const {
+	uint8_t ptc;
+	uint8_t pfc;
+	ReturnValue_t result = getPtcPfc(&ptc, &pfc);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	result = SerializeAdapter::serialize(&ptc, buffer, size, maxSize,
+			streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	result = SerializeAdapter::serialize(&pfc, buffer, size, maxSize,
+			streamEndianness);
+
+	return result;
+
+}
+
+size_t Type::getSerializedSize() const {
+	uint8_t dontcare = 0;
+	return 2 * SerializeAdapter::getSerializedSize(&dontcare);
+}
+
+ReturnValue_t Type::deSerialize(const uint8_t** buffer, size_t* size,
+		Endianness streamEndianness) {
+	uint8_t ptc;
+	uint8_t pfc;
+	ReturnValue_t result = SerializeAdapter::deSerialize(&ptc, buffer,
+			size, streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	result = SerializeAdapter::deSerialize(&pfc, buffer, size,
+			streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	actualType = getActualType(ptc, pfc);
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t Type::getPtcPfc(uint8_t* ptc, uint8_t* pfc) const {
+	switch (actualType) {
+	case UINT8_T:
+		*ptc = 3;
+		*pfc = 4;
+		break;
+	case INT8_T:
+		*ptc = 4;
+		*pfc = 4;
+		break;
+	case UINT16_T:
+		*ptc = 3;
+		*pfc = 12;
+		break;
+	case INT16_T:
+		*ptc = 4;
+		*pfc = 12;
+		break;
+	case UINT32_T:
+		*ptc = 3;
+		*pfc = 14;
+		break;
+	case INT32_T:
+		*ptc = 4;
+		*pfc = 14;
+		break;
+	case FLOAT:
+		*ptc = 5;
+		*pfc = 1;
+		break;
+	case DOUBLE:
+		*ptc = 5;
+		*pfc = 2;
+		break;
+	default:
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+Type::ActualType_t Type::getActualType(uint8_t ptc, uint8_t pfc) {
+	switch (ptc) {
+	case 3:
+		switch (pfc) {
+		case 4:
+			return UINT8_T;
+		case 12:
+			return UINT16_T;
+		case 14:
+			return UINT32_T;
+		}
+		break;
+	case 4:
+		switch (pfc) {
+		case 4:
+			return INT8_T;
+		case 12:
+			return INT16_T;
+		case 14:
+			return INT32_T;
+		}
+		break;
+	case 5:
+		switch (pfc) {
+		case 1:
+			return FLOAT;
+		case 2:
+			return DOUBLE;
+		}
+		break;
+	}
+	return UNKNOWN_TYPE;
+}

src/core/globalfunctions/arrayprinter.cpp

@@ -0,0 +1,138 @@
+#include "arrayprinter.h"
+#include "../serviceinterface/ServiceInterface.h"
+#include <bitset>
+
+
+void arrayprinter::print(const uint8_t *data, size_t size, OutputType type,
+        bool printInfo, size_t maxCharPerLine) {
+    if(size == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::info << "Size is zero, nothing to print" << std::endl;
+#else
+        sif::printInfo("Size is zero, nothing to print\n");
+#endif
+        return;
+    }
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    if(printInfo) {
+         sif::info << "Printing data with size " << size << ": " << std::endl;
+    }
+#else
+#if FSFW_NO_C99_IO == 1
+    sif::printInfo("Printing data with size %lu: \n", static_cast<unsigned long>(size));
+#else
+    sif::printInfo("Printing data with size %zu: \n", size);
+#endif /* FSFW_NO_C99_IO == 1 */
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+
+    if(type == OutputType::HEX) {
+        arrayprinter::printHex(data, size, maxCharPerLine);
+    }
+    else if (type == OutputType::DEC) {
+        arrayprinter::printDec(data, size, maxCharPerLine);
+    }
+    else if(type == OutputType::BIN) {
+        arrayprinter::printBin(data, size);
+    }
+}
+
+void arrayprinter::printHex(const uint8_t *data, size_t size,
+        size_t maxCharPerLine) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    if(sif::info.crAdditionEnabled()) {
+        std::cout << "\r" << std::endl;
+    }
+
+    std::cout << "[" << std::hex;
+    for(size_t i = 0; i < size; i++) {
+        std::cout << "0x" << static_cast<int>(data[i]);
+        if(i < size - 1) {
+            std::cout << " , ";
+            if(i > 0 and (i + 1) % maxCharPerLine == 0) {
+                std::cout << std::endl;
+
+            }
+        }
+    }
+    std::cout << std::dec;
+    std::cout << "]" << std::endl;
+#else
+    // General format: 0x01, 0x02, 0x03 so it is number of chars times 6
+    // plus line break plus small safety margin.
+    char printBuffer[(size + 1) * 7 + 1] = {};
+    size_t currentPos = 0;
+    for(size_t i = 0; i < size; i++) {
+        // To avoid buffer overflows.
+        if(sizeof(printBuffer) - currentPos <= 7) {
+            break;
+        }
+
+        currentPos += snprintf(printBuffer + currentPos, 6, "0x%02x", data[i]);
+        if(i < size - 1) {
+            currentPos += sprintf(printBuffer + currentPos, ", ");
+            if(i > 0 and (i + 1) % maxCharPerLine == 0) {
+                currentPos += sprintf(printBuffer + currentPos, "\n");
+            }
+        }
+    }
+#if FSFW_DISABLE_PRINTOUT == 0
+    printf("[%s]\n", printBuffer);
+#endif /* FSFW_DISABLE_PRINTOUT == 0 */
+#endif
+}
+
+void arrayprinter::printDec(const uint8_t *data, size_t size,
+        size_t maxCharPerLine) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    if(sif::info.crAdditionEnabled()) {
+        std::cout << "\r" << std::endl;
+    }
+
+    std::cout << "[" << std::dec;
+    for(size_t i = 0; i < size; i++) {
+        std::cout << static_cast<int>(data[i]);
+        if(i < size - 1){
+            std::cout << " , ";
+            if(i > 0 and (i + 1) % maxCharPerLine == 0) {
+                std::cout << std::endl;
+            }
+        }
+    }
+    std::cout << "]" << std::endl;
+#else
+    // General format: 32, 243, -12 so it is number of chars times 5
+    // plus line break plus small safety margin.
+    char printBuffer[(size + 1) * 5 + 1] = {};
+    size_t currentPos = 0;
+    for(size_t i = 0; i < size; i++) {
+        // To avoid buffer overflows.
+        if(sizeof(printBuffer) - currentPos <= 5) {
+            break;
+        }
+
+        currentPos += snprintf(printBuffer + currentPos, 3, "%d", data[i]);
+        if(i < size - 1) {
+            currentPos += sprintf(printBuffer + currentPos, ", ");
+            if(i > 0 and (i + 1) % maxCharPerLine == 0) {
+                currentPos += sprintf(printBuffer + currentPos, "\n");
+            }
+        }
+    }
+#if FSFW_DISABLE_PRINTOUT == 0
+    printf("[%s]\n", printBuffer);
+#endif /* FSFW_DISABLE_PRINTOUT == 0 */
+#endif
+}
+
+void arrayprinter::printBin(const uint8_t *data, size_t size) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    for(size_t i = 0; i < size; i++) {
+        sif::info << "Byte " << i + 1 << ": 0b" <<  std::bitset<8>(data[i]) << std::endl;
+    }
+#else
+    for(size_t i = 0; i < size; i++) {
+        sif::printInfo("Byte %d: 0b" BYTE_TO_BINARY_PATTERN "\n", i + 1, BYTE_TO_BINARY(data[i]));
+    }
+#endif
+}

src/core/globalfunctions/bitutility.cpp

@@ -0,0 +1,33 @@
+#include "bitutility.h"
+
+void bitutil::bitSet(uint8_t *byte, uint8_t position) {
+    if(position > 7) {
+        return;
+    }
+    uint8_t shiftNumber = position + (7 - 2 * position);
+    *byte |= 1 << shiftNumber;
+}
+
+void bitutil::bitToggle(uint8_t *byte, uint8_t position) {
+    if(position > 7) {
+        return;
+    }
+    uint8_t shiftNumber = position + (7 - 2 * position);
+    *byte ^= 1 << shiftNumber;
+}
+
+void bitutil::bitClear(uint8_t *byte, uint8_t position) {
+    if(position > 7) {
+        return;
+    }
+    uint8_t shiftNumber = position + (7 - 2 * position);
+    *byte &= ~(1 << shiftNumber);
+}
+
+bool bitutil::bitGet(const uint8_t *byte, uint8_t position) {
+    if(position > 7) {
+        return false;
+    }
+    uint8_t shiftNumber = position + (7 - 2 * position);
+    return *byte & (1 << shiftNumber);
+}

src/core/globalfunctions/math/CMakeLists.txt

@@ -0,0 +1,4 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		QuaternionOperations.cpp
+)

src/core/globalfunctions/math/QuaternionOperations.cpp

@@ -0,0 +1,156 @@
+#include "QuaternionOperations.h"
+#include "VectorOperations.h"
+
+#include <cmath>
+#include <cstring>
+#include <stdint.h>
+
+QuaternionOperations::~QuaternionOperations() {
+}
+
+void QuaternionOperations::multiply(const double* q1, const double* q2,
+		double* q) {
+	double out[4];
+
+	out[0] = q1[3] * q2[0] + q1[2] * q2[1] - q1[1] * q2[2] + q1[0] * q2[3];
+	out[1] = -q1[2] * q2[0] + q1[3] * q2[1] + q1[0] * q2[2] + q1[1] * q2[3];
+	out[2] = q1[1] * q2[0] - q1[0] * q2[1] + q1[3] * q2[2] + q1[2] * q2[3];
+	out[3] = -q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] + q1[3] * q2[3];
+
+	memcpy(q, out, 4 * sizeof(*q));
+}
+
+void QuaternionOperations::toDcm(const double* quaternion, double dcm[][3]) {
+	dcm[0][0] = 2
+			* (quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3])
+			- 1;
+	dcm[0][1] = 2
+			* (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
+	dcm[0][2] = 2
+			* (quaternion[0] * quaternion[2] - quaternion[1] * quaternion[3]);
+
+	dcm[1][0] = 2
+			* (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]);
+	dcm[1][1] = 2
+			* (quaternion[1] * quaternion[1] + quaternion[3] * quaternion[3])
+			- 1;
+	dcm[1][2] = 2
+			* (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]);
+
+	dcm[2][0] = 2
+			* (quaternion[0] * quaternion[2] + quaternion[1] * quaternion[3]);
+	dcm[2][1] = 2
+			* (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
+	dcm[2][2] = 2
+			* (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3])
+			- 1;
+}
+
+void QuaternionOperations::inverse(const double* quaternion,
+		double* inverseQuaternion) {
+	memcpy(inverseQuaternion, quaternion, 4 * sizeof(*quaternion));
+	VectorOperations<double>::mulScalar(inverseQuaternion, -1,
+			inverseQuaternion, 3);
+}
+
+QuaternionOperations::QuaternionOperations() {
+
+}
+
+void QuaternionOperations::normalize(const double* quaternion,
+		double* unitQuaternion) {
+	VectorOperations<double>::normalize(quaternion, unitQuaternion, 4);
+}
+
+float QuaternionOperations::norm(const double* quaternion) {
+	return VectorOperations<double>::norm(quaternion, 4);
+}
+
+void QuaternionOperations::fromDcm(const double dcm[][3], double* quaternion,
+		uint8_t *index) {
+
+	double a[4];
+
+	a[0] = 1 + dcm[0][0] - dcm[1][1] - dcm[2][2];
+	a[1] = 1 - dcm[0][0] + dcm[1][1] - dcm[2][2];
+	a[2] = 1 - dcm[0][0] - dcm[1][1] + dcm[2][2];
+	a[3] = 1 + dcm[0][0] + dcm[1][1] + dcm[2][2];
+
+	uint8_t maxAIndex = 0;
+
+	VectorOperations<double>::maxValue(a, 4, &maxAIndex);
+
+	if (index != 0) {
+		*index = maxAIndex;
+	}
+
+	switch (maxAIndex) {
+	case 0:
+		quaternion[0] = 0.5 * sqrt(a[0]);
+		quaternion[1] = (dcm[0][1] + dcm[1][0]) / (2 * sqrt(a[0]));
+		quaternion[2] = (dcm[0][2] + dcm[2][0]) / (2 * sqrt(a[0]));
+		quaternion[3] = (dcm[1][2] - dcm[2][1]) / (2 * sqrt(a[0]));
+		break;
+	case 1:
+		quaternion[0] = (dcm[0][1] + dcm[1][0]) / (2 * sqrt(a[1]));
+		quaternion[1] = 0.5 * sqrt(a[1]);
+		quaternion[2] = (dcm[1][2] + dcm[2][1]) / (2 * sqrt(a[1]));
+		quaternion[3] = (dcm[2][0] - dcm[0][2]) / (2 * sqrt(a[1]));
+		break;
+	case 2:
+		quaternion[0] = (dcm[0][2] + dcm[2][0]) / (2 * sqrt(a[2]));
+		quaternion[1] = (dcm[1][2] + dcm[2][1]) / (2 * sqrt(a[2]));
+		quaternion[2] = 0.5 * sqrt(a[2]);
+		quaternion[3] = (dcm[0][1] - dcm[1][0]) / (2 * sqrt(a[2]));
+		break;
+	case 3:
+		quaternion[0] = (dcm[1][2] - dcm[2][1]) / (2 * sqrt(a[3]));
+		quaternion[1] = (dcm[2][0] - dcm[0][2]) / (2 * sqrt(a[3]));
+		quaternion[2] = (dcm[0][1] - dcm[1][0]) / (2 * sqrt(a[3]));
+		quaternion[3] = 0.5 * sqrt(a[3]);
+		break;
+	}
+
+}
+
+void QuaternionOperations::toDcm(const double* quaternion, float dcm[][3]) {
+	dcm[0][0] = 2
+			* (quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3])
+			- 1;
+	dcm[0][1] = 2
+			* (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
+	dcm[0][2] = 2
+			* (quaternion[0] * quaternion[2] - quaternion[1] * quaternion[3]);
+
+	dcm[1][0] = 2
+			* (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]);
+	dcm[1][1] = 2
+			* (quaternion[1] * quaternion[1] + quaternion[3] * quaternion[3])
+			- 1;
+	dcm[1][2] = 2
+			* (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]);
+
+	dcm[2][0] = 2
+			* (quaternion[0] * quaternion[2] + quaternion[1] * quaternion[3]);
+	dcm[2][1] = 2
+			* (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
+	dcm[2][2] = 2
+			* (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3])
+			- 1;
+}
+
+void QuaternionOperations::normalize(double* quaternion) {
+	normalize(quaternion, quaternion);
+}
+
+double QuaternionOperations::getAngle(const double* quaternion, bool abs) {
+	if (quaternion[3] >= 0) {
+		return 2 * acos(quaternion[3]);
+	} else {
+		if (abs) {
+			return 2 * acos(-quaternion[3]);
+		} else {
+			return -2 * acos(-quaternion[3]);
+		}
+	}
+}

src/core/globalfunctions/timevalOperations.cpp

@@ -0,0 +1,99 @@
+#include "timevalOperations.h"
+
+timeval& operator+=(timeval& lhs, const timeval& rhs) {
+	int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
+	sum += rhs.tv_sec * 1000000. + rhs.tv_usec;
+	lhs.tv_sec = sum / 1000000;
+	lhs.tv_usec = sum - lhs.tv_sec * 1000000;
+	return lhs;
+}
+
+timeval operator+(timeval lhs, const timeval& rhs) {
+	lhs += rhs;
+	return lhs;
+}
+
+timeval& operator-=(timeval& lhs, const timeval& rhs) {
+	int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
+	sum -= rhs.tv_sec * 1000000. + rhs.tv_usec;
+	lhs.tv_sec = sum / 1000000;
+	lhs.tv_usec = sum - lhs.tv_sec * 1000000;
+	return lhs;
+}
+
+timeval operator-(timeval lhs, const timeval& rhs) {
+	lhs -= rhs;
+	return lhs;
+}
+
+double operator/(const timeval& lhs, const timeval& rhs) {
+	double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
+	double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
+	return lhs64 / rhs64;
+}
+
+timeval& operator/=(timeval& lhs, double scalar) {
+	int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec;
+	product /= scalar;
+	lhs.tv_sec = product / 1000000;
+	lhs.tv_usec = product - lhs.tv_sec * 1000000;
+	return lhs;
+}
+
+timeval operator/(timeval lhs, double scalar) {
+	lhs /= scalar;
+	return lhs;
+}
+
+timeval& operator*=(timeval& lhs, double scalar) {
+	int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec;
+	product *= scalar;
+	lhs.tv_sec = product / 1000000;
+	lhs.tv_usec = product - lhs.tv_sec * 1000000;
+	return lhs;
+}
+
+timeval operator*(timeval lhs, double scalar) {
+	lhs *= scalar;
+	return lhs;
+}
+
+timeval operator*(double scalar, timeval rhs) {
+	rhs *= scalar;
+	return rhs;
+}
+
+bool operator==(const timeval& lhs, const timeval& rhs) {
+	int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
+	int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
+	return lhs64 == rhs64;
+}
+bool operator!=(const timeval& lhs, const timeval& rhs) {
+	return !operator==(lhs, rhs);
+}
+bool operator<(const timeval& lhs, const timeval& rhs) {
+	int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
+	int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
+	return lhs64 < rhs64;
+}
+bool operator>(const timeval& lhs, const timeval& rhs) {
+	return operator<(rhs, lhs);
+}
+bool operator<=(const timeval& lhs, const timeval& rhs) {
+	return !operator>(lhs, rhs);
+}
+bool operator>=(const timeval& lhs, const timeval& rhs) {
+	return !operator<(lhs, rhs);
+}
+
+double timevalOperations::toDouble(const timeval timeval) {
+	double result = timeval.tv_sec * 1000000. + timeval.tv_usec;
+	return result / 1000000.;
+}
+
+timeval timevalOperations::toTimeval(const double seconds) {
+	timeval tval;
+	tval.tv_sec = seconds;
+	tval.tv_usec = seconds *(double) 1e6 - (tval.tv_sec *1e6);
+	return tval;
+}

src/core/health/CMakeLists.txt

@@ -0,0 +1,6 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		HealthHelper.cpp
+		HealthMessage.cpp
+		HealthTable.cpp	
+)

src/core/health/HealthHelper.cpp

@@ -0,0 +1,113 @@
+#include "HealthHelper.h"
+#include "../serviceinterface/ServiceInterface.h"
+
+HealthHelper::HealthHelper(HasHealthIF* owner, object_id_t objectId) :
+		objectId(objectId), owner(owner) {
+}
+
+HealthHelper::~HealthHelper() {
+}
+
+ReturnValue_t HealthHelper::handleHealthCommand(CommandMessage* message) {
+	switch (message->getCommand()) {
+	case HealthMessage::HEALTH_SET:
+		handleSetHealthCommand(message);
+		return HasReturnvaluesIF::RETURN_OK;
+	case HealthMessage::HEALTH_ANNOUNCE: {
+		eventSender->forwardEvent(HasHealthIF::HEALTH_INFO, getHealth(),
+				getHealth());
+	}
+		return HasReturnvaluesIF::RETURN_OK;
+	default:
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+}
+
+HasHealthIF::HealthState HealthHelper::getHealth() {
+	return healthTable->getHealth(objectId);
+}
+
+ReturnValue_t HealthHelper::initialize(MessageQueueId_t parentQueue) {
+	setParentQueue(parentQueue);
+	return initialize();
+}
+
+void HealthHelper::setParentQueue(MessageQueueId_t parentQueue) {
+	this->parentQueue = parentQueue;
+}
+
+ReturnValue_t HealthHelper::initialize() {
+	healthTable = ObjectManager::instance()->get<HealthTableIF>(objects::HEALTH_TABLE);
+	eventSender = ObjectManager::instance()->get<EventReportingProxyIF>(objectId);
+
+	if (healthTable == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::error << "HealthHelper::initialize: Health table object needs"
+	            "to be created in factory." << std::endl;
+#endif
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	if(eventSender == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::error << "HealthHelper::initialize: Owner has to implement "
+	            "ReportingProxyIF." << std::endl;
+#endif
+	    return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	ReturnValue_t result = healthTable->registerObject(objectId,
+			HasHealthIF::HEALTHY);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void HealthHelper::setHealth(HasHealthIF::HealthState health) {
+	HasHealthIF::HealthState oldHealth = getHealth();
+	eventSender->forwardEvent(HasHealthIF::HEALTH_INFO, health, oldHealth);
+	if (health != oldHealth) {
+		healthTable->setHealth(objectId, health);
+		informParent(health, oldHealth);
+	}
+}
+
+void HealthHelper::informParent(HasHealthIF::HealthState health,
+		HasHealthIF::HealthState oldHealth) {
+	if (parentQueue == MessageQueueIF::NO_QUEUE) {
+		return;
+	}
+	CommandMessage information;
+	HealthMessage::setHealthMessage(&information, HealthMessage::HEALTH_INFO,
+			health, oldHealth);
+	if (MessageQueueSenderIF::sendMessage(parentQueue, &information,
+	        owner->getCommandQueue()) != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::debug << "HealthHelper::informParent: sending health reply failed."
+				<< std::endl;
+#endif
+	}
+}
+
+void HealthHelper::handleSetHealthCommand(CommandMessage* command) {
+	ReturnValue_t result = owner->setHealth(HealthMessage::getHealth(command));
+	if (command->getSender() == MessageQueueIF::NO_QUEUE) {
+		return;
+	}
+	CommandMessage reply;
+	if (result == HasReturnvaluesIF::RETURN_OK) {
+		HealthMessage::setHealthMessage(&reply,
+				HealthMessage::REPLY_HEALTH_SET);
+	} else {
+		reply.setReplyRejected(result, command->getCommand());
+	}
+	if (MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
+	        owner->getCommandQueue()) != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::debug << "HealthHelper::handleHealthCommand: sending health "
+		        "reply failed." << std::endl;
+#endif
+
+	}
+}

src/core/health/HealthMessage.cpp

@@ -0,0 +1,30 @@
+#include "HealthMessage.h"
+
+void HealthMessage::setHealthMessage(CommandMessage* message, Command_t command,
+		HasHealthIF::HealthState health, HasHealthIF::HealthState oldHealth) {
+	message->setCommand(command);
+	message->setParameter(health);
+	message->setParameter2(oldHealth);
+}
+
+void HealthMessage::setHealthMessage(CommandMessage* message,
+        Command_t command) {
+	message->setCommand(command);
+}
+
+HasHealthIF::HealthState HealthMessage::getHealth(
+        const CommandMessage* message) {
+	return (HasHealthIF::HealthState) message->getParameter();
+}
+
+void HealthMessage::clear(CommandMessage* message) {
+	message->setCommand(CommandMessage::CMD_NONE);
+}
+
+HealthMessage::HealthMessage() {
+}
+
+HasHealthIF::HealthState HealthMessage::getOldHealth(
+		const CommandMessage* message) {
+	return (HasHealthIF::HealthState) message->getParameter2();
+}

src/core/health/HealthTable.cpp

@@ -0,0 +1,110 @@
+#include "HealthTable.h"
+#include "../ipc/MutexGuard.h"
+#include "../ipc/MutexFactory.h"
+#include "../serialize/SerializeAdapter.h"
+
+HealthTable::HealthTable(object_id_t objectid) :
+		SystemObject(objectid) {
+	mutex = MutexFactory::instance()->createMutex();;
+
+	mapIterator = healthMap.begin();
+}
+
+void HealthTable::setMutexTimeout(MutexIF::TimeoutType timeoutType,
+		uint32_t timeoutMs) {
+	this->timeoutType = timeoutType;
+	this->mutexTimeoutMs = timeoutMs;
+}
+
+HealthTable::~HealthTable() {
+	MutexFactory::instance()->deleteMutex(mutex);
+}
+
+ReturnValue_t HealthTable::registerObject(object_id_t object,
+		HasHealthIF::HealthState initilialState) {
+	if (healthMap.count(object) != 0) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	healthMap.emplace(object, initilialState);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void HealthTable::setHealth(object_id_t object,
+		HasHealthIF::HealthState newState) {
+    MutexGuard(mutex, timeoutType, mutexTimeoutMs);
+	HealthMap::iterator iter = healthMap.find(object);
+	if (iter != healthMap.end()) {
+		iter->second = newState;
+	}
+}
+
+HasHealthIF::HealthState HealthTable::getHealth(object_id_t object) {
+	HasHealthIF::HealthState state = HasHealthIF::HEALTHY;
+	MutexGuard(mutex, timeoutType, mutexTimeoutMs);
+	HealthMap::iterator iter = healthMap.find(object);
+	if (iter != healthMap.end()) {
+		state = iter->second;
+	}
+	return state;
+}
+
+bool HealthTable::hasHealth(object_id_t object) {
+	MutexGuard(mutex, timeoutType, mutexTimeoutMs);
+	HealthMap::iterator iter = healthMap.find(object);
+	if (iter != healthMap.end()) {
+		return true;
+	}
+	return false;
+}
+
+size_t HealthTable::getPrintSize() {
+    MutexGuard(mutex, timeoutType, mutexTimeoutMs);
+    uint32_t size = healthMap.size() * sizeof(object_id_t) +
+            sizeof(HasHealthIF::HealthState) + sizeof(uint16_t);
+    return size;
+}
+
+void HealthTable::printAll(uint8_t* pointer, size_t maxSize) {
+    MutexGuard(mutex, timeoutType, mutexTimeoutMs);
+	size_t size = 0;
+	uint16_t count = healthMap.size();
+	ReturnValue_t result = SerializeAdapter::serialize(&count,
+			&pointer, &size, maxSize, SerializeIF::Endianness::BIG);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::warning << "HealthTable::printAll: Serialization of health table failed" << std::endl;
+#else
+	    sif::printWarning("HealthTable::printAll: Serialization of health table failed\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+	    return;
+	}
+	for (const auto& health: healthMap) {
+		result = SerializeAdapter::serialize(&health.first,
+				&pointer, &size, maxSize, SerializeIF::Endianness::BIG);
+		if(result != HasReturnvaluesIF::RETURN_OK) {
+		    return;
+		}
+		uint8_t healthValue = health.second;
+		result = SerializeAdapter::serialize(&healthValue, &pointer, &size,
+				maxSize, SerializeIF::Endianness::BIG);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return;
+        }
+	}
+}
+
+ReturnValue_t HealthTable::iterate(HealthEntry *value, bool reset) {
+	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+	MutexGuard(mutex, timeoutType, mutexTimeoutMs);
+	if (reset) {
+		mapIterator = healthMap.begin();
+	}
+	if (mapIterator == healthMap.end()) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	*value = *mapIterator;
+	mapIterator++;
+	return result;
+}

src/core/housekeeping/HousekeepingMessage.cpp

@@ -0,0 +1,232 @@
+#include "HousekeepingMessage.h"
+
+#include "../objectmanager/ObjectManager.h"
+#include <cstring>
+
+HousekeepingMessage::~HousekeepingMessage() {}
+
+void HousekeepingMessage::setHkReportReply(CommandMessage* message, sid_t sid,
+		store_address_t storeId) {
+	message->setCommand(HK_REPORT);
+	message->setMessageSize(HK_MESSAGE_SIZE);
+	setSid(message, sid);
+	message->setParameter3(storeId.raw);
+}
+
+void HousekeepingMessage::setHkDiagnosticsReply(CommandMessage* message,
+		sid_t sid, store_address_t storeId) {
+	message->setCommand(DIAGNOSTICS_REPORT);
+	message->setMessageSize(HK_MESSAGE_SIZE);
+	setSid(message, sid);
+	message->setParameter3(storeId.raw);
+}
+
+sid_t HousekeepingMessage::getHkDataReply(const CommandMessage *message,
+		store_address_t *storeIdToSet) {
+	if(storeIdToSet != nullptr) {
+	    *storeIdToSet = message->getParameter3();
+	}
+	return getSid(message);
+}
+
+void HousekeepingMessage::setToggleReportingCommand(CommandMessage *message,
+		sid_t sid, bool enableReporting, bool isDiagnostics) {
+	if(isDiagnostics) {
+		if(enableReporting) {
+			message->setCommand(ENABLE_PERIODIC_DIAGNOSTICS_GENERATION);
+		}
+		else {
+			message->setCommand(DISABLE_PERIODIC_DIAGNOSTICS_GENERATION);
+		}
+	}
+	else {
+		if(enableReporting) {
+			message->setCommand(ENABLE_PERIODIC_HK_REPORT_GENERATION);
+		}
+		else {
+			message->setCommand(DISABLE_PERIODIC_HK_REPORT_GENERATION);
+		}
+	}
+
+	setSid(message, sid);
+}
+
+void HousekeepingMessage::setStructureReportingCommand(CommandMessage *command,
+		sid_t sid, bool isDiagnostics) {
+	if(isDiagnostics) {
+		command->setCommand(REPORT_DIAGNOSTICS_REPORT_STRUCTURES);
+	}
+	else {
+		command->setCommand(REPORT_HK_REPORT_STRUCTURES);
+	}
+
+	setSid(command, sid);
+}
+
+void HousekeepingMessage::setOneShotReportCommand(CommandMessage *command,
+		sid_t sid, bool isDiagnostics) {
+	if(isDiagnostics) {
+		command->setCommand(GENERATE_ONE_DIAGNOSTICS_REPORT);
+	}
+	else {
+		command->setCommand(GENERATE_ONE_PARAMETER_REPORT);
+	}
+
+	setSid(command, sid);
+}
+
+void HousekeepingMessage::setCollectionIntervalModificationCommand(
+		CommandMessage *command, sid_t sid, float collectionInterval,
+		bool isDiagnostics) {
+	if(isDiagnostics) {
+		command->setCommand(MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL);
+	}
+	else {
+		command->setCommand(MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL);
+	}
+
+	/* Raw storage of the float in the message. Do not use setParameter3, does
+	implicit conversion to integer type! */
+	std::memcpy(command->getData() + 2 * sizeof(uint32_t), &collectionInterval,
+	        sizeof(collectionInterval));
+
+	setSid(command, sid);
+}
+
+sid_t HousekeepingMessage::getCollectionIntervalModificationCommand(
+		const CommandMessage* command, float* newCollectionInterval) {
+
+	if(newCollectionInterval != nullptr) {
+	    std::memcpy(newCollectionInterval, command->getData() + 2 * sizeof(uint32_t),
+	            sizeof(*newCollectionInterval));
+	}
+
+	return getSid(command);
+}
+
+void HousekeepingMessage::setHkRequestSuccessReply(CommandMessage *reply,
+		sid_t sid) {
+	setSid(reply, sid);
+	reply->setCommand(HK_REQUEST_SUCCESS);
+}
+
+void HousekeepingMessage::setHkRequestFailureReply(CommandMessage *reply,
+		sid_t sid, ReturnValue_t error) {
+	setSid(reply, sid);
+	reply->setCommand(HK_REQUEST_FAILURE);
+	reply->setParameter3(error);
+}
+
+sid_t HousekeepingMessage::getHkRequestFailureReply(const CommandMessage *reply,
+		ReturnValue_t *error) {
+	if(error != nullptr) {
+		*error = reply->getParameter3();
+	}
+	return getSid(reply);
+}
+
+sid_t HousekeepingMessage::getSid(const CommandMessage* message) {
+	sid_t sid;
+	std::memcpy(&sid.raw, message->getData(), sizeof(sid.raw));
+	return sid;
+}
+
+gp_id_t HousekeepingMessage::getGpid(const CommandMessage* message) {
+    gp_id_t globalPoolId;
+    std::memcpy(&globalPoolId.raw, message->getData(), sizeof(globalPoolId.raw));
+    return globalPoolId;
+}
+
+void HousekeepingMessage::setHkStuctureReportReply(CommandMessage *reply,
+        sid_t sid, store_address_t storeId) {
+    reply->setCommand(HK_DEFINITIONS_REPORT);
+    setSid(reply, sid);
+    reply->setParameter3(storeId.raw);
+}
+
+void HousekeepingMessage::setDiagnosticsStuctureReportReply(
+        CommandMessage *reply, sid_t sid, store_address_t storeId) {
+    reply->setCommand(DIAGNOSTICS_DEFINITION_REPORT);
+    setSid(reply, sid);
+    reply->setParameter3(storeId.raw);
+}
+
+void HousekeepingMessage::clear(CommandMessage* message) {
+    switch(message->getCommand()) {
+    case(HK_REPORT):
+    case(DIAGNOSTICS_REPORT):
+    case(HK_DEFINITIONS_REPORT):
+    case(DIAGNOSTICS_DEFINITION_REPORT):
+    case(UPDATE_SNAPSHOT_SET):
+    case(UPDATE_SNAPSHOT_VARIABLE): {
+        store_address_t storeId;
+        getHkDataReply(message, &storeId);
+        StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
+                objects::IPC_STORE);
+        if (ipcStore != nullptr) {
+            ipcStore->deleteData(storeId);
+        }
+    }
+    }
+    message->setCommand(CommandMessage::CMD_NONE);
+}
+
+void HousekeepingMessage::setUpdateNotificationSetCommand(
+        CommandMessage *command, sid_t sid) {
+    command->setCommand(UPDATE_NOTIFICATION_SET);
+    setSid(command, sid);
+}
+
+void HousekeepingMessage::setUpdateNotificationVariableCommand(
+        CommandMessage *command, gp_id_t globalPoolId) {
+    command->setCommand(UPDATE_NOTIFICATION_VARIABLE);
+    setGpid(command, globalPoolId);
+}
+
+void HousekeepingMessage::setUpdateSnapshotSetCommand(CommandMessage *command,
+        sid_t sid, store_address_t storeId) {
+    command->setCommand(UPDATE_SNAPSHOT_SET);
+    setSid(command, sid);
+    command->setParameter3(storeId.raw);
+}
+
+void HousekeepingMessage::setUpdateSnapshotVariableCommand(
+        CommandMessage *command, gp_id_t globalPoolId, store_address_t storeId) {
+    command->setCommand(UPDATE_SNAPSHOT_VARIABLE);
+    setGpid(command, globalPoolId);
+    command->setParameter3(storeId.raw);
+}
+
+sid_t HousekeepingMessage::getUpdateNotificationSetCommand(
+        const CommandMessage *command) {
+    return getSid(command);
+}
+
+gp_id_t HousekeepingMessage::getUpdateNotificationVariableCommand(
+        const CommandMessage *command) {
+    return getGpid(command);
+}
+
+sid_t HousekeepingMessage::getUpdateSnapshotSetCommand(
+        const CommandMessage *command, store_address_t *storeId) {
+    if(storeId != nullptr) {
+        *storeId = command->getParameter3();
+    }
+    return getSid(command);
+}
+
+gp_id_t HousekeepingMessage::getUpdateSnapshotVariableCommand(
+        const CommandMessage *command, store_address_t *storeId) {
+    if(storeId != nullptr) {
+        *storeId = command->getParameter3();
+    }
+    return getGpid(command);
+}
+
+void HousekeepingMessage::setSid(CommandMessage *message, sid_t sid) {
+    std::memcpy(message->getData(), &sid.raw, sizeof(sid.raw));
+}
+
+void HousekeepingMessage::setGpid(CommandMessage *message, gp_id_t globalPoolId) {
+    std::memcpy(message->getData(), &globalPoolId.raw, sizeof(globalPoolId.raw));
+}

src/core/housekeeping/PeriodicHousekeepingHelper.cpp

@@ -0,0 +1,90 @@
+#include "PeriodicHousekeepingHelper.h"
+
+#include "../datapoollocal/LocalPoolDataSetBase.h"
+#include <cmath>
+
+PeriodicHousekeepingHelper::PeriodicHousekeepingHelper(
+        LocalPoolDataSetBase* owner): owner(owner) {}
+
+void PeriodicHousekeepingHelper::initialize(float collectionInterval,
+        dur_millis_t minimumPeriodicInterval, uint8_t nonDiagIntervalFactor) {
+    this->minimumPeriodicInterval = minimumPeriodicInterval;
+    this->nonDiagIntervalFactor = nonDiagIntervalFactor;
+    collectionIntervalTicks = intervalSecondsToIntervalTicks(collectionInterval);
+    /* This will cause a checkOpNecessary call to be true immediately. I think it's okay
+	if a HK packet is generated immediately instead of waiting one generation cycle. */
+    internalTickCounter = collectionIntervalTicks;
+}
+
+float PeriodicHousekeepingHelper::getCollectionIntervalInSeconds() const {
+    return intervalTicksToSeconds(collectionIntervalTicks);
+}
+
+bool PeriodicHousekeepingHelper::checkOpNecessary() {
+    if(internalTickCounter >= collectionIntervalTicks) {
+        internalTickCounter = 1;
+        return true;
+    }
+    internalTickCounter++;
+    return false;
+}
+
+uint32_t PeriodicHousekeepingHelper::intervalSecondsToIntervalTicks(
+        float collectionIntervalSeconds) {
+    if(owner == nullptr) {
+        return 0;
+    }
+    bool isDiagnostics = owner->isDiagnostics();
+
+    /* Avoid division by zero */
+    if(minimumPeriodicInterval == 0) {
+        if(isDiagnostics) {
+            /* Perform operation each cycle */
+            return 1;
+        }
+        else {
+            return nonDiagIntervalFactor;
+        }
+    }
+    else {
+        dur_millis_t intervalInMs = collectionIntervalSeconds * 1000;
+        uint32_t divisor = minimumPeriodicInterval;
+        if(not isDiagnostics) {
+            /* We need to multiply the divisor because non-diagnostics only
+            allow a multiple of the minimum periodic interval */
+            divisor *= nonDiagIntervalFactor;
+        }
+        uint32_t ticks = std::ceil(static_cast<float>(intervalInMs) / divisor);
+        if(not isDiagnostics) {
+            /* Now we need to multiply the calculated ticks with the factor as as well
+            because the minimum tick count to generate a non-diagnostic is the factor itself.
+
+            Example calculation for non-diagnostic with
+            0.4 second interval and 0.2 second task interval.
+            Resultant tick count of 5 is equal to operation each second.
+
+            Examle calculation for non-diagnostic with  2.0 second interval and 0.2 second
+            task interval.
+            Resultant tick count of 10 is equal to operatin every 2 seconds.
+
+            Example calculation for diagnostic with 0.4 second interval and 0.3
+            second task interval. Resulting tick count of 2 is equal to operation
+            every 0.6 seconds. */
+            ticks *= nonDiagIntervalFactor;
+        }
+        return ticks;
+    }
+}
+
+float PeriodicHousekeepingHelper::intervalTicksToSeconds(
+        uint32_t collectionInterval) const {
+    /* Number of ticks times the minimum interval is in milliseconds, so we divide by 1000 to get
+    the value in seconds */
+    return static_cast<float>(collectionInterval * minimumPeriodicInterval / 1000.0);
+}
+
+void PeriodicHousekeepingHelper::changeCollectionInterval(
+        float newIntervalSeconds) {
+    collectionIntervalTicks = intervalSecondsToIntervalTicks(newIntervalSeconds);
+}
+

src/core/internalError/CMakeLists.txt

@@ -0,0 +1,4 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		InternalErrorReporter.cpp
+)

src/core/internalError/InternalErrorReporter.cpp

@@ -0,0 +1,202 @@
+#include "InternalErrorReporter.h"
+
+#include "../ipc/QueueFactory.h"
+#include "../ipc/MutexFactory.h"
+#include "../serviceinterface/ServiceInterface.h"
+#include "../datapool/PoolReadGuard.h"
+
+InternalErrorReporter::InternalErrorReporter(object_id_t setObjectId,
+        uint32_t messageQueueDepth): SystemObject(setObjectId),
+        commandQueue(QueueFactory::instance()->
+                createMessageQueue(messageQueueDepth)),
+                poolManager(this, commandQueue),
+                internalErrorSid(setObjectId, InternalErrorDataset::ERROR_SET_ID),
+                internalErrorDataset(this) {
+    mutex = MutexFactory::instance()->createMutex();
+}
+
+InternalErrorReporter::~InternalErrorReporter() {
+    MutexFactory::instance()->deleteMutex(mutex);
+}
+
+void InternalErrorReporter::setDiagnosticPrintout(bool enable) {
+    this->diagnosticPrintout = enable;
+}
+
+ReturnValue_t InternalErrorReporter::performOperation(uint8_t opCode) {
+    CommandMessage message;
+    ReturnValue_t result = commandQueue->receiveMessage(&message);
+    if(result != MessageQueueIF::EMPTY) {
+        poolManager.handleHousekeepingMessage(&message);
+    }
+
+    uint32_t newQueueHits = getAndResetQueueHits();
+    uint32_t newTmHits = getAndResetTmHits();
+    uint32_t newStoreHits = getAndResetStoreHits();
+
+#if FSFW_VERBOSE_LEVEL >= 1
+    if(diagnosticPrintout) {
+        if((newQueueHits > 0) or (newTmHits > 0) or (newStoreHits > 0)) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::debug << "InternalErrorReporter::performOperation: Errors "
+                    << "occured!" << std::endl;
+            sif::debug << "Queue errors: " << newQueueHits << std::endl;
+            sif::debug << "TM errors: " << newTmHits << std::endl;
+            sif::debug << "Store errors: " << newStoreHits << std::endl;
+#else
+            sif::printDebug("InternalErrorReporter::performOperation: Errors occured!\n");
+            sif::printDebug("Queue errors: %lu\n", static_cast<unsigned int>(newQueueHits));
+            sif::printDebug("TM errors: %lu\n", static_cast<unsigned int>(newTmHits));
+            sif::printDebug("Store errors: %lu\n", static_cast<unsigned int>(newStoreHits));
+#endif
+        }
+    }
+#endif
+
+    {
+        PoolReadGuard readGuard(&internalErrorDataset);
+        if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
+            internalErrorDataset.queueHits.value += newQueueHits;
+            internalErrorDataset.storeHits.value += newStoreHits;
+            internalErrorDataset.tmHits.value += newTmHits;
+            internalErrorDataset.setValidity(true, true);
+        }
+    }
+
+    poolManager.performHkOperation();
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void InternalErrorReporter::queueMessageNotSent() {
+    incrementQueueHits();
+}
+
+void InternalErrorReporter::lostTm() {
+    incrementTmHits();
+}
+
+uint32_t InternalErrorReporter::getAndResetQueueHits() {
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = queueHits;
+    queueHits = 0;
+    mutex->unlockMutex();
+    return value;
+}
+
+uint32_t InternalErrorReporter::getQueueHits() {
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = queueHits;
+    mutex->unlockMutex();
+    return value;
+}
+
+void InternalErrorReporter::incrementQueueHits() {
+    mutex->lockMutex(timeoutType, timeoutMs);
+    queueHits++;
+    mutex->unlockMutex();
+}
+
+uint32_t InternalErrorReporter::getAndResetTmHits() {
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = tmHits;
+    tmHits = 0;
+    mutex->unlockMutex();
+    return value;
+}
+
+uint32_t InternalErrorReporter::getTmHits() {
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = tmHits;
+    mutex->unlockMutex();
+    return value;
+}
+
+void InternalErrorReporter::incrementTmHits() {
+    mutex->lockMutex(timeoutType, timeoutMs);
+    tmHits++;
+    mutex->unlockMutex();
+}
+
+void InternalErrorReporter::storeFull() {
+    incrementStoreHits();
+}
+
+uint32_t InternalErrorReporter::getAndResetStoreHits() {
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = storeHits;
+    storeHits = 0;
+    mutex->unlockMutex();
+    return value;
+}
+
+uint32_t InternalErrorReporter::getStoreHits() {
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = storeHits;
+    mutex->unlockMutex();
+    return value;
+}
+
+void InternalErrorReporter::incrementStoreHits() {
+    mutex->lockMutex(timeoutType, timeoutMs);
+    storeHits++;
+    mutex->unlockMutex();
+}
+
+object_id_t InternalErrorReporter::getObjectId() const {
+    return SystemObject::getObjectId();
+}
+
+MessageQueueId_t InternalErrorReporter::getCommandQueue() const {
+    return this->commandQueue->getId();
+}
+
+ReturnValue_t InternalErrorReporter::initializeLocalDataPool(
+        localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
+    localDataPoolMap.emplace(errorPoolIds::TM_HITS, new PoolEntry<uint32_t>());
+    localDataPoolMap.emplace(errorPoolIds::QUEUE_HITS, new PoolEntry<uint32_t>());
+    localDataPoolMap.emplace(errorPoolIds::STORE_HITS, new PoolEntry<uint32_t>());
+    poolManager.subscribeForPeriodicPacket(internalErrorSid, false, getPeriodicOperationFrequency(),
+            true);
+    internalErrorDataset.setValidity(true, true);
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+dur_millis_t InternalErrorReporter::getPeriodicOperationFrequency() const {
+    return this->executingTask->getPeriodMs();
+}
+
+LocalPoolDataSetBase* InternalErrorReporter::getDataSetHandle(sid_t sid) {
+    return &internalErrorDataset;
+}
+
+void InternalErrorReporter::setTaskIF(PeriodicTaskIF *task) {
+    this->executingTask = task;
+}
+
+ReturnValue_t InternalErrorReporter::initialize() {
+    ReturnValue_t result = poolManager.initialize(commandQueue);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+    return SystemObject::initialize();
+}
+
+ReturnValue_t InternalErrorReporter::initializeAfterTaskCreation() {
+    return poolManager.initializeAfterTaskCreation();
+}
+
+void InternalErrorReporter::setMutexTimeout(MutexIF::TimeoutType timeoutType,
+        uint32_t timeoutMs) {
+    this->timeoutType = timeoutType;
+    this->timeoutMs = timeoutMs;
+}
+
+LocalDataPoolManager* InternalErrorReporter::getHkManagerHandle() {
+    return &poolManager;
+}

src/core/ipc/CMakeLists.txt

@@ -0,0 +1,6 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		CommandMessage.cpp
+		CommandMessageCleaner.cpp
+		MessageQueueMessage.cpp
+)

src/core/ipc/CommandMessage.cpp

@@ -0,0 +1,111 @@
+#include "CommandMessage.h"
+#include "CommandMessageCleaner.h"
+#include <cstring>
+
+CommandMessage::CommandMessage() {
+    MessageQueueMessage::setMessageSize(DEFAULT_COMMAND_MESSAGE_SIZE);
+    setCommand(CMD_NONE);
+}
+
+CommandMessage::CommandMessage(Command_t command, uint32_t parameter1,
+        uint32_t parameter2) {
+    MessageQueueMessage::setMessageSize(DEFAULT_COMMAND_MESSAGE_SIZE);
+    setCommand(command);
+    setParameter(parameter1);
+    setParameter2(parameter2);
+}
+
+Command_t CommandMessage::getCommand() const {
+    Command_t command;
+    std::memcpy(&command, MessageQueueMessage::getData(), sizeof(Command_t));
+    return command;
+}
+
+void CommandMessage::setCommand(Command_t command) {
+    std::memcpy(MessageQueueMessage::getData(), &command, sizeof(Command_t));
+}
+
+uint8_t CommandMessage::getMessageType() const {
+    // first byte of command ID.
+    return getCommand() >> 8 & 0xff;
+}
+
+uint32_t CommandMessage::getParameter() const {
+    uint32_t parameter1;
+    std::memcpy(&parameter1, this->getData(), sizeof(parameter1));
+    return parameter1;
+}
+
+void CommandMessage::setParameter(uint32_t parameter1) {
+    std::memcpy(this->getData(), &parameter1, sizeof(parameter1));
+}
+
+uint32_t CommandMessage::getParameter2() const {
+    uint32_t parameter2;
+    std::memcpy(&parameter2, this->getData() + sizeof(uint32_t),
+            sizeof(parameter2));
+    return parameter2;
+}
+
+void CommandMessage::setParameter2(uint32_t parameter2) {
+    std::memcpy(this->getData()  + sizeof(uint32_t), &parameter2,
+            sizeof(parameter2));
+}
+
+uint32_t CommandMessage::getParameter3() const {
+    uint32_t parameter3;
+    std::memcpy(&parameter3, this->getData() + 2 * sizeof(uint32_t),
+            sizeof(parameter3));
+    return parameter3;
+}
+
+void CommandMessage::setParameter3(uint32_t parameter3) {
+    std::memcpy(this->getData()  + 2 * sizeof(uint32_t), &parameter3,
+            sizeof(parameter3));
+}
+
+size_t CommandMessage::getMinimumMessageSize() const {
+    return MINIMUM_COMMAND_MESSAGE_SIZE;
+}
+
+void CommandMessage::clearCommandMessage() {
+    clear();
+}
+
+void CommandMessage::clear() {
+    CommandMessageCleaner::clearCommandMessage(this);
+}
+
+bool CommandMessage::isClearedCommandMessage() {
+    return getCommand() == CMD_NONE;
+}
+
+void CommandMessage::setToUnknownCommand() {
+    Command_t initialCommand = getCommand();
+    this->clear();
+    setReplyRejected(UNKNOWN_COMMAND, initialCommand);
+}
+
+void CommandMessage::setReplyRejected(ReturnValue_t reason,
+        Command_t initialCommand) {
+	setCommand(REPLY_REJECTED);
+    setParameter(reason);
+    setParameter2(initialCommand);
+}
+
+ReturnValue_t CommandMessage::getReplyRejectedReason(
+        Command_t *initialCommand) const {
+    ReturnValue_t reason = getParameter();
+    if(initialCommand != nullptr) {
+        *initialCommand = getParameter2();
+    }
+    return reason;
+}
+
+uint8_t* CommandMessage::getData() {
+    return MessageQueueMessage::getData() + sizeof(Command_t);
+}
+
+const uint8_t* CommandMessage::getData() const {
+    return MessageQueueMessage::getData() + sizeof(Command_t);
+}

src/core/ipc/CommandMessageCleaner.cpp

@@ -0,0 +1,53 @@
+#include "CommandMessageCleaner.h"
+
+#include "../memory/GenericFileSystemMessage.h"
+#include "../devicehandlers/DeviceHandlerMessage.h"
+#include "../health/HealthMessage.h"
+#include "../memory/MemoryMessage.h"
+#include "../modes/ModeMessage.h"
+#include "../monitoring/MonitoringMessage.h"
+#include "../subsystem/modes/ModeSequenceMessage.h"
+#include "../tmstorage/TmStoreMessage.h"
+#include "../housekeeping/HousekeepingMessage.h"
+#include "../parameters/ParameterMessage.h"
+
+void CommandMessageCleaner::clearCommandMessage(CommandMessage* message) {
+	switch(message->getMessageType()){
+	case messagetypes::MODE_COMMAND:
+		ModeMessage::clear(message);
+		break;
+	case messagetypes::HEALTH_COMMAND:
+		HealthMessage::clear(message);
+		break;
+	case messagetypes::MODE_SEQUENCE:
+		ModeSequenceMessage::clear(message);
+		break;
+	case messagetypes::ACTION:
+		ActionMessage::clear(message);
+		break;
+	case messagetypes::DEVICE_HANDLER_COMMAND:
+		DeviceHandlerMessage::clear(message);
+		break;
+	case messagetypes::MEMORY:
+		MemoryMessage::clear(message);
+		break;
+	case messagetypes::MONITORING:
+		MonitoringMessage::clear(message);
+		break;
+	case messagetypes::TM_STORE:
+		TmStoreMessage::clear(message);
+		break;
+	case messagetypes::PARAMETER:
+		ParameterMessage::clear(message);
+		break;
+	case messagetypes::HOUSEKEEPING:
+	    HousekeepingMessage::clear(message);
+	    break;
+	case messagetypes::FILE_SYSTEM_MESSAGE:
+	    GenericFileSystemMessage::clear(message);
+	    break;
+	default:
+		messagetypes::clearMissionMessage(message);
+		break;
+	}
+}

src/core/ipc/MessageQueueMessage.cpp

@@ -0,0 +1,92 @@
+#include "MessageQueueMessage.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../globalfunctions/arrayprinter.h"
+#include <cstring>
+
+MessageQueueMessage::MessageQueueMessage() :
+		messageSize(getMinimumMessageSize()) {
+	memset(this->internalBuffer, 0, sizeof(this->internalBuffer));
+}
+
+MessageQueueMessage::MessageQueueMessage(uint8_t* data, size_t size) :
+		messageSize(this->HEADER_SIZE + size) {
+	if (size <= this->MAX_DATA_SIZE) {
+		memcpy(this->getData(), data, size);
+		this->messageSize = this->HEADER_SIZE + size;
+	}
+	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "MessageQueueMessage: Passed size larger than maximum"
+				"allowed size! Setting content to 0" << std::endl;
+#endif
+		memset(this->internalBuffer, 0, sizeof(this->internalBuffer));
+		this->messageSize = this->HEADER_SIZE;
+	}
+}
+
+MessageQueueMessage::~MessageQueueMessage() {
+}
+
+const uint8_t* MessageQueueMessage::getBuffer() const {
+	return this->internalBuffer;
+}
+
+uint8_t* MessageQueueMessage::getBuffer() {
+	return this->internalBuffer;
+}
+
+const uint8_t* MessageQueueMessage::getData() const {
+	return this->internalBuffer + this->HEADER_SIZE;
+}
+
+uint8_t* MessageQueueMessage::getData() {
+	return this->internalBuffer + this->HEADER_SIZE;
+}
+
+MessageQueueId_t MessageQueueMessage::getSender() const {
+	MessageQueueId_t temp_id;
+	memcpy(&temp_id, this->internalBuffer, sizeof(MessageQueueId_t));
+	return temp_id;
+}
+
+void MessageQueueMessage::setSender(MessageQueueId_t setId) {
+	memcpy(this->internalBuffer, &setId, sizeof(MessageQueueId_t));
+}
+
+void MessageQueueMessage::print(bool printWholeMessage) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	sif::debug << "MessageQueueMessage content: " << std::endl;
+#endif
+	if(printWholeMessage) {
+	    arrayprinter::print(getData(), getMaximumMessageSize());
+	}
+	else {
+	    arrayprinter::print(getData(), getMessageSize());
+	}
+
+}
+
+void MessageQueueMessage::clear() {
+	memset(this->getBuffer(), 0, this->MAX_MESSAGE_SIZE);
+}
+
+size_t MessageQueueMessage::getMessageSize() const {
+	return this->messageSize;
+}
+
+void MessageQueueMessage::setMessageSize(size_t messageSize) {
+	this->messageSize = messageSize;
+}
+
+size_t MessageQueueMessage::getMinimumMessageSize() const {
+    return this->MIN_MESSAGE_SIZE;
+}
+
+size_t MessageQueueMessage::getMaximumMessageSize() const {
+    return this->MAX_MESSAGE_SIZE;
+}
+
+size_t MessageQueueMessage::getMaximumDataSize() const {
+    return this->MAX_DATA_SIZE;
+}
+

src/core/modes/CMakeLists.txt

@@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		ModeHelper.cpp
+		ModeMessage.cpp
+)

src/core/modes/ModeHelper.cpp

@@ -0,0 +1,137 @@
+#include "HasModesIF.h"
+#include "ModeHelper.h"
+
+#include "../ipc/MessageQueueSenderIF.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+ModeHelper::ModeHelper(HasModesIF *owner) :
+		commandedMode(HasModesIF::MODE_OFF),
+		commandedSubmode(HasModesIF::SUBMODE_NONE),
+		owner(owner), forced(false) {}
+
+ModeHelper::~ModeHelper() {
+
+}
+
+ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* command) {
+	CommandMessage reply;
+	Mode_t mode;
+	Submode_t submode;
+	switch (command->getCommand()) {
+	case ModeMessage::CMD_MODE_COMMAND_FORCED:
+		forced = true;
+		/* NO BREAK falls through*/
+	case ModeMessage::CMD_MODE_COMMAND: {
+		mode = ModeMessage::getMode(command);
+		submode = ModeMessage::getSubmode(command);
+		uint32_t timeout;
+		ReturnValue_t result = owner->checkModeCommand(mode, submode, &timeout);
+		if (result != HasReturnvaluesIF::RETURN_OK) {
+			ModeMessage::setCantReachMode(&reply, result);
+			MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
+			        owner->getCommandQueue());
+			break;
+		}
+		//Free to start transition
+		theOneWhoCommandedAMode = command->getSender();
+		commandedMode = mode;
+		commandedSubmode = submode;
+
+		if ((parentQueueId != MessageQueueIF::NO_QUEUE)
+				&& (theOneWhoCommandedAMode != parentQueueId)) {
+			owner->setToExternalControl();
+		}
+
+		countdown.setTimeout(timeout);
+		owner->startTransition(mode, submode);
+	}
+		break;
+	case ModeMessage::CMD_MODE_READ: {
+		owner->getMode(&mode, &submode);
+		ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY, mode,
+				submode);
+		MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
+		       owner->getCommandQueue());
+	}
+		break;
+	case ModeMessage::CMD_MODE_ANNOUNCE:
+		owner->announceMode(false);
+		break;
+	case ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY:
+		owner->announceMode(true);
+		break;
+	default:
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t ModeHelper::initialize(MessageQueueId_t parentQueueId) {
+	setParentQueue(parentQueueId);
+	return initialize();
+}
+
+void ModeHelper::modeChanged(Mode_t ownerMode, Submode_t ownerSubmode) {
+	forced = false;
+	sendModeReplyMessage(ownerMode, ownerSubmode);
+	sendModeInfoMessage(ownerMode, ownerSubmode);
+	theOneWhoCommandedAMode = MessageQueueIF::NO_QUEUE;
+}
+
+void ModeHelper::sendModeReplyMessage(Mode_t ownerMode,
+		Submode_t ownerSubmode) {
+	CommandMessage reply;
+	if (theOneWhoCommandedAMode != MessageQueueIF::NO_QUEUE)
+	{
+		if (ownerMode != commandedMode or ownerSubmode != commandedSubmode)
+		{
+			ModeMessage::setModeMessage(&reply,
+					ModeMessage::REPLY_WRONG_MODE_REPLY, ownerMode,
+					ownerSubmode);
+		}
+		else
+		{
+			ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY,
+					ownerMode, ownerSubmode);
+		}
+		MessageQueueSenderIF::sendMessage(theOneWhoCommandedAMode, &reply,
+		        owner->getCommandQueue());
+	}
+}
+
+void ModeHelper::sendModeInfoMessage(Mode_t ownerMode,
+		Submode_t ownerSubmode) {
+	CommandMessage reply;
+	if (theOneWhoCommandedAMode != parentQueueId
+			and parentQueueId != MessageQueueIF::NO_QUEUE)
+	{
+		ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_INFO,
+				ownerMode, ownerSubmode);
+		MessageQueueSenderIF::sendMessage(parentQueueId, &reply,
+		        owner->getCommandQueue());
+	}
+}
+
+void ModeHelper::startTimer(uint32_t timeoutMs) {
+	countdown.setTimeout(timeoutMs);
+}
+
+void ModeHelper::setParentQueue(MessageQueueId_t parentQueueId) {
+	this->parentQueueId = parentQueueId;
+}
+
+ReturnValue_t ModeHelper::initialize(void) {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+bool ModeHelper::isTimedOut() {
+	return countdown.hasTimedOut();
+}
+
+bool ModeHelper::isForced() {
+	return forced;
+}
+
+void ModeHelper::setForced(bool forced) {
+	this->forced = forced;
+}

src/core/modes/ModeMessage.cpp

@@ -0,0 +1,31 @@
+#include "ModeMessage.h"
+
+Mode_t ModeMessage::getMode(const CommandMessage* message) {
+	return message->getParameter();
+}
+
+Submode_t ModeMessage::getSubmode(const CommandMessage* message) {
+	return message->getParameter2();
+}
+
+void ModeMessage::setModeMessage(CommandMessage* message,
+        Command_t command, Mode_t mode, Submode_t submode) {
+	message->setCommand( command );
+	message->setParameter( mode );
+	message->setParameter2( submode );
+}
+
+ReturnValue_t ModeMessage::getCantReachModeReason(const CommandMessage* message) {
+	return message->getParameter();
+}
+
+void ModeMessage::clear(CommandMessage* message) {
+	message->setCommand(CommandMessage::CMD_NONE);
+}
+
+void ModeMessage::setCantReachMode(CommandMessage* message,
+        ReturnValue_t reason) {
+	message->setCommand(REPLY_CANT_REACH_MODE);
+	message->setParameter(reason);
+	message->setParameter2(0);
+}

src/core/objectmanager/CMakeLists.txt

@@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		ObjectManager.cpp
+		SystemObject.cpp
+)

src/core/objectmanager/ObjectManager.cpp

@@ -0,0 +1,145 @@
+#include "ObjectManager.h"
+#include "../serviceinterface/ServiceInterface.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+#include <iomanip>
+#endif
+#include <cstdlib>
+
+ObjectManager* ObjectManager::objManagerInstance = nullptr;
+
+ObjectManager* ObjectManager::instance() {
+    if(objManagerInstance == nullptr) {
+        objManagerInstance = new ObjectManager();
+    }
+    return objManagerInstance;
+}
+
+void ObjectManager::setObjectFactoryFunction(produce_function_t objFactoryFunc, void *factoryArgs) {
+    this->objectFactoryFunction = objFactoryFunc;
+    this->factoryArgs = factoryArgs;
+}
+
+
+ObjectManager::ObjectManager() {}
+
+
+ObjectManager::~ObjectManager() {
+	for (auto const& iter : objectList) {
+		delete iter.second;
+	}
+}
+
+ReturnValue_t ObjectManager::insert( object_id_t id, SystemObjectIF* object) {
+	auto returnPair = objectList.emplace(id, object);
+	if (returnPair.second) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    // sif::debug << "ObjectManager::insert: Object " << std::hex
+	    //            << (int)id << std::dec << " inserted." << std::endl;
+#endif
+		return this->RETURN_OK;
+	} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "ObjectManager::insert: Object ID " << std::hex <<
+		        static_cast<uint32_t>(id) << std::dec << " is already in use!" << std::endl;
+		sif::error << "Terminating program" << std::endl;
+#else
+		sif::printError("ObjectManager::insert: Object ID 0x%08x is already in use!\n",
+		        static_cast<unsigned int>(id));
+		sif::printError("Terminating program");
+#endif
+		//This is very severe and difficult to handle in other places.
+		std::exit(INSERTION_FAILED);
+	}
+}
+
+ReturnValue_t ObjectManager::remove( object_id_t id ) {
+	if ( this->getSystemObject(id) != NULL ) {
+		this->objectList.erase( id );
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		//sif::debug << "ObjectManager::removeObject: Object " << std::hex
+		//           << (int)id << std::dec << " removed." << std::endl;
+#endif
+		return RETURN_OK;
+	} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "ObjectManager::removeObject: Requested object "
+		        << std::hex << (int)id << std::dec << " not found." << std::endl;
+#endif
+		return NOT_FOUND;
+	}
+}
+
+
+
+SystemObjectIF* ObjectManager::getSystemObject( object_id_t id ) {
+	auto listIter = this->objectList.find( id );
+	if (listIter == this->objectList.end() ) {
+		return nullptr;
+	} else {
+		return listIter->second;
+	}
+}
+
+void ObjectManager::initialize() {
+	if(objectFactoryFunction == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "ObjectManager::initialize: Passed produceObjects "
+				"functions is nullptr!" << std::endl;
+#else
+		sif::printError("ObjectManager::initialize: Passed produceObjects functions is nullptr!\n");
+#endif
+		return;
+	}
+	objectFactoryFunction(factoryArgs);
+	ReturnValue_t result = RETURN_FAILED;
+	uint32_t errorCount = 0;
+	for (auto const& it : objectList) {
+		result = it.second->initialize();
+		if ( result != RETURN_OK ) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			object_id_t var = it.first;
+			sif::error << "ObjectManager::initialize: Object 0x" << std::hex <<
+					std::setw(8) << std::setfill('0')<< var << " failed to "
+					"initialize with code 0x" << result << std::dec <<
+					std::setfill(' ') << std::endl;
+#endif
+			errorCount++;
+		}
+	}
+	if (errorCount > 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "ObjectManager::ObjectManager: Counted " << errorCount
+		           << " failed initializations." << std::endl;
+#endif
+	}
+	//Init was successful. Now check successful interconnections.
+	errorCount = 0;
+	for (auto const& it : objectList) {
+		result = it.second->checkObjectConnections();
+		if ( result != RETURN_OK ) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::error << "ObjectManager::ObjectManager: Object 0x" << std::hex <<
+					(int) it.first  << " connection check failed with code 0x" << result <<
+					std::dec << std::endl;
+#endif
+			errorCount++;
+		}
+	}
+	if (errorCount > 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "ObjectManager::ObjectManager: Counted " << errorCount
+		           << " failed connection checks." << std::endl;
+#endif
+	}
+}
+
+void ObjectManager::printList() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	sif::debug << "ObjectManager: Object List contains:" << std::endl;
+	for (auto const& it : objectList) {
+		sif::debug << std::hex << it.first << " | " << it.second << std::endl;
+
+	}
+#endif
+}

src/core/objectmanager/SystemObject.cpp

@@ -0,0 +1,37 @@
+#include "ObjectManager.h"
+#include "SystemObject.h"
+#include "../events/EventManagerIF.h"
+
+SystemObject::SystemObject(object_id_t setObjectId, bool doRegister) :
+		objectId(setObjectId), registered(doRegister) {
+    if (registered) {
+        ObjectManager::instance()->insert(objectId, this);
+    }
+}
+
+SystemObject::~SystemObject() {
+	if (registered) {
+	    ObjectManager::instance()->remove(objectId);
+	}
+}
+
+object_id_t SystemObject::getObjectId() const {
+	return objectId;
+}
+
+void SystemObject::triggerEvent(Event event, uint32_t parameter1,
+		uint32_t parameter2) {
+	EventManagerIF::triggerEvent(objectId, event, parameter1, parameter2);
+}
+
+ReturnValue_t SystemObject::initialize() {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t SystemObject::checkObjectConnections() {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void SystemObject::forwardEvent(Event event, uint32_t parameter1, uint32_t parameter2) const {
+	EventManagerIF::triggerEvent(objectId, event, parameter1, parameter2);
+}

src/core/parameters/CMakeLists.txt

@@ -0,0 +1,6 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		ParameterHelper.cpp
+		ParameterMessage.cpp
+		ParameterWrapper.cpp
+)

src/core/parameters/ParameterHelper.cpp

@@ -0,0 +1,140 @@
+#include "ParameterHelper.h"
+#include "ParameterMessage.h"
+
+#include "../objectmanager/ObjectManager.h"
+
+ParameterHelper::ParameterHelper(ReceivesParameterMessagesIF* owner):
+        owner(owner) {}
+
+ParameterHelper::~ParameterHelper() {
+}
+
+ReturnValue_t ParameterHelper::handleParameterMessage(CommandMessage *message) {
+    if(storage == nullptr) {
+        // ParameterHelper was not initialized
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+    switch (message->getCommand()) {
+    case ParameterMessage::CMD_PARAMETER_DUMP: {
+        ParameterWrapper description;
+        uint8_t domain = HasParametersIF::getDomain(
+                ParameterMessage::getParameterId(message));
+        uint8_t uniqueIdentifier = HasParametersIF::getUniqueIdentifierId(
+                ParameterMessage::getParameterId(message));
+        result = owner->getParameter(domain, uniqueIdentifier,
+                &description, &description, 0);
+        if (result == HasReturnvaluesIF::RETURN_OK) {
+            result = sendParameter(message->getSender(),
+                    ParameterMessage::getParameterId(message), &description);
+        }
+    }
+    break;
+    case ParameterMessage::CMD_PARAMETER_LOAD: {
+        ParameterId_t parameterId = 0;
+        uint8_t ptc = 0;
+        uint8_t pfc = 0;
+        uint8_t rows = 0;
+        uint8_t columns = 0;
+        store_address_t storeId = ParameterMessage::getParameterLoadCommand(
+                message, &parameterId, &ptc, &pfc, &rows, &columns);
+        Type type(Type::getActualType(ptc, pfc));
+
+        uint8_t domain = HasParametersIF::getDomain(parameterId);
+        uint8_t uniqueIdentifier = HasParametersIF::getUniqueIdentifierId(
+                parameterId);
+        uint16_t linearIndex = HasParametersIF::getIndex(parameterId);
+
+        ConstStorageAccessor accessor(storeId);
+        result = storage->getData(storeId, accessor);
+        if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "ParameterHelper::handleParameterMessage: Getting"
+                    << " store data failed for load command." << std::endl;
+#endif
+            break;
+        }
+
+        ParameterWrapper streamWrapper;
+        result = streamWrapper.set(type, rows, columns, accessor.data(),
+                accessor.size());
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+
+        ParameterWrapper ownerWrapper;
+        result = owner->getParameter(domain, uniqueIdentifier, &ownerWrapper,
+                &streamWrapper, linearIndex);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+
+        result = ownerWrapper.copyFrom(&streamWrapper, linearIndex);
+        if (result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+
+        result = sendParameter(message->getSender(),
+                ParameterMessage::getParameterId(message), &ownerWrapper);
+        break;
+    }
+    default:
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        rejectCommand(message->getSender(), result, message->getCommand());
+    }
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t ParameterHelper::sendParameter(MessageQueueId_t to, uint32_t id,
+        const ParameterWrapper* description) {
+    size_t serializedSize = description->getSerializedSize();
+
+    uint8_t *storeElement = nullptr;
+    store_address_t address;
+
+    ReturnValue_t result = storage->getFreeElement(&address, serializedSize,
+            &storeElement);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    size_t storeElementSize = 0;
+
+    result = description->serialize(&storeElement, &storeElementSize,
+            serializedSize, SerializeIF::Endianness::BIG);
+
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        storage->deleteData(address);
+        return result;
+    }
+
+    CommandMessage reply;
+
+    ParameterMessage::setParameterDumpReply(&reply, id, address);
+
+    MessageQueueSenderIF::sendMessage(to, &reply, ownerQueueId);
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t ParameterHelper::initialize() {
+    ownerQueueId = owner->getCommandQueue();
+
+    storage = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+    if (storage == nullptr) {
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void ParameterHelper::rejectCommand(MessageQueueId_t to, ReturnValue_t reason,
+        Command_t initialCommand) {
+    CommandMessage reply;
+    reply.setReplyRejected(reason, initialCommand);
+    MessageQueueSenderIF::sendMessage(to, &reply, ownerQueueId);
+}

src/core/parameters/ParameterMessage.cpp

@@ -0,0 +1,65 @@
+#include "ParameterMessage.h"
+
+#include "../objectmanager/ObjectManager.h"
+
+ParameterId_t ParameterMessage::getParameterId(const CommandMessage* message) {
+	return message->getParameter();
+}
+
+store_address_t ParameterMessage::getStoreId(const CommandMessage* message) {
+	store_address_t address;
+	address.raw = message->getParameter2();
+	return address;
+}
+
+void ParameterMessage::setParameterDumpCommand(CommandMessage* message,
+		ParameterId_t id) {
+	message->setCommand(CMD_PARAMETER_DUMP);
+	message->setParameter(id);
+}
+
+void ParameterMessage::setParameterDumpReply(CommandMessage* message,
+		ParameterId_t id, store_address_t storageID) {
+	message->setCommand(REPLY_PARAMETER_DUMP);
+	message->setParameter(id);
+	message->setParameter2(storageID.raw);
+}
+
+void ParameterMessage::setParameterLoadCommand(CommandMessage* message,
+		ParameterId_t id, store_address_t storeId, uint8_t ptc, uint8_t pfc,
+		uint8_t rows = 1, uint8_t columns = 1) {
+	message->setCommand(CMD_PARAMETER_LOAD);
+	message->setParameter(id);
+	message->setParameter2(storeId.raw);
+	uint32_t packedParameterSettings = (ptc << 24) | (pfc << 16) |
+	        (rows << 8) | columns;
+	message->setParameter3(packedParameterSettings);
+}
+
+store_address_t ParameterMessage::getParameterLoadCommand(
+        const CommandMessage *message, ParameterId_t* parameterId, uint8_t *ptc,
+        uint8_t *pfc, uint8_t *rows, uint8_t *columns) {
+    *parameterId = message->getParameter();
+    uint32_t packedParamSettings = message->getParameter3();
+    *ptc = packedParamSettings >> 24 & 0xff;
+    *pfc = packedParamSettings >> 16 & 0xff;
+    *rows = packedParamSettings >> 8 & 0xff;
+    *columns = packedParamSettings & 0xff;
+    return message->getParameter2();
+}
+
+void ParameterMessage::clear(CommandMessage* message) {
+	switch (message->getCommand()) {
+	case CMD_PARAMETER_LOAD:
+	case REPLY_PARAMETER_DUMP: {
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
+				objects::IPC_STORE);
+		if (ipcStore != NULL) {
+			ipcStore->deleteData(getStoreId(message));
+		}
+		break;
+	}
+	default:
+		break;
+	}
+}

src/core/parameters/ParameterWrapper.cpp

@@ -0,0 +1,351 @@
+#include "ParameterWrapper.h"
+#include <FSFWConfig.h>
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+ParameterWrapper::ParameterWrapper() :
+		pointsToStream(false), type(Type::UNKNOWN_TYPE) {
+}
+
+ParameterWrapper::ParameterWrapper(Type type, uint8_t rows, uint8_t columns,
+		void *data):
+		pointsToStream(false), type(type), rows(rows), columns(columns),
+		data(data), readonlyData(data) {
+}
+
+ParameterWrapper::ParameterWrapper(Type type, uint8_t rows, uint8_t columns,
+		const void *data):
+		pointsToStream(false), type(type), rows(rows), columns(columns),
+		data(nullptr), readonlyData(data) {
+}
+
+ParameterWrapper::~ParameterWrapper() {
+}
+
+ReturnValue_t ParameterWrapper::serialize(uint8_t **buffer, size_t *size,
+		size_t maxSize, Endianness streamEndianness) const {
+	ReturnValue_t result;
+
+	result = SerializeAdapter::serialize(&type, buffer, size, maxSize,
+			streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	result = SerializeAdapter::serialize(&columns, buffer, size, maxSize,
+			streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	result = SerializeAdapter::serialize(&rows, buffer, size, maxSize,
+			streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	/* serialize uses readonlyData, as it is always valid */
+	if (readonlyData == nullptr) {
+		return NOT_SET;
+	}
+	switch (type) {
+	case Type::UINT8_T:
+		result = serializeData<uint8_t>(buffer, size, maxSize,
+				streamEndianness);
+		break;
+	case Type::INT8_T:
+		result = serializeData<int8_t>(buffer, size, maxSize, streamEndianness);
+		break;
+	case Type::UINT16_T:
+		result = serializeData<uint16_t>(buffer, size, maxSize,
+				streamEndianness);
+		break;
+	case Type::INT16_T:
+		result = serializeData<int16_t>(buffer, size, maxSize,
+				streamEndianness);
+		break;
+	case Type::UINT32_T:
+		result = serializeData<uint32_t>(buffer, size, maxSize,
+				streamEndianness);
+		break;
+	case Type::INT32_T:
+		result = serializeData<int32_t>(buffer, size, maxSize,
+				streamEndianness);
+		break;
+	case Type::FLOAT:
+		result = serializeData<float>(buffer, size, maxSize, streamEndianness);
+		break;
+	case Type::DOUBLE:
+		result = serializeData<double>(buffer, size, maxSize, streamEndianness);
+		break;
+	default:
+		result = UNKNOWN_DATATYPE;
+		break;
+	}
+	return result;
+}
+
+size_t ParameterWrapper::getSerializedSize() const {
+	uint32_t serializedSize = 0;
+	serializedSize += type.getSerializedSize();
+	serializedSize += sizeof(rows);
+	serializedSize += sizeof(columns);
+	serializedSize += rows * columns * type.getSize();
+
+	return serializedSize;
+}
+
+template<typename T>
+ReturnValue_t ParameterWrapper::serializeData(uint8_t **buffer, size_t *size,
+		size_t maxSize, Endianness streamEndianness) const {
+	const T *element = (const T*) readonlyData;
+	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+	uint16_t dataSize = columns * rows;
+	while (dataSize != 0) {
+		result = SerializeAdapter::serialize(element, buffer, size, maxSize,
+				streamEndianness);
+		if (result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+		element++;
+		dataSize--;
+	}
+	return result;
+}
+
+template<typename T>
+ReturnValue_t ParameterWrapper::deSerializeData(uint8_t startingRow,
+		uint8_t startingColumn, const void *from, uint8_t fromRows,
+		uint8_t fromColumns) {
+
+	//treat from as a continuous Stream as we copy all of it
+	const uint8_t *fromAsStream = reinterpret_cast<const uint8_t*>(from);
+	size_t streamSize = fromRows * fromColumns * sizeof(T);
+
+	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+
+	for (uint8_t fromRow = 0; fromRow < fromRows; fromRow++) {
+
+		//get the start element of this row in data
+	    uint16_t offset = (((startingRow + fromRow) *
+	    		static_cast<uint16_t>(columns)) + startingColumn);
+		T *dataWithDataType = static_cast<T*>(data) + offset;
+
+		for (uint8_t fromColumn = 0; fromColumn < fromColumns; fromColumn++) {
+			result = SerializeAdapter::deSerialize(
+					dataWithDataType + fromColumn, &fromAsStream, &streamSize,
+					SerializeIF::Endianness::BIG);
+			if (result != HasReturnvaluesIF::RETURN_OK) {
+				return result;
+			}
+		}
+	}
+
+	return result;
+
+}
+
+
+ReturnValue_t ParameterWrapper::deSerialize(const uint8_t **buffer,
+		size_t *size, Endianness streamEndianness) {
+	return deSerialize(buffer, size, streamEndianness, 0);
+}
+
+ReturnValue_t ParameterWrapper::deSerialize(const uint8_t **buffer,
+		size_t *size, Endianness streamEndianness,
+		uint16_t startWritingAtIndex) {
+	ParameterWrapper streamDescription;
+
+	ReturnValue_t result = streamDescription.set(*buffer, *size, buffer, size);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	return copyFrom(&streamDescription, startWritingAtIndex);
+}
+
+ReturnValue_t ParameterWrapper::set(Type type, uint8_t rows, uint8_t columns,
+        const void *data, size_t dataSize) {
+    this->type = type;
+    this->rows = rows;
+    this->columns = columns;
+
+    size_t expectedSize = type.getSize() * rows * columns;
+    if (expectedSize < dataSize) {
+        return SerializeIF::STREAM_TOO_SHORT;
+    }
+
+    this->data = nullptr;
+    this->readonlyData = data;
+    pointsToStream = true;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t ParameterWrapper::set(const uint8_t *stream, size_t streamSize,
+		const uint8_t **remainingStream, size_t *remainingSize) {
+	ReturnValue_t result = SerializeAdapter::deSerialize(&type, &stream,
+			&streamSize, SerializeIF::Endianness::BIG);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	result = SerializeAdapter::deSerialize(&columns, &stream, &streamSize,
+			SerializeIF::Endianness::BIG);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	result = SerializeAdapter::deSerialize(&rows, &stream, &streamSize,
+			SerializeIF::Endianness::BIG);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+
+	size_t dataSize = type.getSize() * rows * columns;
+
+	if (streamSize < dataSize) {
+		return SerializeIF::STREAM_TOO_SHORT;
+	}
+
+	data = nullptr;
+	readonlyData = stream;
+	pointsToStream = true;
+
+	stream += dataSize;
+	if (remainingStream !=  nullptr) {
+		*remainingStream = stream;
+	}
+	streamSize -= dataSize;
+	if (remainingSize != nullptr) {
+		*remainingSize = streamSize;
+	}
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
+		uint16_t startWritingAtIndex) {
+	if (data == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::warning << "ParameterWrapper::copyFrom: Called on read-only variable!" << std::endl;
+#else
+	    sif::printWarning("ParameterWrapper::copyFrom: Called on read-only variable!\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+		return READONLY;
+	}
+
+	if (from->readonlyData == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "ParameterWrapper::copyFrom: Source not set!" << std::endl;
+#else
+        sif::printWarning("ParameterWrapper::copyFrom: Source not set!\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+		return SOURCE_NOT_SET;
+	}
+
+	if (type != from->type) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "ParameterWrapper::copyFrom: Datatype missmatch!" << std::endl;
+#else
+        sif::printWarning("ParameterWrapper::copyFrom: Datatype missmatch!\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+		return DATATYPE_MISSMATCH;
+	}
+
+	// The smallest allowed value for rows and columns is one.
+	if(rows == 0 or columns == 0) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "ParameterWrapper::copyFrom: Columns or rows zero!" << std::endl;
+#else
+        sif::printWarning("ParameterWrapper::copyFrom: Columns or rows zero!\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+	    return COLUMN_OR_ROWS_ZERO;
+	}
+
+	//check if from fits into this
+	uint8_t startingRow = 0;
+	uint8_t startingColumn = 0;
+	ParameterWrapper::convertLinearIndexToRowAndColumn(startWritingAtIndex,
+	        &startingRow, &startingColumn);
+
+	if ((from->rows > (rows - startingRow))
+			|| (from->columns > (columns - startingColumn))) {
+		return TOO_BIG;
+	}
+
+	uint8_t typeSize = type.getSize();
+
+	ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+	//copy data
+	if (from->pointsToStream) {
+		switch (type) {
+		case Type::UINT8_T:
+			result = deSerializeData<uint8_t>(startingRow, startingColumn,
+					from->readonlyData, from->rows, from->columns);
+			break;
+		case Type::INT8_T:
+			result = deSerializeData<int8_t>(startingRow, startingColumn,
+					from->readonlyData, from->rows, from->columns);
+			break;
+		case Type::UINT16_T:
+			result = deSerializeData<uint16_t>(startingRow, startingColumn,
+					from->readonlyData, from->rows, from->columns);
+			break;
+		case Type::INT16_T:
+			result = deSerializeData<int16_t>(startingRow, startingColumn,
+					from->readonlyData, from->rows, from->columns);
+			break;
+		case Type::UINT32_T:
+			result = deSerializeData<uint32_t>(startingRow, startingColumn,
+					from->readonlyData, from->rows, from->columns);
+			break;
+		case Type::INT32_T:
+			result = deSerializeData<int32_t>(startingRow, startingColumn,
+					from->readonlyData, from->rows, from->columns);
+			break;
+		case Type::FLOAT:
+			result = deSerializeData<float>(startingRow, startingColumn,
+					from->readonlyData, from->rows, from->columns);
+			break;
+		case Type::DOUBLE:
+			result = deSerializeData<double>(startingRow, startingColumn,
+					from->readonlyData, from->rows, from->columns);
+			break;
+		default:
+			result = UNKNOWN_DATATYPE;
+			break;
+		}
+	}
+	else {
+		//need a type to do arithmetic
+		uint8_t* typedData = static_cast<uint8_t*>(data);
+		for (uint8_t fromRow = 0; fromRow < from->rows; fromRow++) {
+			size_t offset = (((startingRow + fromRow) * static_cast<uint16_t>(
+					columns)) + startingColumn) * typeSize;
+			std::memcpy(typedData +  offset, from->readonlyData,
+					typeSize * from->columns);
+		}
+	}
+
+	return result;
+}
+
+void ParameterWrapper::convertLinearIndexToRowAndColumn(uint16_t index,
+        uint8_t *row, uint8_t *column) {
+    if(row == nullptr or column == nullptr) {
+        return;
+    }
+    // Integer division.
+    *row = index / columns;
+    *column = index % columns;
+}
+
+uint16_t ParameterWrapper::convertRowAndColumnToLinearIndex(uint8_t row,
+        uint8_t column) {
+    return row * columns + column;
+}

src/core/power/CMakeLists.txt

@@ -0,0 +1,7 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		Fuse.cpp
+		PowerComponent.cpp
+		PowerSensor.cpp
+		PowerSwitcher.cpp
+)

src/core/power/Fuse.cpp

@@ -0,0 +1,264 @@
+#include "Fuse.h"
+
+#include "../monitoring/LimitViolationReporter.h"
+#include "../monitoring/MonitoringMessageContent.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../serialize/SerialFixedArrayListAdapter.h"
+#include "../ipc/QueueFactory.h"
+
+object_id_t Fuse::powerSwitchId = 0;
+
+Fuse::Fuse(object_id_t fuseObjectId, uint8_t fuseId,
+		sid_t variableSet, VariableIds ids,
+		float maxCurrent, uint16_t confirmationCount) :
+		SystemObject(fuseObjectId), oldFuseState(0), fuseId(fuseId),
+		currentLimit(fuseObjectId, 1, ids.pidCurrent, confirmationCount,
+				maxCurrent, FUSE_CURRENT_HIGH),
+		powerMonitor(fuseObjectId, 2, ids.poolIdPower,
+				confirmationCount),
+		set(variableSet), voltage(ids.pidVoltage, &set),
+		current(ids.pidCurrent, &set), state(ids.pidState, &set),
+		power(ids.poolIdPower, &set, PoolVariableIF::VAR_READ_WRITE),
+		parameterHelper(this), healthHelper(this, fuseObjectId) {
+	commandQueue = QueueFactory::instance()->createMessageQueue();
+}
+
+Fuse::~Fuse() {
+	QueueFactory::instance()->deleteMessageQueue(commandQueue);
+}
+
+void Fuse::addDevice(PowerComponentIF* switchSet) {
+	devices.push_back(switchSet);
+}
+
+ReturnValue_t Fuse::initialize() {
+	ReturnValue_t result = SystemObject::initialize();
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	result = parameterHelper.initialize();
+	if (result != RETURN_OK) {
+		return result;
+	}
+	result = healthHelper.initialize();
+	if (result != RETURN_OK) {
+		return result;
+	}
+	powerIF = ObjectManager::instance()->get<PowerSwitchIF>(powerSwitchId);
+	if (powerIF == NULL) {
+		return RETURN_FAILED;
+	}
+	return RETURN_OK;
+}
+
+void Fuse::calculatePowerLimits(float* low, float* high) {
+	for (DeviceList::iterator iter = devices.begin(); iter != devices.end();
+			iter++) {
+		if (areSwitchesOfComponentOn(iter)) {
+			*low += (*iter)->getMin();
+			*high += (*iter)->getMax();
+		}
+	}
+}
+
+ReturnValue_t Fuse::check() {
+	set.read();
+	if (!healthHelper.healthTable->isHealthy(getObjectId())) {
+		setAllMonitorsToUnchecked();
+		set.setValidity(false, true);
+		return set.commit();
+	}
+	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+	checkFuseState();
+	calculateFusePower();
+	//Check if power is valid and if fuse state is off or invalid.
+	if (!power.isValid() || (state == 0) || !state.isValid()) {
+		result = powerMonitor.setToInvalid();
+	} else {
+		float lowLimit = 0.0;
+		float highLimit = RESIDUAL_POWER;
+		calculatePowerLimits(&lowLimit, &highLimit);
+		result = powerMonitor.checkPower(power.value, lowLimit, highLimit);
+		if (result == MonitoringIF::BELOW_LOW_LIMIT) {
+			reportEvents(POWER_BELOW_LOW_LIMIT);
+		} else if (result == MonitoringIF::ABOVE_HIGH_LIMIT) {
+			reportEvents(POWER_ABOVE_HIGH_LIMIT);
+		}
+	}
+	set.commit();
+	return result;
+}
+
+ReturnValue_t Fuse::serialize(uint8_t** buffer, size_t* size,
+		size_t maxSize, Endianness streamEndianness) const {
+	ReturnValue_t result = RETURN_FAILED;
+	for (DeviceList::const_iterator iter = devices.begin();
+			iter != devices.end(); iter++) {
+		result = (*iter)->serialize(buffer, size, maxSize, streamEndianness);
+		if (result != RETURN_OK) {
+			return result;
+		}
+	}
+	return RETURN_OK;
+}
+
+size_t Fuse::getSerializedSize() const {
+	uint32_t size = 0;
+	for (DeviceList::const_iterator iter = devices.begin();
+			iter != devices.end(); iter++) {
+		size += (*iter)->getSerializedSize();
+	}
+	return size;
+}
+
+ReturnValue_t Fuse::deSerialize(const uint8_t** buffer, size_t* size,
+        Endianness streamEndianness) {
+	ReturnValue_t result = RETURN_FAILED;
+	for (DeviceList::iterator iter = devices.begin(); iter != devices.end();
+			iter++) {
+		result = (*iter)->deSerialize(buffer, size, streamEndianness);
+		if (result != RETURN_OK) {
+			return result;
+		}
+	}
+	return RETURN_OK;
+}
+
+uint8_t Fuse::getFuseId() const {
+	return fuseId;
+}
+
+void Fuse::calculateFusePower() {
+	ReturnValue_t result1 = currentLimit.check();
+	if (result1 != HasReturnvaluesIF::RETURN_OK || !(voltage.isValid())) {
+		power.setValid(PoolVariableIF::INVALID);
+		return;
+	}
+	//Calculate fuse power.
+	power.value = current.value * voltage.value;
+	power.setValid(PoolVariableIF::VALID);
+}
+
+ReturnValue_t Fuse::performOperation(uint8_t opCode) {
+	checkCommandQueue();
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void Fuse::reportEvents(Event event) {
+	if (!powerMonitor.isEventEnabled()) {
+		return;
+	}
+	for (DeviceList::iterator iter = devices.begin(); iter != devices.end();
+			iter++) {
+		if (areSwitchesOfComponentOn(iter)) {
+			EventManagerIF::triggerEvent((*iter)->getDeviceObjectId(), event);
+		}
+	}
+}
+
+MessageQueueId_t Fuse::getCommandQueue() const {
+	return commandQueue->getId();
+}
+
+void Fuse::setAllMonitorsToUnchecked() {
+	currentLimit.setToUnchecked();
+	powerMonitor.setToUnchecked();
+}
+
+void Fuse::checkCommandQueue() {
+	CommandMessage command;
+	ReturnValue_t result = commandQueue->receiveMessage(&command);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return;
+	}
+	result = healthHelper.handleHealthCommand(&command);
+	if (result == HasReturnvaluesIF::RETURN_OK) {
+		return;
+	}
+	result = parameterHelper.handleParameterMessage(&command);
+	if (result == HasReturnvaluesIF::RETURN_OK) {
+		return;
+	}
+	command.setToUnknownCommand();
+	commandQueue->reply(&command);
+}
+
+void Fuse::checkFuseState() {
+	if (!state.isValid()) {
+		oldFuseState = 0;
+		return;
+	}
+	if (state == 0) {
+		if (oldFuseState != 0) {
+			reportEvents(FUSE_WENT_OFF);
+		}
+	}
+	oldFuseState = state.value;
+}
+
+float Fuse::getPower() {
+	if (power.isValid()) {
+		return power.value;
+	} else {
+		return 0.0;
+	}
+}
+
+void Fuse::setDataPoolEntriesInvalid() {
+	set.read();
+	set.setValidity(false, true);
+	set.commit();
+}
+
+ReturnValue_t Fuse::getParameter(uint8_t domainId, uint8_t uniqueId,
+		ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
+		uint16_t startAtIndex) {
+	ReturnValue_t result = currentLimit.getParameter(domainId, uniqueId,
+			parameterWrapper, newValues, startAtIndex);
+	if (result != INVALID_DOMAIN_ID) {
+		return result;
+	}
+	result = powerMonitor.getParameter(domainId, uniqueId, parameterWrapper,
+			newValues, startAtIndex);
+	return result;
+}
+
+bool Fuse::areSwitchesOfComponentOn(DeviceList::iterator iter) {
+	if (powerIF->getSwitchState((*iter)->getSwitchId1())
+			!= PowerSwitchIF::SWITCH_ON) {
+		return false;
+	}
+	if ((*iter)->hasTwoSwitches()) {
+		if ((powerIF->getSwitchState((*iter)->getSwitchId2())
+				!= PowerSwitchIF::SWITCH_ON)) {
+			return false;
+		}
+	}
+	return true;
+}
+
+bool Fuse::isPowerValid() {
+	return power.isValid();
+}
+
+ReturnValue_t Fuse::setHealth(HealthState health) {
+	healthHelper.setHealth(health);
+	return RETURN_OK;
+}
+
+HasHealthIF::HealthState Fuse::getHealth() {
+	return healthHelper.getHealth();
+}
+
+ReturnValue_t Fuse::PowerMonitor::checkPower(float sample, float lowerLimit,
+		float upperLimit) {
+	if (sample > upperLimit) {
+		return this->monitorStateIs(MonitoringIF::ABOVE_HIGH_LIMIT, sample,
+				upperLimit);
+	} else if (sample < lowerLimit) {
+		return this->monitorStateIs(MonitoringIF::BELOW_LOW_LIMIT, sample,
+				lowerLimit);
+	} else {
+		return this->monitorStateIs(RETURN_OK, sample, 0.0); //Within limits.
+	}
+}

src/core/power/PowerComponent.cpp

@@ -0,0 +1,82 @@
+#include "PowerComponent.h"
+#include "../serialize/SerializeAdapter.h"
+
+
+PowerComponent::PowerComponent(): switchId1(0xFF), switchId2(0xFF),
+        doIHaveTwoSwitches(false) {
+}
+
+PowerComponent::PowerComponent(object_id_t setId, uint8_t moduleId, float min,
+        float max, uint8_t switchId1, bool twoSwitches, uint8_t switchId2) :
+		deviceObjectId(setId), switchId1(switchId1), switchId2(switchId2),
+		doIHaveTwoSwitches(twoSwitches), minPower(min), maxPower(max),
+		moduleId(moduleId) {
+}
+
+ReturnValue_t PowerComponent::serialize(uint8_t** buffer, size_t* size,
+		size_t maxSize, Endianness streamEndianness) const {
+	ReturnValue_t result = SerializeAdapter::serialize(&minPower, buffer,
+			size, maxSize, streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	return SerializeAdapter::serialize(&maxPower, buffer, size, maxSize,
+			streamEndianness);
+}
+
+size_t PowerComponent::getSerializedSize() const {
+	return sizeof(minPower) + sizeof(maxPower);
+}
+
+object_id_t PowerComponent::getDeviceObjectId() {
+	return deviceObjectId;
+}
+
+uint8_t PowerComponent::getSwitchId1() {
+	return switchId1;
+}
+
+uint8_t PowerComponent::getSwitchId2() {
+	return switchId2;
+}
+
+bool PowerComponent::hasTwoSwitches() {
+	return doIHaveTwoSwitches;
+}
+
+float PowerComponent::getMin() {
+	return minPower;
+}
+
+float PowerComponent::getMax() {
+	return maxPower;
+}
+
+ReturnValue_t PowerComponent::deSerialize(const uint8_t** buffer, size_t* size,
+        Endianness streamEndianness) {
+	ReturnValue_t result = SerializeAdapter::deSerialize(&minPower, buffer,
+			size, streamEndianness);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	return SerializeAdapter::deSerialize(&maxPower, buffer, size, streamEndianness);
+}
+
+ReturnValue_t PowerComponent::getParameter(uint8_t domainId, uint8_t uniqueId,
+        ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
+        uint16_t startAtIndex) {
+	if (domainId != moduleId) {
+		return INVALID_DOMAIN_ID;
+	}
+	switch (uniqueId) {
+	case 0:
+		parameterWrapper->set<>(minPower);
+		break;
+	case 1:
+		parameterWrapper->set<>(maxPower);
+		break;
+	default:
+		return INVALID_IDENTIFIER_ID;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}

src/core/power/PowerSensor.cpp

@@ -0,0 +1,133 @@
+#include "PowerSensor.h"
+
+#include "../ipc/QueueFactory.h"
+
+PowerSensor::PowerSensor(object_id_t objectId, sid_t setId,  VariableIds ids,
+		DefaultLimits limits, SensorEvents events, uint16_t confirmationCount) :
+		SystemObject(objectId), parameterHelper(this),
+		healthHelper(this, objectId),
+		powerSensorSet(setId), current(ids.pidCurrent, &powerSensorSet),
+		voltage(ids.pidVoltage, &powerSensorSet),
+		power(ids.poolIdPower, &powerSensorSet, PoolVariableIF::VAR_WRITE),
+		currentLimit(objectId, MODULE_ID_CURRENT, ids.pidCurrent, confirmationCount,
+				limits.currentMin, limits.currentMax, events.currentLow,
+				events.currentHigh),
+		voltageLimit(objectId, MODULE_ID_VOLTAGE,
+				ids.pidVoltage, confirmationCount, limits.voltageMin,
+				limits.voltageMax, events.voltageLow, events.voltageHigh) {
+		commandQueue = QueueFactory::instance()->createMessageQueue();
+}
+
+PowerSensor::~PowerSensor() {
+	QueueFactory::instance()->deleteMessageQueue(commandQueue);
+}
+
+ReturnValue_t PowerSensor::calculatePower() {
+	powerSensorSet.read();
+	ReturnValue_t result1 = HasReturnvaluesIF::RETURN_FAILED;
+	ReturnValue_t result2 = HasReturnvaluesIF::RETURN_FAILED;
+	if (healthHelper.healthTable->isHealthy(getObjectId()) && voltage.isValid()
+			&& current.isValid()) {
+		result1 = voltageLimit.doCheck(voltage.value);
+		result2 = currentLimit.doCheck(current.value);
+	} else {
+		voltageLimit.setToInvalid();
+		currentLimit.setToInvalid();
+		result1 = OBJECT_NOT_HEALTHY;
+	}
+	if (result1 != HasReturnvaluesIF::RETURN_OK
+			|| result2 != HasReturnvaluesIF::RETURN_OK) {
+		result1 = MonitoringIF::INVALID;
+		power.setValid(PoolVariableIF::INVALID);
+	} else {
+		power.setValid(PoolVariableIF::VALID);
+		power.value = current.value * voltage.value;
+	}
+	powerSensorSet.commit();
+	return result1;
+}
+
+ReturnValue_t PowerSensor::performOperation(uint8_t opCode) {
+	checkCommandQueue();
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+MessageQueueId_t PowerSensor::getCommandQueue() const {
+	return commandQueue->getId();
+}
+
+ReturnValue_t PowerSensor::initialize() {
+	ReturnValue_t result = SystemObject::initialize();
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	result = healthHelper.initialize();
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	result = parameterHelper.initialize();
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	return result;
+}
+
+void PowerSensor::setAllMonitorsToUnchecked() {
+	currentLimit.setToUnchecked();
+	voltageLimit.setToUnchecked();
+}
+
+void PowerSensor::checkCommandQueue() {
+	CommandMessage command;
+	ReturnValue_t result = commandQueue->receiveMessage(&command);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return;
+	}
+	result = healthHelper.handleHealthCommand(&command);
+	if (result == HasReturnvaluesIF::RETURN_OK) {
+		return;
+	}
+	result = parameterHelper.handleParameterMessage(&command);
+	if (result == HasReturnvaluesIF::RETURN_OK) {
+		return;
+	}
+	command.setToUnknownCommand();
+	commandQueue->reply(&command);
+}
+
+void PowerSensor::setDataPoolEntriesInvalid() {
+	powerSensorSet.read();
+	powerSensorSet.setValidity(false, true);
+	powerSensorSet.commit();
+}
+
+float PowerSensor::getPower() {
+	if (power.isValid()) {
+		return power.value;
+	} else {
+		return 0.0;
+	}
+
+}
+
+ReturnValue_t PowerSensor::setHealth(HealthState health) {
+	healthHelper.setHealth(health);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+HasHealthIF::HealthState PowerSensor::getHealth() {
+	return healthHelper.getHealth();
+}
+
+ReturnValue_t PowerSensor::getParameter(uint8_t domainId, uint8_t uniqueId,
+		ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
+		uint16_t startAtIndex) {
+	ReturnValue_t result = currentLimit.getParameter(domainId, uniqueId,
+			parameterWrapper, newValues, startAtIndex);
+	if (result != INVALID_DOMAIN_ID) {
+		return result;
+	}
+	result = voltageLimit.getParameter(domainId, uniqueId, parameterWrapper,
+			newValues, startAtIndex);
+	return result;
+}

src/core/power/PowerSwitcher.cpp

@@ -0,0 +1,135 @@
+#include "PowerSwitcher.h"
+
+#include "../objectmanager/ObjectManager.h"
+#include "../serviceinterface/ServiceInterface.h"
+
+PowerSwitcher::PowerSwitcher(uint8_t setSwitch1, uint8_t setSwitch2,
+		PowerSwitcher::State_t setStartState):
+		state(setStartState), firstSwitch(setSwitch1),
+		secondSwitch(setSwitch2) {
+}
+
+ReturnValue_t PowerSwitcher::initialize(object_id_t powerSwitchId) {
+	power = ObjectManager::instance()->get<PowerSwitchIF>(powerSwitchId);
+	if (power == nullptr) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t PowerSwitcher::getStateOfSwitches() {
+	SwitchReturn_t result = howManySwitches();
+
+	switch (result) {
+	case ONE_SWITCH:
+		return power->getSwitchState(firstSwitch);
+	case TWO_SWITCHES: {
+	    ReturnValue_t firstSwitchState = power->getSwitchState(firstSwitch);
+	    ReturnValue_t secondSwitchState = power->getSwitchState(firstSwitch);
+		if ((firstSwitchState == PowerSwitchIF::SWITCH_ON)
+				&& (secondSwitchState == PowerSwitchIF::SWITCH_ON)) {
+			return PowerSwitchIF::SWITCH_ON;
+		}
+		else if ((firstSwitchState == PowerSwitchIF::SWITCH_OFF)
+				&& (secondSwitchState  == PowerSwitchIF::SWITCH_OFF)) {
+			return PowerSwitchIF::SWITCH_OFF;
+		}
+		else {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+	}
+	default:
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+}
+
+void PowerSwitcher::commandSwitches(ReturnValue_t onOff) {
+	SwitchReturn_t result = howManySwitches();
+	switch (result) {
+	case TWO_SWITCHES:
+		power->sendSwitchCommand(secondSwitch, onOff);
+		/* NO BREAK falls through*/
+	case ONE_SWITCH:
+		power->sendSwitchCommand(firstSwitch, onOff);
+		break;
+	}
+	return;
+
+}
+
+void PowerSwitcher::turnOn() {
+	commandSwitches(PowerSwitchIF::SWITCH_ON);
+	state = WAIT_ON;
+}
+
+void PowerSwitcher::turnOff() {
+	commandSwitches(PowerSwitchIF::SWITCH_OFF);
+	state = WAIT_OFF;
+}
+
+PowerSwitcher::SwitchReturn_t PowerSwitcher::howManySwitches() {
+	if (secondSwitch == NO_SWITCH) {
+		return ONE_SWITCH;
+	} else {
+		return TWO_SWITCHES;
+	}
+}
+
+void PowerSwitcher::doStateMachine() {
+	switch (state) {
+	case SWITCH_IS_OFF:
+	case SWITCH_IS_ON:
+		//Do nothing.
+		break;
+	case WAIT_OFF:
+		if (getStateOfSwitches() == PowerSwitchIF::SWITCH_OFF) {
+			state = SWITCH_IS_OFF;
+		}
+		break;
+	case WAIT_ON:
+		if (getStateOfSwitches() == PowerSwitchIF::SWITCH_ON) {
+			state = SWITCH_IS_ON;
+		}
+		break;
+	default:
+		//Should never happen.
+		break;
+	}
+}
+
+ReturnValue_t PowerSwitcher::checkSwitchState() {
+	switch (state) {
+	case WAIT_OFF:
+	case WAIT_ON:
+		return IN_POWER_TRANSITION;
+	case SWITCH_IS_OFF:
+		if (getStateOfSwitches() == PowerSwitchIF::SWITCH_OFF) {
+			return RETURN_OK;
+		} else {
+			return SWITCH_STATE_MISMATCH;
+		}
+	case SWITCH_IS_ON:
+		if (getStateOfSwitches() == PowerSwitchIF::SWITCH_ON) {
+			return RETURN_OK;
+		} else {
+			return SWITCH_STATE_MISMATCH;
+		}
+	}
+	return RETURN_FAILED;
+}
+
+PowerSwitcher::State_t PowerSwitcher::getState() {
+	return state;
+}
+
+uint32_t PowerSwitcher::getSwitchDelay() {
+	return power->getSwitchDelayMs();
+}
+
+uint8_t PowerSwitcher::getFirstSwitch() const {
+	return firstSwitch;
+}
+
+uint8_t PowerSwitcher::getSecondSwitch() const {
+	return secondSwitch;
+}

src/core/serialize/CMakeLists.txt

@@ -0,0 +1,4 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		SerialBufferAdapter.cpp
+)

src/core/serialize/SerialBufferAdapter.cpp

@@ -0,0 +1,133 @@
+#include "../serialize/SerialBufferAdapter.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include <cstring>
+
+template<typename count_t>
+SerialBufferAdapter<count_t>::SerialBufferAdapter(const uint8_t* buffer,
+		count_t bufferLength, bool serializeLength) :
+		serializeLength(serializeLength),
+		constBuffer(buffer), buffer(nullptr),
+		bufferLength(bufferLength) {}
+
+template<typename count_t>
+SerialBufferAdapter<count_t>::SerialBufferAdapter(uint8_t* buffer,
+		count_t bufferLength, bool serializeLength) :
+		serializeLength(serializeLength), constBuffer(buffer), buffer(buffer),
+		bufferLength(bufferLength) {}
+
+
+template<typename count_t>
+SerialBufferAdapter<count_t>::~SerialBufferAdapter() {
+}
+
+template<typename count_t>
+ReturnValue_t SerialBufferAdapter<count_t>::serialize(uint8_t** buffer,
+		size_t* size, size_t maxSize, Endianness streamEndianness) const {
+	if (serializeLength) {
+		ReturnValue_t result = SerializeAdapter::serialize(&bufferLength,
+				buffer, size, maxSize, streamEndianness);
+		if(result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+	}
+
+	if (*size + bufferLength > maxSize) {
+		return BUFFER_TOO_SHORT;
+	}
+
+	if (this->constBuffer != nullptr) {
+		std::memcpy(*buffer, this->constBuffer, bufferLength);
+	}
+	else if (this->buffer != nullptr) {
+		// This will propably be never reached, constBuffer should always be
+		// set if non-const buffer is set.
+		std::memcpy(*buffer, this->buffer, bufferLength);
+	}
+	else {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	*size += bufferLength;
+	(*buffer) += bufferLength;
+	return HasReturnvaluesIF::RETURN_OK;
+
+}
+
+template<typename count_t>
+size_t SerialBufferAdapter<count_t>::getSerializedSize() const {
+	if (serializeLength) {
+		return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength);
+	} else {
+		return bufferLength;
+	}
+}
+
+template<typename count_t>
+ReturnValue_t SerialBufferAdapter<count_t>::deSerialize(const uint8_t** buffer,
+		size_t* size, Endianness streamEndianness) {
+	if (this->buffer == nullptr) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	if(serializeLength){
+		count_t lengthField = 0;
+		ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField,
+				buffer, size, streamEndianness);
+		if(result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+		if(lengthField > bufferLength) {
+			return TOO_MANY_ELEMENTS;
+		}
+		bufferLength = lengthField;
+	}
+
+	if (bufferLength <= *size) {
+		*size -= bufferLength;
+		std::memcpy(this->buffer, *buffer, bufferLength);
+		(*buffer) += bufferLength;
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		return STREAM_TOO_SHORT;
+	}
+}
+
+template<typename count_t>
+uint8_t * SerialBufferAdapter<count_t>::getBuffer() {
+	if(buffer == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "Wrong access function for stored type !"
+				 " Use getConstBuffer()." << std::endl;
+#endif
+		return nullptr;
+	}
+	return buffer;
+}
+
+template<typename count_t>
+const uint8_t * SerialBufferAdapter<count_t>::getConstBuffer() {
+	if(constBuffer == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "SerialBufferAdapter::getConstBuffer:"
+				" Buffers are unitialized!" << std::endl;
+#endif
+		return nullptr;
+	}
+	return constBuffer;
+}
+
+template<typename count_t>
+void SerialBufferAdapter<count_t>::setBuffer(uint8_t* buffer,
+		count_t bufferLength) {
+	this->buffer = buffer;
+	this->constBuffer = buffer;
+	this->bufferLength = bufferLength;
+}
+
+
+//forward Template declaration for linker
+template class SerialBufferAdapter<uint8_t>;
+template class SerialBufferAdapter<uint16_t>;
+template class SerialBufferAdapter<uint32_t>;
+template class SerialBufferAdapter<uint64_t>;
+

src/core/serviceinterface/CMakeLists.txt

@@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+	ServiceInterfaceStream.cpp
+	ServiceInterfaceBuffer.cpp
+	ServiceInterfacePrinter.cpp
+)

src/core/serviceinterface/ServiceInterfaceBuffer.cpp

@@ -0,0 +1,268 @@
+#include "ServiceInterfaceBuffer.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
+#include "../timemanager/Clock.h"
+
+#include "serviceInterfaceDefintions.h"
+#include <cstring>
+#include <inttypes.h>
+
+#if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
+#include "Windows.h"
+#endif
+
+// to be implemented by bsp
+extern "C" void printChar(const char*, bool errStream);
+
+#ifndef UT699
+
+ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string setMessage,
+		bool addCrToPreamble, bool buffered , bool errStream, uint16_t port):
+		isActive(true), logMessage(setMessage),
+		addCrToPreamble(addCrToPreamble), buffered(buffered),
+		errStream(errStream) {
+	if(buffered) {
+		// Set pointers if the stream is buffered.
+		setp( buf, buf + BUF_SIZE );
+	}
+
+#if FSFW_COLORED_OUTPUT == 1
+	if(setMessage.find("DEBUG") != std::string::npos) {
+		colorPrefix = sif::ANSI_COLOR_CYAN;
+	}
+	else if(setMessage.find("INFO") != std::string::npos) {
+		colorPrefix = sif::ANSI_COLOR_GREEN;
+	}
+	else if(setMessage.find("WARNING") != std::string::npos) {
+		colorPrefix = sif::ANSI_COLOR_MAGENTA;
+	}
+	else if(setMessage.find("ERROR") != std::string::npos) {
+		colorPrefix = sif::ANSI_COLOR_RED;
+	}
+	else {
+		colorPrefix = sif::ANSI_COLOR_RESET;
+	}
+
+#ifdef WIN32
+	HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
+	DWORD dwMode = 0;
+	GetConsoleMode(hOut, &dwMode);
+	dwMode |= ENABLE_VIRTUAL_TERMINAL_PROCESSING;
+	SetConsoleMode(hOut, dwMode);
+#endif
+
+#endif
+
+	preamble.reserve(MAX_PREAMBLE_SIZE);
+	preamble.resize(MAX_PREAMBLE_SIZE);
+}
+
+void ServiceInterfaceBuffer::putChars(char const* begin, char const* end) {
+	char array[BUF_SIZE];
+	uint32_t length = end - begin;
+	if (length > sizeof(array)) {
+		length = sizeof(array);
+	}
+	memcpy(array, begin, length);
+
+	for(; begin != end; begin++){
+		if(errStream) {
+			printChar(begin, true);
+		}
+		else {
+			printChar(begin, false);
+		}
+	}
+}
+
+#endif
+
+int ServiceInterfaceBuffer::overflow(int c) {
+	if(not buffered and this->isActive) {
+		if (c != Traits::eof()) {
+			if(errStream) {
+				printChar(reinterpret_cast<const char*>(&c), true);
+			}
+			else {
+				printChar(reinterpret_cast<const char*>(&c), false);
+			}
+		}
+		return 0;
+	}
+	// Handle output
+	putChars(pbase(), pptr());
+	if (c != Traits::eof()) {
+		char c2 = c;
+		// Handle the one character that didn't fit to buffer
+		putChars(&c2, &c2 + 1);
+	}
+	// This tells that buffer is empty again
+	setp(buf, buf + BUF_SIZE - 1);
+	// I'm not sure about this return value!
+	return 0;
+}
+
+int ServiceInterfaceBuffer::sync(void) {
+	if(not this->isActive and not buffered) {
+		if(not buffered) {
+			setp(buf, buf + BUF_SIZE - 1);
+		}
+		return 0;
+	}
+	if(not buffered) {
+		return 0;
+	}
+
+	size_t preambleSize  = 0;
+	std::string* preamble = getPreamble(&preambleSize);
+	// Write logMessage and time
+	this->putChars(preamble->data(), preamble->data() + preambleSize);
+	// Handle output
+	this->putChars(pbase(), pptr());
+	// This tells that buffer is empty again
+	setp(buf, buf + BUF_SIZE - 1);
+	return 0;
+}
+
+bool ServiceInterfaceBuffer::isBuffered() const {
+	return buffered;
+}
+
+std::string* ServiceInterfaceBuffer::getPreamble(size_t * preambleSize) {
+	Clock::TimeOfDay_t loggerTime;
+	Clock::getDateAndTime(&loggerTime);
+	size_t currentSize = 0;
+	char* parsePosition = &preamble[0];
+	if(addCrToPreamble) {
+		preamble[0] = '\r';
+		currentSize += 1;
+		parsePosition += 1;
+	}
+
+#if FSFW_COLORED_OUTPUT == 1
+	currentSize += sprintf(parsePosition, "%s", colorPrefix.c_str());
+	parsePosition += colorPrefix.size();
+#endif
+
+	int32_t charCount = sprintf(parsePosition,
+			"%s: | %02" SCNu32 ":%02" SCNu32 ":%02" SCNu32 ".%03" SCNu32 " | ",
+					this->logMessage.c_str(), loggerTime.hour,
+					loggerTime.minute,
+					loggerTime.second,
+					loggerTime.usecond /1000);
+	if(charCount < 0) {
+		printf("ServiceInterfaceBuffer: Failure parsing preamble\r\n");
+		return &preamble;
+	}
+	if(charCount > MAX_PREAMBLE_SIZE) {
+		printf("ServiceInterfaceBuffer: Char count too large for maximum "
+				"preamble size");
+		return &preamble;
+	}
+	currentSize += charCount;
+	if(preambleSize != nullptr) {
+		*preambleSize = currentSize;
+	}
+	return &preamble;
+}
+
+bool ServiceInterfaceBuffer::crAdditionEnabled() const {
+    return addCrToPreamble;
+}
+
+void ServiceInterfaceBuffer::setAsciiColorPrefix(std::string colorPrefix) {
+    this->colorPrefix = colorPrefix;
+}
+
+#ifdef UT699
+#include "../osal/rtems/Interrupt.h"
+
+ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string set_message,
+		uint16_t port) {
+	this->log_message = set_message;
+	this->isActive = true;
+	setp( buf, buf + BUF_SIZE );
+}
+
+void ServiceInterfaceBuffer::putChars(char const* begin, char const* end) {
+	char array[BUF_SIZE];
+	uint32_t length = end - begin;
+	if (length > sizeof(array)) {
+		length = sizeof(array);
+	}
+	memcpy(array, begin, length);
+
+	if (!Interrupt::isInterruptInProgress()) {
+		std::cout << array;
+	} else {
+		//Uncomment the following line if you need ISR debug output.
+//		printk(array);
+	}
+}
+#endif //UT699
+
+#ifdef ML505
+#include <bsp_flp/network/networkconfig.h>
+ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string set_message,
+		uint16_t port) :
+		isActive(true), log_message(set_message), udpSocket(0), remoteAddressLength(
+				sizeof(remoteAddress)) {
+	setp(buf, buf + BUF_SIZE);
+	memset((uint8_t*) &remoteAddress, 0, sizeof(remoteAddress));
+	remoteAddress.sin_family = AF_INET;
+	remoteAddress.sin_port = htons(port);
+	remoteAddress.sin_addr.s_addr = htonl(inet_addr("192.168.250.100"));
+}
+
+void ServiceInterfaceBuffer::putChars(char const* begin, char const* end) {
+
+	char array[BUF_SIZE];
+	uint32_t length = end - begin;
+	if (length > sizeof(array)) {
+		length = sizeof(array);
+	}
+	memcpy(array, begin, length);
+
+	if (udpSocket <= 0) {
+		initSocket();
+	}
+
+	if (udpSocket > 0) {
+		sendto(udpSocket, array, length, 0, (sockaddr*) &remoteAddress,
+				sizeof(remoteAddress));
+	}
+
+}
+
+void ServiceInterfaceBuffer::initSocket() {
+	sockaddr_in address;
+	memset((uint8_t*) &address, 0, sizeof(address));
+	address.sin_family = AF_INET;
+	address.sin_port = htons(0);
+	address.sin_addr.s_addr = htonl(INADDR_ANY);
+
+	udpSocket = socket(PF_INET, SOCK_DGRAM, 0);
+	if (socket < 0) {
+		printf("Error opening socket!\n");
+		return;
+	}
+	timeval timeout = { 0, 20 };
+	if (setsockopt(udpSocket, SOL_SOCKET, SO_RCVTIMEO, &timeout,
+			sizeof(timeout)) < 0) {
+		printf("Error setting SO_RCVTIMEO socket options!\n");
+		return;
+	}
+	if (setsockopt(udpSocket, SOL_SOCKET, SO_SNDTIMEO, &timeout,
+			sizeof(timeout)) < 0) {
+		printf("Error setting SO_SNDTIMEO socket options!\n");
+		return;
+	}
+	if (bind(udpSocket, (sockaddr *) &address, sizeof(address)) < 0) {
+		printf("Error binding socket!\n");
+	}
+}
+
+#endif //ML505
+
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */

src/core/serviceinterface/ServiceInterfacePrinter.cpp

@@ -0,0 +1,140 @@
+#include <FSFWConfig.h>
+#include "ServiceInterfacePrinter.h"
+#include "serviceInterfaceDefintions.h"
+#include "../timemanager/Clock.h"
+
+#include <cstdarg>
+#include <cstdint>
+
+static sif::PrintLevel printLevel = sif::PrintLevel::DEBUG_LEVEL;
+#if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
+static bool consoleInitialized = false;
+#endif /* defined(WIN32) && FSFW_COLORED_OUTPUT == 1 */
+
+#if FSFW_DISABLE_PRINTOUT == 0
+
+static bool addCrAtEnd = false;
+
+uint8_t printBuffer[fsfwconfig::FSFW_PRINT_BUFFER_SIZE];
+
+void fsfwPrint(sif::PrintLevel printType, const char* fmt, va_list arg) {
+
+#if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
+	if(not consoleInitialized) {
+		HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
+		DWORD dwMode = 0;
+		GetConsoleMode(hOut, &dwMode);
+		dwMode |= ENABLE_VIRTUAL_TERMINAL_PROCESSING;
+		SetConsoleMode(hOut, dwMode);
+	}
+	consoleInitialized = true;
+#endif
+
+    size_t len = 0;
+    char* bufferPosition = reinterpret_cast<char*>(printBuffer);
+
+    /* Check logger level */
+    if(printType == sif::PrintLevel::NONE or printType > printLevel) {
+        return;
+    }
+
+    /* Log message to terminal */
+
+#if FSFW_COLORED_OUTPUT == 1
+    if(printType == sif::PrintLevel::INFO_LEVEL) {
+    	len += sprintf(bufferPosition, sif::ANSI_COLOR_GREEN);
+    }
+    else if(printType == sif::PrintLevel::DEBUG_LEVEL) {
+    	len += sprintf(bufferPosition, sif::ANSI_COLOR_CYAN);
+    }
+    else if(printType == sif::PrintLevel::WARNING_LEVEL) {
+    	len += sprintf(bufferPosition, sif::ANSI_COLOR_YELLOW);
+    }
+    else if(printType == sif::PrintLevel::ERROR_LEVEL) {
+    	len += sprintf(bufferPosition, sif::ANSI_COLOR_RED);
+    }
+#endif
+
+   if (printType == sif::PrintLevel::INFO_LEVEL) {
+       len += sprintf(bufferPosition + len, "INFO: ");
+   }
+   if(printType == sif::PrintLevel::DEBUG_LEVEL) {
+	   len += sprintf(bufferPosition + len, "DEBUG: ");
+   }
+   if(printType == sif::PrintLevel::WARNING_LEVEL) {
+	   len += sprintf(bufferPosition + len, "WARNING: ");
+   }
+
+   if(printType == sif::PrintLevel::ERROR_LEVEL) {
+	   len += sprintf(bufferPosition + len, "ERROR: ");
+   }
+
+    Clock::TimeOfDay_t now;
+    Clock::getDateAndTime(&now);
+    /*
+     * Log current time to terminal if desired.
+     */
+    len += sprintf(bufferPosition + len, "| %lu:%02lu:%02lu.%03lu | ",
+    		(unsigned long) now.hour,
+			(unsigned long) now.minute,
+			(unsigned long) now.second,
+			(unsigned long) now.usecond /1000);
+
+    len += vsnprintf(bufferPosition + len, sizeof(printBuffer) - len, fmt, arg);
+
+    if(addCrAtEnd) {
+    	len += sprintf(bufferPosition + len, "\r");
+    }
+
+    printf("%s", printBuffer);
+}
+
+
+void sif::printInfo(const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    fsfwPrint(sif::PrintLevel::INFO_LEVEL, fmt, args);
+    va_end(args);
+}
+
+void sif::printWarning(const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    fsfwPrint(sif::PrintLevel::WARNING_LEVEL, fmt, args);
+    va_end(args);
+}
+
+void sif::printDebug(const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    fsfwPrint(sif::PrintLevel::DEBUG_LEVEL, fmt, args);
+    va_end(args);
+}
+
+void sif::setToAddCrAtEnd(bool addCrAtEnd_) {
+	addCrAtEnd = addCrAtEnd_;
+}
+
+void sif::printError(const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    fsfwPrint(sif::PrintLevel::ERROR_LEVEL, fmt, args);
+    va_end(args);
+}
+
+#else
+
+void sif::printInfo(const char *fmt, ...) {}
+void sif::printWarning(const char *fmt, ...) {}
+void sif::printDebug(const char *fmt, ...) {}
+void sif::printError(const char *fmt, ...) {}
+
+#endif /* FSFW_DISABLE_PRINTOUT == 0 */
+
+void sif::setPrintLevel(PrintLevel printLevel_) {
+	printLevel = printLevel_;
+}
+
+sif::PrintLevel sif::getPrintLevel() {
+	return printLevel;
+}

src/core/serviceinterface/ServiceInterfaceStream.cpp

@@ -0,0 +1,27 @@
+#include "ServiceInterfaceStream.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
+ServiceInterfaceStream::ServiceInterfaceStream(std::string setMessage,
+		bool addCrToPreamble, bool buffered, bool errStream, uint16_t port) :
+		std::ostream(&streambuf),
+		streambuf(setMessage, addCrToPreamble, buffered, errStream, port) {}
+
+void ServiceInterfaceStream::setActive( bool myActive) {
+	this->streambuf.isActive = myActive;
+}
+
+std::string* ServiceInterfaceStream::getPreamble() {
+	return streambuf.getPreamble();
+}
+
+bool ServiceInterfaceStream::crAdditionEnabled() const {
+    return streambuf.crAdditionEnabled();
+}
+
+void ServiceInterfaceStream::setAsciiColorPrefix(std::string asciiColorCode) {
+    streambuf.setAsciiColorPrefix(asciiColorCode);
+}
+
+#endif
+

src/core/storagemanager/CMakeLists.txt

@@ -0,0 +1,7 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		ConstStorageAccessor.cpp
+		StorageAccessor.cpp
+		LocalPool.cpp
+		PoolManager.cpp
+)

src/core/storagemanager/ConstStorageAccessor.cpp

@@ -0,0 +1,98 @@
+#include "ConstStorageAccessor.h"
+#include "StorageManagerIF.h"
+
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../globalfunctions/arrayprinter.h"
+
+#include <algorithm>
+
+ConstStorageAccessor::ConstStorageAccessor(store_address_t storeId):
+		storeId(storeId) {}
+
+ConstStorageAccessor::ConstStorageAccessor(store_address_t storeId,
+		StorageManagerIF* store):
+		storeId(storeId), store(store) {
+	internalState = AccessState::ASSIGNED;
+}
+
+ConstStorageAccessor::~ConstStorageAccessor() {
+	if(deleteData and store != nullptr) {
+		store->deleteData(storeId);
+	}
+}
+
+ConstStorageAccessor::ConstStorageAccessor(ConstStorageAccessor&& other):
+		constDataPointer(other.constDataPointer), storeId(other.storeId),
+		size_(other.size_), store(other.store), deleteData(other.deleteData),
+		internalState(other.internalState) {
+	// This prevent premature deletion
+	other.store = nullptr;
+}
+
+ConstStorageAccessor& ConstStorageAccessor::operator=(
+		ConstStorageAccessor&& other) {
+	constDataPointer = other.constDataPointer;
+	storeId = other.storeId;
+	store = other.store;
+	size_ = other.size_;
+	deleteData = other.deleteData;
+	this->store = other.store;
+	// This prevents premature deletion
+	other.store = nullptr;
+	return *this;
+}
+
+const uint8_t* ConstStorageAccessor::data() const {
+	return constDataPointer;
+}
+
+size_t ConstStorageAccessor::size() const {
+	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
+	}
+	return size_;
+}
+
+ReturnValue_t ConstStorageAccessor::getDataCopy(uint8_t *pointer,
+		size_t maxSize) {
+	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	if(size_ > maxSize) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "StorageAccessor: Supplied buffer not large enough"
+				<< std::endl;
+#endif
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	std::copy(constDataPointer, constDataPointer + size_, pointer);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void ConstStorageAccessor::release() {
+	deleteData = false;
+}
+
+store_address_t ConstStorageAccessor::getId() const {
+	return storeId;
+}
+
+void ConstStorageAccessor::print() const {
+	if(internalState == AccessState::UNINIT or constDataPointer == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
+		return;
+	}
+	arrayprinter::print(constDataPointer, size_);
+}
+
+void ConstStorageAccessor::assignStore(StorageManagerIF* store) {
+	internalState = AccessState::ASSIGNED;
+	this->store = store;
+}

src/core/storagemanager/LocalPool.cpp

@@ -0,0 +1,370 @@
+#include "LocalPool.h"
+#include "FSFWConfig.h"
+
+#include "../objectmanager/ObjectManager.h"
+
+#include <cstring>
+
+LocalPool::LocalPool(object_id_t setObjectId, const LocalPoolConfig& poolConfig,
+		bool registered, bool spillsToHigherPools):
+		SystemObject(setObjectId, registered),
+		NUMBER_OF_SUBPOOLS(poolConfig.size()),
+		spillsToHigherPools(spillsToHigherPools) {
+	if(NUMBER_OF_SUBPOOLS == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "LocalPool::LocalPool: Passed pool configuration is "
+				<< " invalid!" << std::endl;
+#endif
+	}
+	max_subpools_t index = 0;
+	for (const auto& currentPoolConfig: poolConfig) {
+		this->numberOfElements[index] = currentPoolConfig.first;
+		this->elementSizes[index] = currentPoolConfig.second;
+		store[index] = std::vector<uint8_t>(
+				numberOfElements[index] * elementSizes[index]);
+		sizeLists[index] = std::vector<size_type>(numberOfElements[index]);
+		for(auto& size: sizeLists[index]) {
+			size = STORAGE_FREE;
+		}
+		index++;
+	}
+}
+
+LocalPool::~LocalPool(void) {}
+
+
+ReturnValue_t LocalPool::addData(store_address_t* storageId,
+        const uint8_t* data, size_t size, bool ignoreFault) {
+    ReturnValue_t status = reserveSpace(size, storageId, ignoreFault);
+    if (status == RETURN_OK) {
+        write(*storageId, data, size);
+    }
+    return status;
+}
+
+ReturnValue_t LocalPool::getData(store_address_t packetId,
+        const uint8_t **packetPtr, size_t *size) {
+    uint8_t* tempData = nullptr;
+    ReturnValue_t status = modifyData(packetId, &tempData, size);
+    *packetPtr = tempData;
+    return status;
+}
+
+ReturnValue_t LocalPool::getData(store_address_t storeId,
+        ConstStorageAccessor& storeAccessor) {
+    uint8_t* tempData = nullptr;
+    ReturnValue_t status = modifyData(storeId, &tempData,
+            &storeAccessor.size_);
+    storeAccessor.assignStore(this);
+    storeAccessor.constDataPointer = tempData;
+    return status;
+}
+
+ConstAccessorPair LocalPool::getData(store_address_t storeId) {
+    uint8_t* tempData = nullptr;
+    ConstStorageAccessor constAccessor(storeId, this);
+    ReturnValue_t status = modifyData(storeId, &tempData, &constAccessor.size_);
+    constAccessor.constDataPointer = tempData;
+    return ConstAccessorPair(status, std::move(constAccessor));
+}
+
+ReturnValue_t LocalPool::getFreeElement(store_address_t *storageId,
+        const size_t size, uint8_t **pData, bool ignoreFault) {
+    ReturnValue_t status = reserveSpace(size, storageId, ignoreFault);
+    if (status == RETURN_OK) {
+        *pData = &store[storageId->poolIndex][getRawPosition(*storageId)];
+    }
+    else {
+        *pData = nullptr;
+    }
+    return status;
+}
+
+
+AccessorPair LocalPool::modifyData(store_address_t storeId) {
+    StorageAccessor accessor(storeId, this);
+    ReturnValue_t status = modifyData(storeId, &accessor.dataPointer,
+            &accessor.size_);
+    accessor.assignConstPointer();
+    return AccessorPair(status, std::move(accessor));
+}
+
+ReturnValue_t LocalPool::modifyData(store_address_t storeId,
+        StorageAccessor& storeAccessor) {
+    storeAccessor.assignStore(this);
+    ReturnValue_t status = modifyData(storeId, &storeAccessor.dataPointer,
+            &storeAccessor.size_);
+    storeAccessor.assignConstPointer();
+    return status;
+}
+
+ReturnValue_t LocalPool::modifyData(store_address_t storeId,
+        uint8_t **packetPtr, size_t *size) {
+    ReturnValue_t status = RETURN_FAILED;
+    if (storeId.poolIndex >= NUMBER_OF_SUBPOOLS) {
+        return ILLEGAL_STORAGE_ID;
+    }
+    if ((storeId.packetIndex >= numberOfElements[storeId.poolIndex])) {
+        return ILLEGAL_STORAGE_ID;
+    }
+
+    if (sizeLists[storeId.poolIndex][storeId.packetIndex]
+            != STORAGE_FREE) {
+        size_type packetPosition = getRawPosition(storeId);
+        *packetPtr = &store[storeId.poolIndex][packetPosition];
+        *size = sizeLists[storeId.poolIndex][storeId.packetIndex];
+        status = RETURN_OK;
+    }
+    else {
+        status = DATA_DOES_NOT_EXIST;
+    }
+    return status;
+}
+
+ReturnValue_t LocalPool::deleteData(store_address_t storeId) {
+#if FSFW_VERBOSE_PRINTOUT == 2
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::debug << "Delete: Pool: " << std::dec << storeId.poolIndex
+              << " Index: " << storeId.packetIndex << std::endl;
+#endif
+
+#endif
+    ReturnValue_t status = RETURN_OK;
+    size_type pageSize = getSubpoolElementSize(storeId.poolIndex);
+    if ((pageSize != 0) and
+            (storeId.packetIndex < numberOfElements[storeId.poolIndex])) {
+        uint16_t packetPosition = getRawPosition(storeId);
+        uint8_t* ptr = &store[storeId.poolIndex][packetPosition];
+        std::memset(ptr, 0, pageSize);
+        //Set free list
+        sizeLists[storeId.poolIndex][storeId.packetIndex] = STORAGE_FREE;
+    }
+    else {
+        //pool_index or packet_index is too large
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "LocalPool::deleteData: Illegal store ID, no deletion!"
+        		<< std::endl;
+#endif
+        status = ILLEGAL_STORAGE_ID;
+    }
+    return status;
+}
+
+ReturnValue_t LocalPool::deleteData(uint8_t *ptr, size_t size,
+        store_address_t *storeId) {
+    store_address_t localId;
+    ReturnValue_t result = ILLEGAL_ADDRESS;
+    for (uint16_t n = 0; n < NUMBER_OF_SUBPOOLS; n++) {
+        //Not sure if new allocates all stores in order. so better be careful.
+        if ((store[n].data() <= ptr) and
+        		(&store[n][numberOfElements[n]*elementSizes[n]] > ptr)) {
+            localId.poolIndex = n;
+            uint32_t deltaAddress = ptr - store[n].data();
+            // Getting any data from the right "block" is ok.
+            // This is necessary, as IF's sometimes don't point to the first
+            // element of an object.
+            localId.packetIndex = deltaAddress / elementSizes[n];
+            result = deleteData(localId);
+#if FSFW_VERBOSE_PRINTOUT == 2
+            if (deltaAddress % elementSizes[n] != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+                sif::error << "LocalPool::deleteData: Address not aligned!"
+                        << std::endl;
+#endif
+            }
+#endif
+            break;
+        }
+    }
+    if (storeId != nullptr) {
+        *storeId = localId;
+    }
+    return result;
+}
+
+
+ReturnValue_t LocalPool::initialize() {
+    ReturnValue_t result = SystemObject::initialize();
+    if (result != RETURN_OK) {
+        return result;
+    }
+    internalErrorReporter = ObjectManager::instance()->get<InternalErrorReporterIF>(
+            objects::INTERNAL_ERROR_REPORTER);
+    if (internalErrorReporter == nullptr){
+        return ObjectManagerIF::INTERNAL_ERR_REPORTER_UNINIT;
+    }
+
+    //Check if any pool size is large than the maximum allowed.
+    for (uint8_t count = 0; count < NUMBER_OF_SUBPOOLS; count++) {
+        if (elementSizes[count] >= STORAGE_FREE) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "LocalPool::initialize: Pool is too large! "
+                    "Max. allowed size is: " << (STORAGE_FREE - 1) << std::endl;
+#endif
+            return StorageManagerIF::POOL_TOO_LARGE;
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void LocalPool::clearStore() {
+    for(auto& sizeList: sizeLists) {
+        for(auto& size: sizeList) {
+            size = STORAGE_FREE;
+        }
+//        std::memset(sizeList[index], 0xff,
+//                numberOfElements[index] * sizeof(size_type));
+    }
+
+}
+
+ReturnValue_t LocalPool::reserveSpace(const size_t size,
+        store_address_t *storeId, bool ignoreFault) {
+    ReturnValue_t status = getSubPoolIndex(size, &storeId->poolIndex);
+    if (status != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "LocalPool( " << std::hex << getObjectId() << std::dec
+                << " )::reserveSpace: Packet too large." << std::endl;
+#endif
+        return status;
+    }
+    status = findEmpty(storeId->poolIndex, &storeId->packetIndex);
+    while (status != RETURN_OK && spillsToHigherPools) {
+        status = getSubPoolIndex(size, &storeId->poolIndex, storeId->poolIndex + 1);
+        if (status != RETURN_OK) {
+            //We don't find any fitting pool anymore.
+            break;
+        }
+        status = findEmpty(storeId->poolIndex, &storeId->packetIndex);
+    }
+    if (status == RETURN_OK) {
+#if FSFW_VERBOSE_PRINTOUT == 2
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::debug << "Reserve: Pool: " << std::dec
+                << storeId->poolIndex << " Index: " << storeId->packetIndex
+                << std::endl;
+#endif
+#endif
+        sizeLists[storeId->poolIndex][storeId->packetIndex] = size;
+    }
+    else {
+        if ((not ignoreFault) and (internalErrorReporter != nullptr)) {
+            internalErrorReporter->storeFull();
+        }
+    }
+    return status;
+}
+
+void LocalPool::write(store_address_t storeId, const uint8_t *data,
+        size_t size) {
+    uint8_t* ptr = nullptr;
+    size_type packetPosition = getRawPosition(storeId);
+
+    // Size was checked before calling this function.
+    ptr = &store[storeId.poolIndex][packetPosition];
+    std::memcpy(ptr, data, size);
+    sizeLists[storeId.poolIndex][storeId.packetIndex] = size;
+}
+
+LocalPool::size_type LocalPool::getSubpoolElementSize(max_subpools_t subpoolIndex) {
+    if (subpoolIndex < NUMBER_OF_SUBPOOLS) {
+        return elementSizes[subpoolIndex];
+    }
+    else {
+        return 0;
+    }
+}
+
+void LocalPool::setToSpillToHigherPools(bool enable) {
+	this->spillsToHigherPools = enable;
+}
+
+ReturnValue_t LocalPool::getSubPoolIndex(size_t packetSize, uint16_t *subpoolIndex,
+        uint16_t startAtIndex) {
+    for (uint16_t n = startAtIndex; n < NUMBER_OF_SUBPOOLS; n++) {
+#if FSFW_VERBOSE_PRINTOUT == 2
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::debug << "LocalPool " << getObjectId() << "::getPoolIndex: Pool: "
+             << n << ", Element Size: " << elementSizes[n] << std::endl;
+#endif
+#endif
+        if (elementSizes[n] >= packetSize) {
+            *subpoolIndex = n;
+            return RETURN_OK;
+        }
+    }
+    return DATA_TOO_LARGE;
+}
+
+LocalPool::size_type LocalPool::getRawPosition(store_address_t storeId) {
+    return storeId.packetIndex * elementSizes[storeId.poolIndex];
+}
+
+ReturnValue_t LocalPool::findEmpty(n_pool_elem_t poolIndex, uint16_t *element) {
+    ReturnValue_t status = DATA_STORAGE_FULL;
+    for (uint16_t foundElement = 0; foundElement < numberOfElements[poolIndex];
+            foundElement++) {
+        if (sizeLists[poolIndex][foundElement] == STORAGE_FREE) {
+            *element = foundElement;
+            status = RETURN_OK;
+            break;
+        }
+    }
+    return status;
+}
+
+size_t LocalPool::getTotalSize(size_t* additionalSize) {
+    size_t totalSize = 0;
+    size_t sizesSize = 0;
+    for(uint8_t idx = 0; idx < NUMBER_OF_SUBPOOLS; idx ++) {
+        totalSize += elementSizes[idx] * numberOfElements[idx];
+        sizesSize += numberOfElements[idx] * sizeof(size_type);
+    }
+    if(additionalSize != nullptr) {
+        *additionalSize = sizesSize;
+    }
+    return totalSize;
+}
+
+void LocalPool::getFillCount(uint8_t *buffer, uint8_t *bytesWritten) {
+	if(bytesWritten == nullptr or buffer == nullptr) {
+		return;
+	}
+
+	uint16_t reservedHits = 0;
+	uint8_t idx = 0;
+	uint16_t sum = 0;
+	for(; idx < NUMBER_OF_SUBPOOLS; idx ++) {
+		for(const auto& size: sizeLists[idx]) {
+			if(size != STORAGE_FREE) {
+				reservedHits++;
+			}
+		}
+		buffer[idx] = static_cast<float>(reservedHits) /
+				numberOfElements[idx] * 100;
+		*bytesWritten += 1;
+		sum += buffer[idx];
+		reservedHits = 0;
+	}
+	buffer[idx] = sum / NUMBER_OF_SUBPOOLS;
+	*bytesWritten += 1;
+}
+
+
+void LocalPool::clearSubPool(max_subpools_t  subpoolIndex) {
+	if(subpoolIndex >= NUMBER_OF_SUBPOOLS) {
+		return;
+	}
+
+	// Mark the storage as free
+	for(auto& size: sizeLists[subpoolIndex]) {
+		size = STORAGE_FREE;
+	}
+
+	// Set all the page content to 0.
+	std::memset(store[subpoolIndex].data(), 0, elementSizes[subpoolIndex]);
+}
+
+LocalPool::max_subpools_t LocalPool::getNumberOfSubPools() const {
+    return NUMBER_OF_SUBPOOLS;
+}

src/core/storagemanager/PoolManager.cpp

@@ -0,0 +1,62 @@
+#include "PoolManager.h"
+#include <FSFWConfig.h>
+
+PoolManager::PoolManager(object_id_t setObjectId,
+        const LocalPoolConfig& localPoolConfig):
+        LocalPool(setObjectId, localPoolConfig, true) {
+    mutex = MutexFactory::instance()->createMutex();
+}
+
+
+PoolManager::~PoolManager(void) {
+    MutexFactory::instance()->deleteMutex(mutex);
+}
+
+
+ReturnValue_t PoolManager::reserveSpace(const size_t size,
+        store_address_t* address, bool ignoreFault) {
+    MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING,
+            mutexTimeoutMs);
+    ReturnValue_t status = LocalPool::reserveSpace(size,
+            address,ignoreFault);
+    return status;
+}
+
+
+ReturnValue_t PoolManager::deleteData(
+        store_address_t storeId) {
+#if FSFW_VERBOSE_PRINTOUT == 2
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+     sif::debug << "PoolManager( " << translateObject(getObjectId()) <<
+           " )::deleteData from store " << storeId.poolIndex <<
+           ". id is "<< storeId.packetIndex << std::endl;
+#endif
+#endif
+    MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING,
+            mutexTimeoutMs);
+    return LocalPool::deleteData(storeId);
+}
+
+
+ReturnValue_t PoolManager::deleteData(uint8_t* buffer,
+        size_t size, store_address_t* storeId) {
+    MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, 20);
+    ReturnValue_t status = LocalPool::deleteData(buffer,
+            size, storeId);
+    return status;
+}
+
+
+void PoolManager::setMutexTimeout(
+        uint32_t mutexTimeoutMs) {
+    this->mutexTimeoutMs = mutexTimeoutMs;
+}
+
+ReturnValue_t PoolManager::lockMutex(MutexIF::TimeoutType timeoutType,
+		uint32_t timeoutMs) {
+	return mutex->lockMutex(timeoutType, timeoutMs);
+}
+
+ReturnValue_t PoolManager::unlockMutex() {
+	return mutex->unlockMutex();
+}

src/core/storagemanager/StorageAccessor.cpp

@@ -0,0 +1,79 @@
+#include "StorageAccessor.h"
+#include "StorageManagerIF.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+#include <algorithm>
+
+StorageAccessor::StorageAccessor(store_address_t storeId):
+		ConstStorageAccessor(storeId) {
+}
+
+StorageAccessor::StorageAccessor(store_address_t storeId,
+		StorageManagerIF* store):
+		ConstStorageAccessor(storeId, store) {
+}
+
+StorageAccessor& StorageAccessor::operator =(
+		StorageAccessor&& other) {
+	// Call the parent move assignment and also assign own member.
+	dataPointer = other.dataPointer;
+	StorageAccessor::operator=(std::move(other));
+	return * this;
+}
+
+// Call the parent move ctor and also transfer own member.
+StorageAccessor::StorageAccessor(StorageAccessor&& other):
+		ConstStorageAccessor(std::move(other)), dataPointer(other.dataPointer) {
+}
+
+ReturnValue_t StorageAccessor::getDataCopy(uint8_t *pointer, size_t maxSize) {
+	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	if(size_ > maxSize) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "StorageAccessor: Supplied buffer not large "
+				"enough" << std::endl;
+#endif
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	std::copy(dataPointer, dataPointer + size_, pointer);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+uint8_t* StorageAccessor::data() {
+	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
+	}
+	return dataPointer;
+}
+
+ReturnValue_t StorageAccessor::write(uint8_t *data, size_t size,
+		uint16_t offset) {
+	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	if(offset + size > size_) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "StorageAccessor: Data too large for pool "
+				"entry!" << std::endl;
+#endif
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	std::copy(data, data + size, dataPointer + offset);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void StorageAccessor::assignConstPointer() {
+	constDataPointer = dataPointer;
+}
+
+

src/core/subsystem/CMakeLists.txt

@@ -0,0 +1,7 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		Subsystem.cpp
+		SubsystemBase.cpp
+)
+
+add_subdirectory(modes)

src/core/subsystem/Subsystem.cpp

@@ -0,0 +1,661 @@
+#include "Subsystem.h"
+
+#include "../health/HealthMessage.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../serialize/SerialArrayListAdapter.h"
+#include "../serialize/SerialFixedArrayListAdapter.h"
+#include "../serialize/SerializeElement.h"
+#include "../serialize/SerialLinkedListAdapter.h"
+
+#include <string>
+
+Subsystem::Subsystem(object_id_t setObjectId, object_id_t parent,
+		uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) :
+		SubsystemBase(setObjectId, parent, 0), isInTransition(false),
+		childrenChangedHealth(false), currentTargetTable(),
+		targetSubmode(SUBMODE_NONE), currentSequenceIterator(),
+		modeTables(maxNumberOfTables), modeSequences(maxNumberOfSequences) {}
+
+Subsystem::~Subsystem() {}
+
+ReturnValue_t Subsystem::checkSequence(HybridIterator<ModeListEntry> iter,
+		Mode_t fallbackSequence) {
+
+	//only check for existence, checking the fallback would lead to a (possibly infinite) recursion.
+	//the fallback sequence will be checked when it is needed.
+	if (!existsModeSequence(fallbackSequence)) {
+		return FALLBACK_SEQUENCE_DOES_NOT_EXIST;
+	}
+
+	if (iter.value == NULL) {
+		return NO_TARGET_TABLE;
+	}
+
+	for (; iter.value != NULL; ++iter) {
+		if (!existsModeTable(iter->getTableId())) {
+			return TABLE_DOES_NOT_EXIST;
+		} else {
+			ReturnValue_t result = checkTable(getTable(iter->getTableId()));
+			if (result != RETURN_OK) {
+				return result;
+			}
+		}
+	}
+	return RETURN_OK;
+}
+
+ReturnValue_t Subsystem::checkSequence(Mode_t sequence) {
+	if (!existsModeSequence(sequence)) {
+		return SEQUENCE_DOES_NOT_EXIST;
+	}
+	HybridIterator<ModeListEntry> iter = getSequence(sequence);
+	return checkSequence(iter, getFallbackSequence(sequence));
+}
+
+bool Subsystem::existsModeSequence(Mode_t id) {
+	return modeSequences.exists(id) == RETURN_OK;
+}
+
+bool Subsystem::existsModeTable(Mode_t id) {
+	return modeTables.exists(id) == RETURN_OK;
+}
+
+HybridIterator<ModeListEntry> Subsystem::getCurrentTable() {
+	return getTable(currentSequenceIterator->getTableId());
+}
+
+void Subsystem::performChildOperation() {
+	if (isInTransition) {
+		if (commandsOutstanding <= 0) { //all children of the current table were commanded and replied
+			if (currentSequenceIterator.value == NULL) { //we're through with this sequence
+				if (checkStateAgainstTable(currentTargetTable, targetSubmode)
+						== RETURN_OK) {
+					setMode(targetMode, targetSubmode);
+					isInTransition = false;
+					return;
+				} else {
+					transitionFailed(TARGET_TABLE_NOT_REACHED,
+							getSequence(targetMode)->getTableId());
+					return;
+				}
+			}
+			if (currentSequenceIterator->checkSuccess()) {
+				if (checkStateAgainstTable(getCurrentTable(), targetSubmode)
+						!= RETURN_OK) {
+					transitionFailed(TABLE_CHECK_FAILED,
+							currentSequenceIterator->getTableId());
+					return;
+				}
+			}
+			if (currentSequenceIterator->getWaitSeconds() != 0) {
+				if (uptimeStartTable == 0) {
+					Clock::getUptime(&uptimeStartTable);
+					return;
+				} else {
+					uint32_t uptimeNow;
+					Clock::getUptime(&uptimeNow);
+					if ((uptimeNow - uptimeStartTable)
+							< (currentSequenceIterator->getWaitSeconds() * 1000)) {
+						return;
+					}
+				}
+			}
+			uptimeStartTable = 0;
+			//next Table, but only if there is one
+			if ((++currentSequenceIterator).value != NULL) { //we're through with this sequence
+				executeTable(getCurrentTable(), targetSubmode);
+			}
+		}
+	} else {
+		if (childrenChangedHealth) {
+			triggerEvent(CHILD_CHANGED_HEALTH, 0, 0);
+			childrenChangedHealth = false;
+			startTransition(mode, submode);
+		} else if (childrenChangedMode) {
+			if (checkStateAgainstTable(currentTargetTable, submode)
+					!= RETURN_OK) {
+				triggerEvent(CANT_KEEP_MODE, mode, submode);
+				cantKeepMode();
+			}
+		}
+	}
+}
+
+HybridIterator<ModeListEntry> Subsystem::getSequence(Mode_t id) {
+	SequenceInfo *sequenceInfo = modeSequences.findValue(id);
+	if (sequenceInfo->entries.islinked) {
+		return HybridIterator<ModeListEntry>(
+				sequenceInfo->entries.firstLinkedElement);
+	} else {
+		return HybridIterator<ModeListEntry>(
+				sequenceInfo->entries.array->front(),
+				sequenceInfo->entries.array->back());
+	}
+}
+
+HybridIterator<ModeListEntry> Subsystem::getTable(Mode_t id) {
+	EntryPointer *entry = modeTables.findValue(id);
+	if (entry->islinked) {
+		return HybridIterator<ModeListEntry>(entry->firstLinkedElement);
+	} else {
+		return HybridIterator<ModeListEntry>(entry->array->front(),
+				entry->array->back());
+	}
+}
+
+ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) {
+	switch (message->getCommand()) {
+	case HealthMessage::HEALTH_INFO: {
+		HealthState health = HealthMessage::getHealth(message);
+		if (health != EXTERNAL_CONTROL) {
+			//Ignore external control, as it has an effect only if the mode changes,
+			//which is communicated with an additional mode info event.
+			childrenChangedHealth = true;
+		}
+	}
+		break;
+	case ModeSequenceMessage::ADD_SEQUENCE: {
+		FixedArrayList<ModeListEntry, MAX_LENGTH_OF_TABLE_OR_SEQUENCE> sequence;
+		const uint8_t *pointer;
+		size_t sizeRead;
+		ReturnValue_t result = IPCStore->getData(
+				ModeSequenceMessage::getStoreAddress(message), &pointer,
+				&sizeRead);
+		if (result == RETURN_OK) {
+			Mode_t fallbackId;
+			size_t size = sizeRead;
+			result = SerializeAdapter::deSerialize(&fallbackId, &pointer, &size,
+					SerializeIF::Endianness::BIG);
+			if (result == RETURN_OK) {
+				result = SerialArrayListAdapter<ModeListEntry>::deSerialize(
+						&sequence, &pointer, &size,
+						SerializeIF::Endianness::BIG);
+				if (result == RETURN_OK) {
+					result = addSequence(&sequence,
+							ModeSequenceMessage::getSequenceId(message),
+							fallbackId);
+				}
+			}
+			IPCStore->deleteData(ModeSequenceMessage::getStoreAddress(message));
+		}
+		replyToCommand(result, 0);
+	}
+		break;
+	case ModeSequenceMessage::ADD_TABLE: {
+		FixedArrayList<ModeListEntry, MAX_LENGTH_OF_TABLE_OR_SEQUENCE> table;
+		const uint8_t *pointer;
+		size_t sizeRead;
+		ReturnValue_t result = IPCStore->getData(
+				ModeSequenceMessage::getStoreAddress(message), &pointer,
+				&sizeRead);
+		if (result == RETURN_OK) {
+			size_t size = sizeRead;
+			result = SerialArrayListAdapter<ModeListEntry>::deSerialize(&table,
+					&pointer, &size, SerializeIF::Endianness::BIG);
+			if (result == RETURN_OK) {
+				result = addTable(&table,
+						ModeSequenceMessage::getSequenceId(message));
+			}
+			IPCStore->deleteData(ModeSequenceMessage::getStoreAddress(message));
+		}
+		replyToCommand(result, 0);
+
+	}
+		break;
+	case ModeSequenceMessage::DELETE_SEQUENCE:{
+		if (isInTransition) {
+			replyToCommand(IN_TRANSITION, 0);
+			break;
+		}
+		ReturnValue_t result = deleteSequence(ModeSequenceMessage::getSequenceId(message));
+		replyToCommand(result, 0);
+	}
+		break;
+	case ModeSequenceMessage::DELETE_TABLE:{
+		if (isInTransition) {
+			replyToCommand(IN_TRANSITION, 0);
+			break;
+		}
+		ReturnValue_t result = deleteTable(ModeSequenceMessage::getTableId(message));
+		replyToCommand(result, 0);
+	}
+		break;
+	case ModeSequenceMessage::LIST_SEQUENCES: {
+		SerialFixedArrayListAdapter<Mode_t, MAX_NUMBER_OF_TABLES_OR_SEQUENCES> sequences;
+		FixedMap<Mode_t, SequenceInfo>::Iterator iter;
+		for (iter = modeSequences.begin(); iter != modeSequences.end();
+				++iter) {
+			sequences.insert(iter.value->first);
+		}
+		SerializeIF *pointer = &sequences;
+		sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE_LIST,
+				&pointer, 1);
+	}
+		break;
+	case ModeSequenceMessage::LIST_TABLES: {
+		SerialFixedArrayListAdapter<Mode_t, MAX_NUMBER_OF_TABLES_OR_SEQUENCES> tables;
+		FixedMap<Mode_t, EntryPointer>::Iterator iter;
+		for (iter = modeTables.begin(); iter != modeTables.end(); ++iter) {
+			tables.insert(iter.value->first);
+		}
+		SerializeIF *pointer = &tables;
+		sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE_LIST,
+				&pointer, 1);
+	}
+		break;
+	case ModeSequenceMessage::READ_SEQUENCE: {
+		ReturnValue_t result;
+		Mode_t sequence = ModeSequenceMessage::getSequenceId(message);
+		SequenceInfo *sequenceInfo = NULL;
+		result = modeSequences.find(sequence, &sequenceInfo);
+		if (result != RETURN_OK) {
+			replyToCommand(result, 0);
+		}
+
+		SerializeIF *elements[3];
+		SerializeElement<Mode_t> sequenceId(sequence);
+		SerializeElement<Mode_t> fallbackSequenceId(
+				getFallbackSequence(sequence));
+
+		elements[0] = &sequenceId;
+		elements[1] = &fallbackSequenceId;
+
+		if (sequenceInfo->entries.islinked) {
+			SerialLinkedListAdapter<ModeListEntry> list(
+					sequenceInfo->entries.firstLinkedElement, true);
+			elements[2] = &list;
+			sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE,
+					elements, 3);
+		} else {
+			SerialArrayListAdapter<ModeListEntry> serializableArray(
+					sequenceInfo->entries.array);
+
+			elements[2] = &serializableArray;
+			sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE,
+					elements, 3);
+		}
+	}
+		break;
+	case ModeSequenceMessage::READ_TABLE: {
+		ReturnValue_t result;
+		Mode_t table = ModeSequenceMessage::getSequenceId(message);
+		EntryPointer *entry = NULL;
+		result = modeTables.find(table, &entry);
+		if (result != RETURN_OK) {
+			replyToCommand(result, 0);
+		}
+
+		SerializeIF *elements[2];
+		SerializeElement<Mode_t> tableId(table);
+
+		elements[0] = &tableId;
+
+		if (entry->islinked) {
+			SerialLinkedListAdapter<ModeListEntry> list(
+					entry->firstLinkedElement, true);
+			elements[1] = &list;
+			sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE,
+					elements, 2);
+		} else {
+			SerialArrayListAdapter<ModeListEntry> serializableArray(
+					entry->array);
+			elements[1] = &serializableArray;
+			sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE,
+					elements, 2);
+		}
+	}
+		break;
+	case ModeSequenceMessage::READ_FREE_SEQUENCE_SLOTS: {
+		uint32_t freeSlots = modeSequences.maxSize() - modeSequences.size();
+		CommandMessage reply;
+		ModeSequenceMessage::setModeSequenceMessage(&reply,
+				ModeSequenceMessage::FREE_SEQUENCE_SLOTS, freeSlots);
+		commandQueue->reply(&reply);
+	}
+		break;
+	case ModeSequenceMessage::READ_FREE_TABLE_SLOTS: {
+		uint32_t free = modeTables.maxSize() - modeTables.size();
+		CommandMessage reply;
+		ModeSequenceMessage::setModeSequenceMessage(&reply,
+				ModeSequenceMessage::FREE_TABLE_SLOTS, free);
+		commandQueue->reply(&reply);
+	}
+		break;
+	default:
+		return RETURN_FAILED;
+	}
+	return RETURN_OK;
+}
+
+void Subsystem::replyToCommand(ReturnValue_t status, uint32_t parameter) {
+	if (status == RETURN_OK) {
+		CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, 0);
+		commandQueue->reply(&reply);
+	} else {
+		CommandMessage reply(CommandMessage::REPLY_REJECTED, status, 0);
+		commandQueue->reply(&reply);
+	}
+}
+
+ReturnValue_t Subsystem::addSequence(ArrayList<ModeListEntry> *sequence,
+		Mode_t id, Mode_t fallbackSequence, bool inStore, bool preInit) {
+
+	ReturnValue_t result;
+
+	//Before initialize() is called, tables must not be checked as the
+	//children are not added yet.
+	//Sequences added before are checked by initialize()
+	if (!preInit) {
+		result = checkSequence(
+				HybridIterator<ModeListEntry>(sequence->front(),
+						sequence->back()), fallbackSequence);
+		if (result != RETURN_OK) {
+			return result;
+		}
+	}
+
+	SequenceInfo info;
+
+	info.fallbackSequence = fallbackSequence;
+
+	info.entries.islinked = inStore;
+	info.entries.array = sequence;
+
+	result = modeSequences.insert(id, info);
+
+	if (result != RETURN_OK) {
+		return result;
+	}
+
+	if (inStore) {
+#if FSFW_USE_MODESTORE == 1
+		result = modeStore->storeArray(sequence,
+				&(modeSequences.find(id)->entries.firstLinkedElement));
+		if (result != RETURN_OK) {
+			modeSequences.erase(id);
+		}
+#else
+		modeSequences.erase(id);
+		return RETURN_FAILED;
+#endif
+	}
+
+	return result;
+
+}
+
+ReturnValue_t Subsystem::addTable(ArrayList<ModeListEntry> *table, Mode_t id,
+		bool inStore, bool preInit) {
+
+	ReturnValue_t result;
+
+	//Before initialize() is called, tables must not be checked as the children
+	//are not added yet. Tables added before are checked by initialize()
+	if (!preInit) {
+		result = checkTable(
+				HybridIterator<ModeListEntry>(table->front(), table->back()));
+		if (result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+	}
+
+	EntryPointer pointer;
+
+	pointer.islinked = inStore;
+	pointer.array = table;
+
+	result = modeTables.insert(id, pointer);
+
+	if (result != RETURN_OK) {
+		return result;
+	}
+
+	if (inStore) {
+#if FSFW_USE_MODESTORE == 1
+		result = modeStore->storeArray(table,
+				&(modeTables.find(id)->firstLinkedElement));
+		if (result != RETURN_OK) {
+			modeTables.erase(id);
+		}
+#else
+		modeTables.erase(id);
+		return RETURN_FAILED;
+#endif
+	}
+	return result;
+}
+
+ReturnValue_t Subsystem::deleteSequence(Mode_t id) {
+
+	if (isFallbackSequence(id)) {
+		return IS_FALLBACK_SEQUENCE;
+	}
+
+	SequenceInfo *sequenceInfo;
+	ReturnValue_t result;
+	result = modeSequences.find(id, &sequenceInfo);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	if (!sequenceInfo->entries.islinked) {
+		return ACCESS_DENIED;
+	}
+
+#if FSFW_USE_MODESTORE == 1
+	modeStore->deleteList(sequenceInfo->entries.firstLinkedElement);
+#endif
+	modeSequences.erase(id);
+	return RETURN_OK;
+}
+
+ReturnValue_t Subsystem::deleteTable(Mode_t id) {
+
+	if (isTableUsed(id)) {
+		return TABLE_IN_USE;
+	}
+
+	EntryPointer *pointer;
+	ReturnValue_t result;
+	result = modeTables.find(id, &pointer);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	if (!pointer->islinked) {
+		return ACCESS_DENIED;
+	}
+
+#if FSFW_USE_MODESTORE == 1
+	modeStore->deleteList(pointer->firstLinkedElement);
+#endif
+	modeSequences.erase(id);
+	return RETURN_OK;
+}
+
+ReturnValue_t Subsystem::initialize() {
+	ReturnValue_t result = SubsystemBase::initialize();
+
+	if (result != RETURN_OK) {
+		return result;
+	}
+
+	IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+	if (IPCStore == NULL) {
+		return RETURN_FAILED;
+	}
+
+#if FSFW_USE_MODESTORE == 1
+	modeStore = ObjectManager::instance()->get<ModeStoreIF>(objects::MODE_STORE);
+
+	if (modeStore == nullptr) {
+		return RETURN_FAILED;
+	}
+#endif
+
+	if ((modeSequences.maxSize() > MAX_NUMBER_OF_TABLES_OR_SEQUENCES)
+			|| (modeTables.maxSize() > MAX_NUMBER_OF_TABLES_OR_SEQUENCES)) {
+		return TABLE_OR_SEQUENCE_LENGTH_INVALID;
+	}
+
+	mode = initialMode;
+
+	return RETURN_OK;
+}
+
+MessageQueueId_t Subsystem::getSequenceCommandQueue() const {
+	return SubsystemBase::getCommandQueue();
+}
+
+ReturnValue_t Subsystem::checkModeCommand(Mode_t mode, Submode_t submode,
+		uint32_t *msToReachTheMode) {
+	//Need to accept all submodes to be able to inherit submodes
+//	if (submode != SUBMODE_NONE) {
+//		return INVALID_SUBMODE;
+//	}
+
+	if (isInTransition && (mode != getFallbackSequence(targetMode))) {
+		return HasModesIF::IN_TRANSITION;
+	} else {
+		return checkSequence(mode);
+	}
+}
+
+void Subsystem::startTransition(Mode_t sequence, Submode_t submode) {
+	if (modeHelper.isForced()) {
+		triggerEvent(FORCING_MODE, sequence, submode);
+	} else {
+		triggerEvent(CHANGING_MODE, sequence, submode);
+	}
+	targetMode = sequence;
+	targetSubmode = submode;
+	isInTransition = true;
+	commandsOutstanding = 0;
+	currentSequenceIterator = getSequence(sequence);
+
+	currentTargetTable = getTable(currentSequenceIterator->getTableId());
+
+	++currentSequenceIterator;
+
+	if (currentSequenceIterator.value != NULL) {
+		executeTable(getCurrentTable(), targetSubmode);
+	}
+}
+
+Mode_t Subsystem::getFallbackSequence(Mode_t sequence) {
+	for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
+			iter != modeSequences.end(); ++iter) {
+		if (iter.value->first == sequence) {
+			return iter->second.fallbackSequence;
+		}
+	}
+	return -1;
+}
+
+bool Subsystem::isFallbackSequence(Mode_t SequenceId) {
+	for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
+			iter != modeSequences.end(); iter++) {
+		if (iter->second.fallbackSequence == SequenceId) {
+			return true;
+		}
+	}
+	return false;
+}
+
+bool Subsystem::isTableUsed(Mode_t tableId) {
+	for (FixedMap<Mode_t, SequenceInfo>::Iterator sequence =
+			modeSequences.begin(); sequence != modeSequences.end();
+			sequence++) {
+		HybridIterator<ModeListEntry> sequenceIterator = getSequence(
+				sequence.value->first);
+		while (sequenceIterator.value != NULL) {
+			if (sequenceIterator->getTableId() == tableId) {
+				return true;
+			}
+			++sequenceIterator;
+		}
+	}
+	return false;
+}
+
+void Subsystem::transitionFailed(ReturnValue_t failureCode,
+		uint32_t parameter) {
+	triggerEvent(MODE_TRANSITION_FAILED, failureCode, parameter);
+	if (mode == targetMode) {
+		//already tried going back to the current mode
+		//go into fallback mode, also set current mode to fallback mode,
+	    //so we come here at the next fail
+		modeHelper.setForced(true);
+		ReturnValue_t result;
+		if ((result = checkSequence(getFallbackSequence(mode))) != RETURN_OK) {
+			triggerEvent(FALLBACK_FAILED, result, getFallbackSequence(mode));
+			//keep still and allow arbitrary mode commands to recover
+			isInTransition = false;
+			return;
+		}
+		mode = getFallbackSequence(mode);
+		startTransition(mode, submode);
+	} else {
+		//try to go back to the current mode
+		startTransition(mode, submode);
+	}
+}
+
+void Subsystem::sendSerializablesAsCommandMessage(Command_t command,
+		SerializeIF **elements, uint8_t count) {
+	ReturnValue_t result;
+	size_t maxSize = 0;
+	for (uint8_t i = 0; i < count; i++) {
+		maxSize += elements[i]->getSerializedSize();
+	}
+	uint8_t *storeBuffer;
+	store_address_t address;
+	size_t size = 0;
+
+	result = IPCStore->getFreeElement(&address, maxSize, &storeBuffer);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		replyToCommand(result, 0);
+		return;
+	}
+	for (uint8_t i = 0; i < count; i++) {
+		elements[i]->serialize(&storeBuffer, &size, maxSize,
+				SerializeIF::Endianness::BIG);
+	}
+	CommandMessage reply;
+	ModeSequenceMessage::setModeSequenceMessage(&reply, command, address);
+	if (commandQueue->reply(&reply) != RETURN_OK) {
+		IPCStore->deleteData(address);
+	}
+}
+
+ReturnValue_t Subsystem::checkObjectConnections() {
+	ReturnValue_t result = RETURN_OK;
+	for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
+			iter != modeSequences.end(); iter++) {
+		result = checkSequence(iter.value->first);
+		if (result != RETURN_OK) {
+			return result;
+		}
+
+	}
+	return RETURN_OK;
+}
+
+void Subsystem::setInitialMode(Mode_t mode) {
+	initialMode = mode;
+}
+
+void Subsystem::cantKeepMode() {
+	ReturnValue_t result;
+	if ((result = checkSequence(getFallbackSequence(mode))) != RETURN_OK) {
+		triggerEvent(FALLBACK_FAILED, result, getFallbackSequence(mode));
+		return;
+	}
+
+	modeHelper.setForced(true);
+
+	//already set the mode, so that we do not try to go back in our old mode
+	//when the transition fails
+	mode = getFallbackSequence(mode);
+	//SHOULDDO: We should store submodes for fallback sequence as well,
+	//otherwise we should get rid of submodes completely.
+	startTransition(mode, SUBMODE_NONE);
+}

src/core/subsystem/SubsystemBase.cpp

@@ -0,0 +1,356 @@
+#include "SubsystemBase.h"
+
+#include "../serviceinterface/ServiceInterface.h"
+#include "../objectmanager/ObjectManager.h"
+#include "../ipc/QueueFactory.h"
+
+SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent,
+		Mode_t initialMode, uint16_t commandQueueDepth) :
+		SystemObject(setObjectId), mode(initialMode),
+		commandQueue(QueueFactory::instance()->createMessageQueue(
+		        commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE)),
+		healthHelper(this, setObjectId), modeHelper(this), parentId(parent) {
+}
+
+SubsystemBase::~SubsystemBase() {
+	QueueFactory::instance()->deleteMessageQueue(commandQueue);
+
+}
+
+ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
+	ChildInfo info;
+
+	HasModesIF *child = ObjectManager::instance()->get<HasModesIF>(objectId);
+	// This is a rather ugly hack to have the changedHealth info for all
+	// children available.
+	HasHealthIF* healthChild = ObjectManager::instance()->get<HasHealthIF>(objectId);
+	if (child == nullptr) {
+		if (healthChild == nullptr) {
+			return CHILD_DOESNT_HAVE_MODES;
+		} else {
+			info.commandQueue = healthChild->getCommandQueue();
+			info.mode = MODE_OFF;
+		}
+	} else {
+		info.commandQueue = child->getCommandQueue();
+		info.mode = -1;	//intentional to force an initial command during system startup
+	}
+
+	info.submode = SUBMODE_NONE;
+	info.healthChanged = false;
+
+	auto resultPair = childrenMap.emplace(objectId, info);
+	if (not resultPair.second) {
+		return COULD_NOT_INSERT_CHILD;
+	}
+	return RETURN_OK;
+}
+
+ReturnValue_t SubsystemBase::checkStateAgainstTable(
+		HybridIterator<ModeListEntry> tableIter, Submode_t targetSubmode) {
+
+	std::map<object_id_t, ChildInfo>::iterator childIter;
+
+	for (; tableIter.value != NULL; ++tableIter) {
+		object_id_t object = tableIter.value->getObject();
+
+		if ((childIter = childrenMap.find(object)) == childrenMap.end()) {
+			return RETURN_FAILED;
+		}
+
+		if (childIter->second.mode != tableIter.value->getMode()) {
+			return RETURN_FAILED;
+		}
+
+		Submode_t submodeToCheckAgainst = tableIter.value->getSubmode();
+		if (tableIter.value->inheritSubmode()) {
+			submodeToCheckAgainst = targetSubmode;
+		}
+
+		if (childIter->second.submode != submodeToCheckAgainst) {
+			return RETURN_FAILED;
+		}
+	}
+	return RETURN_OK;
+}
+
+void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter,
+        Submode_t targetSubmode) {
+	CommandMessage command;
+
+	std::map<object_id_t, ChildInfo>::iterator iter;
+
+	commandsOutstanding = 0;
+
+	for (; tableIter.value != nullptr; ++tableIter) {
+		object_id_t object = tableIter.value->getObject();
+		if ((iter = childrenMap.find(object)) == childrenMap.end()) {
+			//illegal table entry, should only happen due to misconfigured mode table
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::debug << std::hex << getObjectId() << ": invalid mode table entry"
+					<< std::endl;
+#endif
+			continue;
+		}
+
+		Submode_t submodeToCommand = tableIter.value->getSubmode();
+		if (tableIter.value->inheritSubmode()) {
+			submodeToCommand = targetSubmode;
+		}
+
+		if (healthHelper.healthTable->hasHealth(object)) {
+			if (healthHelper.healthTable->isFaulty(object)) {
+				ModeMessage::setModeMessage(&command,
+						ModeMessage::CMD_MODE_COMMAND, HasModesIF::MODE_OFF,
+						SUBMODE_NONE);
+			} else {
+				if (modeHelper.isForced()) {
+					ModeMessage::setModeMessage(&command,
+							ModeMessage::CMD_MODE_COMMAND_FORCED,
+							tableIter.value->getMode(), submodeToCommand);
+				} else {
+					if (healthHelper.healthTable->isCommandable(object)) {
+						ModeMessage::setModeMessage(&command,
+								ModeMessage::CMD_MODE_COMMAND,
+								tableIter.value->getMode(), submodeToCommand);
+					} else {
+						continue;
+					}
+				}
+			}
+		} else {
+			ModeMessage::setModeMessage(&command, ModeMessage::CMD_MODE_COMMAND,
+					tableIter.value->getMode(), submodeToCommand);
+		}
+
+		if ((iter->second.mode == ModeMessage::getMode(&command))
+				&& (iter->second.submode == ModeMessage::getSubmode(&command))
+				&& !modeHelper.isForced()) {
+			continue; //don't send redundant mode commands (produces event spam), but still command if mode is forced to reach lower levels
+		}
+		ReturnValue_t result = commandQueue->sendMessage(
+				iter->second.commandQueue, &command);
+		if (result == RETURN_OK) {
+			++commandsOutstanding;
+		}
+	}
+
+}
+
+ReturnValue_t SubsystemBase::updateChildMode(MessageQueueId_t queue,
+		Mode_t mode, Submode_t submode) {
+	std::map<object_id_t, ChildInfo>::iterator iter;
+
+	for (iter = childrenMap.begin(); iter != childrenMap.end(); iter++) {
+		if (iter->second.commandQueue == queue) {
+			iter->second.mode = mode;
+			iter->second.submode = submode;
+			return RETURN_OK;
+		}
+	}
+	return CHILD_NOT_FOUND;
+}
+
+ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue,
+bool changedHealth) {
+	for (auto iter = childrenMap.begin(); iter != childrenMap.end(); iter++) {
+		if (iter->second.commandQueue == queue) {
+			iter->second.healthChanged = changedHealth;
+			return RETURN_OK;
+		}
+	}
+	return CHILD_NOT_FOUND;
+}
+
+MessageQueueId_t SubsystemBase::getCommandQueue() const {
+	return commandQueue->getId();
+}
+
+ReturnValue_t SubsystemBase::initialize() {
+	MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE;
+	ReturnValue_t result = SystemObject::initialize();
+
+	if (result != RETURN_OK) {
+		return result;
+	}
+
+	if (parentId != objects::NO_OBJECT) {
+		SubsystemBase *parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
+		if (parent == nullptr) {
+			return RETURN_FAILED;
+		}
+		parentQueue = parent->getCommandQueue();
+
+		parent->registerChild(getObjectId());
+	}
+
+	result = healthHelper.initialize(parentQueue);
+
+	if (result != RETURN_OK) {
+		return result;
+	}
+
+	result = modeHelper.initialize(parentQueue);
+
+	if (result != RETURN_OK) {
+		return result;
+	}
+
+	return RETURN_OK;
+}
+
+ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) {
+
+	childrenChangedMode = false;
+
+	checkCommandQueue();
+
+	performChildOperation();
+
+	return RETURN_OK;
+}
+
+ReturnValue_t SubsystemBase::handleModeReply(CommandMessage* message) {
+	switch (message->getCommand()) {
+	case ModeMessage::REPLY_MODE_INFO:
+		updateChildMode(message->getSender(), ModeMessage::getMode(message),
+				ModeMessage::getSubmode(message));
+		childrenChangedMode = true;
+		return RETURN_OK;
+	case ModeMessage::REPLY_MODE_REPLY:
+	case ModeMessage::REPLY_WRONG_MODE_REPLY:
+		updateChildMode(message->getSender(), ModeMessage::getMode(message),
+				ModeMessage::getSubmode(message));
+		childrenChangedMode = true;
+		commandsOutstanding--;
+		return RETURN_OK;
+	case ModeMessage::REPLY_CANT_REACH_MODE:
+		commandsOutstanding--;
+		{
+			for (auto iter = childrenMap.begin(); iter != childrenMap.end();
+					iter++) {
+				if (iter->second.commandQueue == message->getSender()) {
+					triggerEvent(MODE_CMD_REJECTED, iter->first,
+							message->getParameter());
+				}
+			}
+		}
+		return RETURN_OK;
+//	case ModeMessage::CMD_MODE_COMMAND:
+//		handleCommandedMode(message);
+//		return RETURN_OK;
+//	case ModeMessage::CMD_MODE_ANNOUNCE:
+//		triggerEvent(MODE_INFO, mode, submode);
+//		return RETURN_OK;
+//	case ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY:
+//		triggerEvent(MODE_INFO, mode, submode);
+//		commandAllChildren(message);
+//		return RETURN_OK;
+	default:
+		return RETURN_FAILED;
+	}
+}
+
+ReturnValue_t SubsystemBase::checkTable(
+		HybridIterator<ModeListEntry> tableIter) {
+	for (; tableIter.value != NULL; ++tableIter) {
+		if (childrenMap.find(tableIter.value->getObject())
+				== childrenMap.end()) {
+			return TABLE_CONTAINS_INVALID_OBJECT_ID;
+		}
+	}
+	return RETURN_OK;
+}
+
+void SubsystemBase::replyToCommand(CommandMessage* message) {
+	commandQueue->reply(message);
+}
+
+void SubsystemBase::setMode(Mode_t newMode, Submode_t newSubmode) {
+	modeHelper.modeChanged(newMode, newSubmode);
+	mode = newMode;
+	submode = newSubmode;
+	modeChanged();
+	announceMode(false);
+}
+
+void SubsystemBase::setMode(Mode_t newMode) {
+	setMode(newMode, submode);
+}
+
+void SubsystemBase::commandAllChildren(CommandMessage* message) {
+	std::map<object_id_t, ChildInfo>::iterator iter;
+	for (iter = childrenMap.begin(); iter != childrenMap.end(); ++iter) {
+		commandQueue->sendMessage(iter->second.commandQueue, message);
+	}
+}
+
+void SubsystemBase::getMode(Mode_t* mode, Submode_t* submode) {
+	*mode = this->mode;
+	*submode = this->submode;
+}
+
+void SubsystemBase::setToExternalControl() {
+	healthHelper.setHealth(EXTERNAL_CONTROL);
+}
+
+void SubsystemBase::announceMode(bool recursive) {
+	triggerEvent(MODE_INFO, mode, submode);
+	if (recursive) {
+		CommandMessage command;
+		ModeMessage::setModeMessage(&command,
+				ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY, 0, 0);
+		commandAllChildren(&command);
+	}
+}
+
+void SubsystemBase::checkCommandQueue() {
+	ReturnValue_t result;
+	CommandMessage command;
+
+	for (result = commandQueue->receiveMessage(&command); result == RETURN_OK;
+			result = commandQueue->receiveMessage(&command)) {
+
+		result = healthHelper.handleHealthCommand(&command);
+		if (result == RETURN_OK) {
+			continue;
+		}
+
+		result = modeHelper.handleModeCommand(&command);
+		if (result == RETURN_OK) {
+			continue;
+		}
+
+		result = handleModeReply(&command);
+		if (result == RETURN_OK) {
+			continue;
+		}
+
+		result = handleCommandMessage(&command);
+		if (result != RETURN_OK) {
+			CommandMessage reply;
+			reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
+					command.getCommand());
+			replyToCommand(&reply);
+		}
+	}
+}
+
+ReturnValue_t SubsystemBase::setHealth(HealthState health) {
+	switch (health) {
+	case HEALTHY:
+	case EXTERNAL_CONTROL:
+		healthHelper.setHealth(health);
+		return RETURN_OK;
+	default:
+		return INVALID_HEALTH_STATE;
+	}
+}
+
+HasHealthIF::HealthState SubsystemBase::getHealth() {
+	return healthHelper.getHealth();
+}
+
+void SubsystemBase::modeChanged() {
+}
+

src/core/subsystem/modes/CMakeLists.txt

@@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		ModeSequenceMessage.cpp
+		ModeStore.cpp
+)

src/core/subsystem/modes/ModeSequenceMessage.cpp

@@ -0,0 +1,76 @@
+#include "ModeSequenceMessage.h"
+
+#include "../../objectmanager/ObjectManager.h"
+#include "../../storagemanager/StorageManagerIF.h"
+
+void ModeSequenceMessage::setModeSequenceMessage(CommandMessage* message,
+		Command_t command, Mode_t sequence, store_address_t storeAddress) {
+	message->setCommand(command);
+	message->setParameter(storeAddress.raw);
+	message->setParameter2(sequence);
+}
+
+void ModeSequenceMessage::setModeSequenceMessage(CommandMessage* message,
+		Command_t command, Mode_t sequence) {
+	message->setCommand(command);
+	message->setParameter2(sequence);
+}
+
+void ModeSequenceMessage::setModeSequenceMessage(CommandMessage* message,
+		Command_t command, store_address_t storeAddress) {
+	message->setCommand(command);
+	message->setParameter(storeAddress.raw);
+}
+
+store_address_t ModeSequenceMessage::getStoreAddress(
+		const CommandMessage* message) {
+	store_address_t address;
+	address.raw = message->getParameter();
+	return address;
+}
+
+Mode_t ModeSequenceMessage::getSequenceId(const CommandMessage* message) {
+	return message->getParameter2();
+}
+
+Mode_t ModeSequenceMessage::getTableId(const CommandMessage* message) {
+	return message->getParameter2();
+}
+
+
+uint32_t ModeSequenceMessage::getNumber(const CommandMessage* message) {
+	return message->getParameter2();
+}
+
+void ModeSequenceMessage::clear(CommandMessage *message) {
+	switch (message->getCommand()) {
+	case ADD_SEQUENCE:
+	case ADD_TABLE:
+	case SEQUENCE_LIST:
+	case TABLE_LIST:
+	case TABLE:
+	case SEQUENCE: {
+		StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
+		        objects::IPC_STORE);
+		if (ipcStore != nullptr){
+			ipcStore->deleteData(ModeSequenceMessage::getStoreAddress(message));
+		}
+	}
+	/* NO BREAK falls through*/
+	case DELETE_SEQUENCE:
+	case DELETE_TABLE:
+	case READ_SEQUENCE:
+	case READ_TABLE:
+	case LIST_SEQUENCES:
+	case LIST_TABLES:
+	case READ_FREE_SEQUENCE_SLOTS:
+	case FREE_SEQUENCE_SLOTS:
+	case READ_FREE_TABLE_SLOTS:
+	case FREE_TABLE_SLOTS:
+	default:
+		message->setCommand(CommandMessage::CMD_NONE);
+		message->setParameter(0);
+		message->setParameter2(0);
+		break;
+	}
+}

src/core/subsystem/modes/ModeStore.cpp

@@ -0,0 +1,128 @@
+#include "ModeStore.h"
+
+// todo: I think some parts are deprecated. If this is used,  the define
+// USE_MODESTORE could be part of the new FSFWConfig.h file.
+#if FSFW_USE_MODESTORE == 1
+
+ModeStore::ModeStore(object_id_t objectId, uint32_t slots) :
+		SystemObject(objectId), store(slots), emptySlot(store.front()) {
+	mutex = MutexFactory::instance()->createMutex();;
+	OSAL::createMutex(objectId + 1, mutex);
+	clear();
+}
+
+ModeStore::~ModeStore() {
+	delete mutex;
+}
+
+uint32_t ModeStore::getFreeSlots() {
+	OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
+	uint32_t count = 0;
+	ArrayList<ModeListEntry, uint32_t>::Iterator iter;
+	for (iter = store.begin(); iter != store.end(); ++iter) {
+		if (iter->getNext() == emptySlot) {
+			++count;
+		}
+	}
+	OSAL::unlockMutex(mutex);
+	return count;
+}
+
+ReturnValue_t ModeStore::storeArray(ArrayList<ModeListEntry>* sequence,
+		ModeListEntry** storedFirstEntry) {
+	if (sequence->size == 0) {
+		return CANT_STORE_EMPTY;
+	}
+	OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
+	*storedFirstEntry = findEmptySlotNoLock(store.front());
+
+	ModeListEntry* pointer =
+			*storedFirstEntry;
+	pointer->setNext(pointer);
+
+	ArrayList<ModeListEntry>::Iterator iter;
+	for (iter = sequence->begin(); iter != sequence->end(); ++iter) {
+		//SHOULDDO: I need to check this in detail. What is the idea? Why does it not work?
+		pointer = pointer->getNext()->value;
+		if (pointer == NULL) {
+			deleteListNoLock(*storedFirstEntry);
+			OSAL::unlockMutex(mutex);
+			return TOO_MANY_ELEMENTS;
+		}
+		pointer->value->value1 = iter->value1;
+		pointer->value->value2 = iter->value2;
+		pointer->value->value3 = iter->value3;
+		pointer->setNext(findEmptySlotNoLock(pointer + 1));
+	}
+	pointer->setNext(NULL);
+	OSAL::unlockMutex(mutex);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t ModeStore::deleteList(ModeListEntry* sequence) {
+	ReturnValue_t result = isValidEntry(sequence);
+		if (result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+		OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
+		deleteListNoLock(sequence);
+		OSAL::unlockMutex(mutex);
+		return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t ModeStore::readList(ModeListEntry* sequence,
+		ArrayList<ModeListEntry>* into) {
+	ReturnValue_t result = isValidEntry(sequence);
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
+	result = into->insert(*sequence->value);
+	while ((result == HasReturnvaluesIF::RETURN_OK) && (sequence->getNext() != NULL)) {
+		result = into->insert(*sequence->value);
+		sequence = sequence->getNext()->value;
+	}
+	OSAL::unlockMutex(mutex);
+	return result;
+}
+
+void ModeStore::clear() {
+	OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
+	store.size = store.maxSize();
+	ArrayList<ModeListEntry, uint32_t>::Iterator iter;
+	for (iter = store.begin(); iter != store.end(); ++iter) {
+		iter->setNext(emptySlot);
+	}
+	OSAL::unlockMutex(mutex);
+}
+
+ModeListEntry* ModeStore::findEmptySlotNoLock(ModeListEntry* startFrom) {
+	ArrayList<ModeListEntry, uint32_t>::Iterator iter(
+			startFrom);
+	for (; iter != store.end(); ++iter) {
+		if (iter.value->getNext() == emptySlot) {
+			OSAL::unlockMutex(mutex);
+			return iter.value;
+		}
+	}
+	return NULL;
+}
+
+void ModeStore::deleteListNoLock(ModeListEntry* sequence) {
+	ModeListEntry* next = sequence;
+	while (next != NULL) {
+		next = sequence->getNext()->value;
+		sequence->setNext(emptySlot);
+		sequence = next;
+	}
+}
+
+ReturnValue_t ModeStore::isValidEntry(ModeListEntry* sequence) {
+	if ((sequence < store.front()) || (sequence > store.back())
+			|| sequence->getNext() == emptySlot) {
+		return INVALID_ENTRY;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+#endif

src/core/tasks/CMakeLists.txt

@@ -0,0 +1,5 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		FixedSequenceSlot.cpp
+		FixedSlotSequence.cpp
+)

src/core/tasks/FixedSequenceSlot.cpp

@@ -0,0 +1,17 @@
+#include "FixedSequenceSlot.h"
+#include "PeriodicTaskIF.h"
+#include <cstddef>
+
+FixedSequenceSlot::FixedSequenceSlot(object_id_t handlerId, uint32_t setTime,
+		int8_t setSequenceId, ExecutableObjectIF* executableObject,
+		PeriodicTaskIF* executingTask) : handlerId(handlerId),
+		pollingTimeMs(setTime), opcode(setSequenceId) {
+    if(executableObject == nullptr) {
+        return;
+    }
+    this->executableObject = executableObject;
+    this->executableObject->setTaskIF(executingTask);
+}
+
+FixedSequenceSlot::~FixedSequenceSlot() {}
+