2023-06-30 15:44:39 +02:00
89 changed files with 1458 additions and 995 deletions
--- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
+++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
@ -375,13 +375,16 @@ float MgmLIS3MDLHandler::getSensitivityFactor(MGMLIS3MDL::Sensitivies sens) {

 ReturnValue_t MgmLIS3MDLHandler::enableTemperatureSensor(const uint8_t *commandData,
                                                         size_t commandDataLen) {
+  if(commandData == nullptr) {
+    return INVALID_COMMAND_PARAMETER;
+  }
  triggerEvent(CHANGE_OF_SETUP_PARAMETER);
  uint32_t size = 2;
  commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1);
  if (commandDataLen > 1) {
    return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
  }
-  switch (*commandData) {
+  switch (commandData[0]) {
    case (MGMLIS3MDL::ON): {
      commandBuffer[1] = registers[0] | (1 << 7);
      break;
--- a/hal/src/fsfw_hal/linux/gpio/Gpio.h
+++ b/hal/src/fsfw_hal/linux/gpio/Gpio.h
@ -0,0 +1,27 @@
+#ifndef FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_
+#define FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_
+
+#include "fsfw_hal/common/gpio/GpioIF.h"
+#include "fsfw_hal/common/gpio/gpioDefinitions.h"
+
+/**
+ * @brief   Additional abstraction layer for handling GPIOs.
+ *
+ * @author  J. Meier
+ */
+class Gpio {
+ public:
+  Gpio(gpioId_t gpioId, GpioIF* gpioIF) : gpioId(gpioId), gpioIF(gpioIF) {
+    if (gpioIF == nullptr) {
+      sif::error << "Gpio::Gpio: Invalid GpioIF" << std::endl;
+    }
+  }
+  ReturnValue_t pullHigh() { return gpioIF->pullHigh(gpioId); }
+  ReturnValue_t pullLow() { return gpioIF->pullLow(gpioId); }
+
+ private:
+  gpioId_t gpioId = gpio::NO_GPIO;
+  GpioIF* gpioIF = nullptr;
+};
+
+#endif /* FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_ */
--- a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp
+++ b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp
@ -2,8 +2,8 @@

 #include <fsfw/serviceinterface.h>

-UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode,
-                       UartBaudRate baudrate, size_t maxReplyLen)
+UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate,
+                       size_t maxReplyLen, UartModes uartMode)
    : handlerId(handlerId),
      deviceFile(deviceFile),
      uartMode(uartMode),
--- a/hal/src/fsfw_hal/linux/uart/UartCookie.h
+++ b/hal/src/fsfw_hal/linux/uart/UartCookie.h
@ -69,8 +69,8 @@ class UartCookie : public CookieIF {
   *                        8 databits (number of bits transfered with one uart frame)
   *                        One stop bit
   */
-  UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode,
-             UartBaudRate baudrate, size_t maxReplyLen);
+  UartCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate,
+             size_t maxReplyLen, UartModes uartMode = UartModes::NON_CANONICAL);

  virtual ~UartCookie();

--- a/src/fsfw/action/CommandActionHelper.h
+++ b/src/fsfw/action/CommandActionHelper.h
@ -16,8 +16,8 @@ class CommandActionHelper {
 public:
  CommandActionHelper(CommandsActionsIF* owner);
  virtual ~CommandActionHelper();
-  ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, const uint8_t* data,
-                              uint32_t size);
+  ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId,
+                              const uint8_t* data = nullptr, uint32_t size = 0);
  ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, SerializeIF* data);
  ReturnValue_t initialize();
  ReturnValue_t handleReply(CommandMessage* reply);
--- a/src/fsfw/cfdp/CFDPHandler.cpp
+++ b/src/fsfw/cfdp/CFDPHandler.cpp
@ -12,7 +12,9 @@ object_id_t CFDPHandler::packetDestination = 0;

 CFDPHandler::CFDPHandler(object_id_t setObjectId, CFDPDistributor* dist)
    : SystemObject(setObjectId) {
-  requestQueue = QueueFactory::instance()->createMessageQueue(CFDP_HANDLER_MAX_RECEPTION);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  requestQueue = QueueFactory::instance()->createMessageQueue(
+      CFDP_HANDLER_MAX_RECEPTION, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
  distributor = dist;
 }

--- a/src/fsfw/container/HybridIterator.h
+++ b/src/fsfw/container/HybridIterator.h
@ -10,16 +10,23 @@ class HybridIterator : public LinkedElement<T>::Iterator, public ArrayList<T, co
  HybridIterator() {}

  HybridIterator(typename LinkedElement<T>::Iterator *iter)
-      : LinkedElement<T>::Iterator(*iter), value(iter->value), linked(true) {}
+      : LinkedElement<T>::Iterator(*iter), value(iter->value), linked(true) {
+    if (iter != nullptr) {
+      value = iter->value;
+    }
+  }

-  HybridIterator(LinkedElement<T> *start)
-      : LinkedElement<T>::Iterator(start), value(start->value), linked(true) {}
+  HybridIterator(LinkedElement<T> *start) : LinkedElement<T>::Iterator(start), linked(true) {
+    if (start != nullptr) {
+      value = start->value;
+    }
+  }

  HybridIterator(typename ArrayList<T, count_t>::Iterator start,
                 typename ArrayList<T, count_t>::Iterator end)
      : ArrayList<T, count_t>::Iterator(start), value(start.value), linked(false), end(end.value) {
    if (value == this->end) {
-      value = NULL;
+      value = nullptr;
    }
  }

--- a/src/fsfw/controller/ControllerBase.cpp
+++ b/src/fsfw/controller/ControllerBase.cpp
@ -13,7 +13,9 @@ ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
      submode(SUBMODE_NONE),
      modeHelper(this),
      healthHelper(this, setObjectId) {
-  commandQueue = QueueFactory::instance()->createMessageQueue(commandQueueDepth);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      commandQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
--- a/src/fsfw/datapool/PoolEntry.cpp
+++ b/src/fsfw/datapool/PoolEntry.cpp
@ -7,24 +7,26 @@
 #include "fsfw/serviceinterface/ServiceInterface.h"

 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) {
+PoolEntry<T>::PoolEntry(uint8_t len, bool setValid) : length(len), valid(setValid) {
+  this->address = new T[this->length]();
  std::memset(this->address, 0, this->getByteSize());
-  } else {
-    std::copy(initValue.begin(), initValue.end(), this->address);
+}
+
+template <typename T>
+PoolEntry<T>::PoolEntry(std::initializer_list<T> initValues, bool setValid)
+    : length(static_cast<uint8_t>(initValues.size())), valid(setValid) {
+  this->address = new T[this->length]();
+  if (initValues.size() > 0) {
+    std::copy(initValues.begin(), initValues.end(), this->address);
  }
 }

 template <typename T>
-PoolEntry<T>::PoolEntry(T* initValue, uint8_t setLength, bool setValid)
+PoolEntry<T>::PoolEntry(const T* initValue, uint8_t setLength, bool setValid)
    : length(setLength), valid(setValid) {
-  this->address = new T[this->length];
+  this->address = new T[this->length]();
  if (initValue != nullptr) {
    std::memcpy(this->address, initValue, this->getByteSize());
-  } else {
-    std::memset(this->address, 0, this->getByteSize());
  }
 }

--- a/src/fsfw/datapool/PoolEntry.h
+++ b/src/fsfw/datapool/PoolEntry.h
@ -33,6 +33,9 @@ class PoolEntry : public PoolEntryIF {
                "instead! The ECSS standard defines a boolean as a one bit "
                "field. Therefore it is preferred to store a boolean as an "
                "uint8_t");
+
+  PoolEntry(uint8_t len = 1, bool setValid = false);
+
  /**
   * @brief	In the classe's constructor, space is allocated on the heap and
   * 			potential initialization values are copied to that space.
@ -49,7 +52,7 @@ class PoolEntry : public PoolEntryIF {
   * @param setValid
   * Sets the initialization flag. It is invalid by default.
   */
-  PoolEntry(std::initializer_list<T> initValue = {0}, bool setValid = false);
+  PoolEntry(std::initializer_list<T> initValue, bool setValid = false);

  /**
   * @brief	In the classe's constructor, space is allocated on the heap and
@ -62,7 +65,7 @@ class PoolEntry : public PoolEntryIF {
   * @param setValid
   * Sets the initialization flag. It is invalid by default.
   */
-  PoolEntry(T* initValue, uint8_t setLength = 1, bool setValid = false);
+  PoolEntry(const T* initValue, uint8_t setLength = 1, bool setValid = false);

  //! Explicitely deleted copy ctor, copying is not allowed.
  PoolEntry(const PoolEntry&) = delete;
--- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h
+++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h
@ -162,6 +162,7 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF {
  object_id_t getCreatorObjectId();

  bool getReportingEnabled() const;
+  void setReportingEnabled(bool enabled);

  /**
   * Returns the current periodic HK generation interval this set
@ -189,7 +190,6 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF {
   * Used for periodic generation.
   */
  bool reportingEnabled = false;
-  void setReportingEnabled(bool enabled);

  void initializePeriodicHelper(float collectionInterval, dur_millis_t minimumPeriodicInterval,
                                uint8_t nonDiagIntervalFactor = 5);
--- a/src/fsfw/devicehandlers/AssemblyBase.cpp
+++ b/src/fsfw/devicehandlers/AssemblyBase.cpp
@ -26,11 +26,7 @@ void AssemblyBase::performChildOperation() {

 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);
-  }
+  triggerModeHelperEvents(mode, submode);
 }

 void AssemblyBase::doStartTransition(Mode_t mode, Submode_t submode) {
@ -77,9 +73,10 @@ bool AssemblyBase::handleChildrenChangedHealth() {
  }
  HealthState healthState = healthHelper.healthTable->getHealth(iter->first);
  if (healthState == HasHealthIF::NEEDS_RECOVERY) {
-    triggerEvent(TRYING_RECOVERY);
+    triggerEvent(TRYING_RECOVERY, iter->first, 0);
    recoveryState = RECOVERY_STARTED;
    recoveringDevice = iter;
+    // The user needs to take care of commanding the children off in commandChildren
    doStartTransition(targetMode, targetSubmode);
  } else {
    triggerEvent(CHILD_CHANGED_HEALTH);
@ -228,6 +225,9 @@ ReturnValue_t AssemblyBase::handleHealthReply(CommandMessage* message) {
 bool AssemblyBase::checkAndHandleRecovery() {
  switch (recoveryState) {
    case RECOVERY_STARTED:
+      // The recovery was already start in #handleChildrenChangedHealth and we just need
+      // to wait for an off time period.
+      // TODO: make time period configurable
      recoveryState = RECOVERY_WAIT;
      recoveryOffTimer.resetTimer();
      return true;
@ -266,3 +266,11 @@ void AssemblyBase::overwriteDeviceHealth(object_id_t objectId, HasHealthIF::Heal
  modeHelper.setForced(true);
  sendHealthCommand(childrenMap[objectId].commandQueue, EXTERNAL_CONTROL);
 }
+
+void AssemblyBase::triggerModeHelperEvents(Mode_t mode, Submode_t submode) {
+  if (modeHelper.isForced()) {
+    triggerEvent(FORCING_MODE, mode, submode);
+  } else {
+    triggerEvent(CHANGING_MODE, mode, submode);
+  }
+}
--- a/src/fsfw/devicehandlers/AssemblyBase.h
+++ b/src/fsfw/devicehandlers/AssemblyBase.h
@ -12,7 +12,8 @@
 * Documentation: Dissertation Baetz p.156, 157.
 *
 * This class reduces the complexity of controller components which would
- * otherwise be needed for the handling of redundant devices.
+ * otherwise be needed for the handling of redundant devices. However, it can also be used to
+ * manage the mode keeping and recovery of non-redundant devices
 *
 * The template class monitors mode and health state of its children
 * and checks availability of devices on every detected change.
@ -26,11 +27,9 @@
 *
 * Important:
 *
- * The implementation must call registerChild(object_id_t child)
- * for all commanded children during initialization.
+ * The implementation must call #registerChild for all commanded children during initialization.
 * The implementation must call the initialization function of the base class.
 * (This will call the function in SubsystemBase)
- *
 */
 class AssemblyBase : public SubsystemBase {
 public:
@ -47,13 +46,14 @@ class AssemblyBase : public SubsystemBase {

 protected:
  /**
-   * Command children to reach [mode,submode] combination
-   * Can be done by setting #commandsOutstanding correctly,
-   * or using executeTable()
+   * Command children to reach [mode,submode] combination. Can be done by setting
+   * #commandsOutstanding correctly, or using #executeTable. In case of an FDIR recovery,
+   * the user needs to ensure that the target devices are healthy. If a device is not healthy,
+   * a recovery might be on-going and the device needs to be commanded to off first.
   * @param mode
   * @param submode
   * @return
-   *    - @c RETURN_OK if ok
+   *    - @c RETURN_OK if OK
   *    - @c NEED_SECOND_STEP if children need to be commanded again
   */
  virtual ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) = 0;
@ -120,8 +120,19 @@ class AssemblyBase : public SubsystemBase {

  virtual ReturnValue_t handleHealthReply(CommandMessage *message);

-  virtual void performChildOperation();
+  /**
+   * @brief Default periodic handler
+   * @details
+   * This is the default periodic handler which will be called by the SubsystemBase
+   * performOperation. It performs the child transitions or reacts to changed health/mode states
+   * of children objects
+   */
+  virtual void performChildOperation() override;

+  /**
+   * This function handles changed mode or health states of children
+   * @return
+   */
  bool handleChildrenChanged();

  /**
@ -134,12 +145,37 @@ class AssemblyBase : public SubsystemBase {

  bool handleChildrenChangedHealth();

+  /**
+   * Core transition handler. The default implementation will only do something if
+   * #commandsOutstanding is smaller or equal to zero, which means that all mode commands
+   * from the #doPerformTransition call were executed successfully.
+   *
+   * Unless a second step was requested, the function will then use #checkChildrenState to
+   * determine whether the target mode was reached.
+   *
+   * There is some special handling for certain (internal) modes:
+   *  - A second step is necessary. #commandChildren will be performed again
+   *  - The device health was overwritten. #commandChildren will be called
+   *  - A recovery is ongoing. #checkAndHandleRecovery will be called.
+   */
  virtual void handleChildrenTransition();

  ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode);

+  /**
+   * Calls #doStartTransition and triggers an informative event as well that the mode will
+   * change
+   * @param mode
+   * @param submode
+   */
  virtual void startTransition(Mode_t mode, Submode_t submode);

+  /**
+   * This function starts the transition by setting the internal #targetSubmode and #targetMode
+   * variables and then calling the #commandChildren function.
+   * @param mode
+   * @param submode
+   */
  virtual void doStartTransition(Mode_t mode, Submode_t submode);

  virtual bool isInTransition();
@ -160,7 +196,7 @@ class AssemblyBase : public SubsystemBase {
   * Manages recovery of a device
   * @return true if recovery is still ongoing, false else.
   */
-  bool checkAndHandleRecovery();
+  virtual bool checkAndHandleRecovery();

  /**
   * Helper method to overwrite health state of one of the children.
@ -168,6 +204,8 @@ class AssemblyBase : public SubsystemBase {
   * @param objectId Must be a registered child.
   */
  void overwriteDeviceHealth(object_id_t objectId, HasHealthIF::HealthState oldHealth);
+
+  void triggerModeHelperEvents(Mode_t mode, Submode_t submode);
 };

 #endif /* FSFW_DEVICEHANDLERS_ASSEMBLYBASE_H_ */
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@ -39,8 +39,9 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device
      childTransitionDelay(5000),
      transitionSourceMode(_MODE_POWER_DOWN),
      transitionSourceSubMode(SUBMODE_NONE) {
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
  commandQueue = QueueFactory::instance()->createMessageQueue(
-      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE);
+      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
  insertInCommandMap(RAW_COMMAND_ID);
  cookieInfo.state = COOKIE_UNUSED;
  cookieInfo.pendingCommand = deviceCommandMap.end();
@ -48,9 +49,6 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device
    printWarningOrError(sif::OutputTypes::OUT_ERROR, "DeviceHandlerBase",
                        HasReturnvaluesIF::RETURN_FAILED, "Invalid cookie");
  }
-  if (this->fdirInstance == nullptr) {
-    this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId, defaultFdirParentId);
-  }
 }

 void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) {
@ -126,6 +124,18 @@ ReturnValue_t DeviceHandlerBase::initialize() {
  if (result != RETURN_OK) {
    return result;
  }
+  if (this->fdirInstance == nullptr) {
+    this->fdirInstance =
+        new DeviceHandlerFailureIsolation(this->getObjectId(), defaultFdirParentId);
+  }
+
+  if (this->parent != objects::NO_OBJECT) {
+    HasModesIF* modeIF = ObjectManager::instance()->get<HasModesIF>(this->parent);
+    HasHealthIF* healthIF = ObjectManager::instance()->get<HasHealthIF>(this->parent);
+    if (modeIF != nullptr and healthIF != nullptr) {
+      setParentQueue(modeIF->getCommandQueue());
+    }
+  }

  communicationInterface =
      ObjectManager::instance()->get<DeviceCommunicationIF>(deviceCommunicationId);
@ -352,14 +362,12 @@ void DeviceHandlerBase::doStateMachine() {
      }
    } break;
    case _MODE_WAIT_OFF: {
-      uint32_t currentUptime;
-      Clock::getUptime(&currentUptime);
-
      if (powerSwitcher == nullptr) {
        setMode(MODE_OFF);
        break;
      }
-
+      uint32_t currentUptime;
+      Clock::getUptime(&currentUptime);
      if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) {
        triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0);
        setMode(MODE_ERROR_ON);
@ -1399,6 +1407,8 @@ void DeviceHandlerBase::setTaskIF(PeriodicTaskIF* task) { executingTask = task;
 void DeviceHandlerBase::debugInterface(uint8_t positionTracker, object_id_t objectId,
                                       uint32_t parameter) {}

+Submode_t DeviceHandlerBase::getInitialSubmode() { return SUBMODE_NONE; }
+
 void DeviceHandlerBase::performOperationHook() {}

 ReturnValue_t DeviceHandlerBase::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@ -1421,7 +1431,7 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() {
  this->poolManager.initializeAfterTaskCreation();

  if (setStartupImmediately) {
-    startTransition(MODE_ON, SUBMODE_NONE);
+    startTransition(MODE_ON, getInitialSubmode());
  }
  return HasReturnvaluesIF::RETURN_OK;
 }
@ -1505,3 +1515,11 @@ MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyI
  }
  return commandIter->second.sendReplyTo;
 }
+
+void DeviceHandlerBase::setCustomFdir(FailureIsolationBase* fdir) { this->fdirInstance = fdir; }
+
+void DeviceHandlerBase::setParent(object_id_t parent) { this->parent = parent; }
+
+void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) {
+  this->powerSwitcher = switcher;
+}
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h
@ -103,6 +103,9 @@ class DeviceHandlerBase : public DeviceHandlerIF,
  DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF *comCookie,
                    FailureIsolationBase *fdirInstance = nullptr, size_t cmdQueueSize = 20);

+  void setCustomFdir(FailureIsolationBase *fdir);
+  void setParent(object_id_t parent);
+  void setPowerSwitcher(PowerSwitchIF *switcher);
  void setHkDestination(object_id_t hkDestination);

  /**
@ -649,6 +652,12 @@ class DeviceHandlerBase : public DeviceHandlerIF,
  virtual void debugInterface(uint8_t positionTracker = 0, object_id_t objectId = 0,
                              uint32_t parameter = 0);

+  /**
+   * @brief Can be overwritten by a child to specify the initial submode when device has been set
+   *        to startup immediately.
+   */
+  virtual Submode_t getInitialSubmode();
+
 protected:
  static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_BASE;

@ -822,6 +831,7 @@ class DeviceHandlerBase : public DeviceHandlerIF,
  /** Pointer to the used FDIR instance. If not provided by child,
   * default class is instantiated. */
  FailureIsolationBase *fdirInstance;
+  object_id_t parent = objects::NO_OBJECT;

  //! To correctly delete the default instance.
  bool defaultFDIRUsed;
--- a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp
@ -29,6 +29,7 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event)
  switch (event->getEvent()) {
    case HasModesIF::MODE_TRANSITION_FAILED:
    case HasModesIF::OBJECT_IN_INVALID_MODE:
+    case DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT:
      // We'll try a recovery as long as defined in MAX_REBOOT.
      // Might cause some AssemblyBase cycles, so keep number low.
      handleRecovery(event->getEvent());
--- a/src/fsfw/devicehandlers/DeviceHandlerIF.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerIF.h
@ -109,6 +109,7 @@ class DeviceHandlerIF {
  static const Event INVALID_DEVICE_COMMAND = MAKE_EVENT(8, severity::LOW);
  static const Event MONITORING_LIMIT_EXCEEDED = MAKE_EVENT(9, severity::LOW);
  static const Event MONITORING_AMBIGUOUS = MAKE_EVENT(10, severity::HIGH);
+  static const Event DEVICE_WANTS_HARD_REBOOT = MAKE_EVENT(11, severity::HIGH);

  static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_IF;

--- a/src/fsfw/devicehandlers/HealthDevice.cpp
+++ b/src/fsfw/devicehandlers/HealthDevice.cpp
@ -8,7 +8,9 @@ HealthDevice::HealthDevice(object_id_t setObjectId, MessageQueueId_t parentQueue
      parentQueue(parentQueue),
      commandQueue(),
      healthHelper(this, setObjectId) {
-  commandQueue = QueueFactory::instance()->createMessageQueue(3);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 HealthDevice::~HealthDevice() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
--- a/src/fsfw/events/EventManager.cpp
+++ b/src/fsfw/events/EventManager.cpp
@ -18,8 +18,9 @@ const LocalPool::LocalPoolConfig EventManager::poolConfig = {
 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);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  eventReportQueue = QueueFactory::instance()->createMessageQueue(
+      MAX_EVENTS_PER_CYCLE, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
 }

 EventManager::~EventManager() {
@ -46,9 +47,20 @@ ReturnValue_t EventManager::performOperation(uint8_t opCode) {

 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());
+  for (auto& listener : listenerList) {
+    if (listener.second.match(message)) {
+      ReturnValue_t result =
+          MessageQueueSenderIF::sendMessage(listener.first, message, message->getSender());
+      if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << std::hex << "EventManager::notifyListeners: MSG to 0x" << std::setfill('0')
+                   << std::setw(8) << listener.first << " failed with result 0x" << std::setw(4)
+                   << result << std::setfill(' ') << std::endl;
+#else
+        sif::printError("Sending message to listener 0x%08x failed with result %04x\n",
+                        listener.first, result);
+#endif
+      }
    }
  }
  unlockMutex();
@ -189,4 +201,19 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage* messag
  }
 }

+void EventManager::printListeners() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::info << "Event manager listener MQ IDs:" << std::setfill('0') << std::hex << std::endl;
+  for (auto& listener : listenerList) {
+    sif::info << "0x" << std::setw(8) << listener.first << std::endl;
+  }
+  sif::info << std::dec << std::setfill(' ');
+#else
+  sif::printInfo("Event manager listener MQ IDs:\n");
+  for (auto& listener : listenerList) {
+    sif::printInfo("0x%08x\n", listener.first);
+  }
+#endif
+}
+
 #endif /* FSFW_OBJ_EVENT_TRANSLATION == 1 */
--- a/src/fsfw/events/EventManager.h
+++ b/src/fsfw/events/EventManager.h
@ -42,6 +42,7 @@ class EventManager : public EventManagerIF, public ExecutableObjectIF, public Sy
                                          object_id_t reporterFrom = 0, object_id_t reporterTo = 0,
                                          bool reporterInverted = false);
  ReturnValue_t performOperation(uint8_t opCode);
+  void printListeners();

 protected:
  MessageQueueIF* eventReportQueue = nullptr;
--- a/src/fsfw/fdir/FailureIsolationBase.cpp
+++ b/src/fsfw/fdir/FailureIsolationBase.cpp
@ -9,8 +9,9 @@
 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);
+  auto mqArgs = MqArgs(owner, static_cast<void*>(this));
+  eventQueue = QueueFactory::instance()->createMessageQueue(
+      messageDepth, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
 }

 FailureIsolationBase::~FailureIsolationBase() {
@ -52,10 +53,11 @@ ReturnValue_t FailureIsolationBase::initialize() {
    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;
+                 << "invalid" << std::endl;
+      sif::error << "Make sure it implements ConfirmsFailuresIF" << std::endl;
+#else
+      sif::printError("FailureIsolationBase::intialize: Parent object invalid\n");
+      sif::printError("Make sure it implements ConfirmsFailuresIF\n");
 #endif
      return ObjectManagerIF::CHILD_INIT_FAILED;
      return RETURN_FAILED;
--- a/src/fsfw/fdir/FailureIsolationBase.h
+++ b/src/fsfw/fdir/FailureIsolationBase.h
@ -14,13 +14,12 @@ class FailureIsolationBase : public HasReturnvaluesIF,
                             public HasParametersIF {
 public:
  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::FDIR_1;
-  static const Event FDIR_CHANGED_STATE =
-      MAKE_EVENT(1, severity::INFO);  //!< FDIR has an internal state, which changed from par2
-                                      //!< (oldState) to par1 (newState).
-  static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT(
-      2, severity::MEDIUM);  //!< FDIR tries to restart device. Par1: event that caused recovery.
-  static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT(
-      3, severity::MEDIUM);  //!< FDIR turns off device. Par1: event that caused recovery.
+  //! FDIR has an internal state, which changed from par2 (oldState) to par1 (newState).
+  static const Event FDIR_CHANGED_STATE = MAKE_EVENT(1, severity::INFO);
+  //! FDIR tries to restart device. Par1: event that caused recovery.
+  static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT(2, severity::MEDIUM);
+  //! FDIR turns off device. Par1: event that caused recovery.
+  static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT(3, severity::MEDIUM);

  FailureIsolationBase(object_id_t owner, object_id_t parent = objects::NO_OBJECT,
                       uint8_t messageDepth = 10, uint8_t parameterDomainBase = 0xF0);
--- a/src/fsfw/globalfunctions/CMakeLists.txt
+++ b/src/fsfw/globalfunctions/CMakeLists.txt
@ -1,9 +1,9 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
+target_sources(${LIB_FSFW_NAME} PRIVATE
    arrayprinter.cpp
    AsciiConverter.cpp
    CRC.cpp
    DleEncoder.cpp
+    DleParser.cpp
    PeriodicOperationDivider.cpp
    timevalOperations.cpp
    Type.cpp
--- a/src/fsfw/globalfunctions/DleParser.cpp
+++ b/src/fsfw/globalfunctions/DleParser.cpp
@ -0,0 +1,231 @@
+#include "DleParser.h"
+
+#include <fsfw/globalfunctions/DleEncoder.h>
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+#include <cstdio>
+
+DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
+                     BufPair decodedBuf, UserHandler handler, void* args)
+    : decodeRingBuf(decodeRingBuf),
+      decoder(decoder),
+      encodedBuf(encodedBuf),
+      decodedBuf(decodedBuf),
+      handler(handler),
+      ctx(args) {
+  if (handler == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "DleParser::DleParser: Invalid user handler" << std::endl;
+#else
+    sif::printError("DleParser::DleParser: Invalid user handler\n");
+#endif
+  }
+}
+
+ReturnValue_t DleParser::passData(uint8_t* data, size_t len) {
+  if (data == nullptr or len == 0 or handler == nullptr) {
+    return RETURN_FAILED;
+  }
+  size_t copyIntoRingBufFromHere = 0;
+  size_t copyAmount = len;
+  size_t startIdx = 0;
+  ReturnValue_t result = RETURN_OK;
+  bool startFoundInThisPacket = false;
+  for (size_t idx = 0; idx < len; idx++) {
+    if (data[idx] == DleEncoder::STX_CHAR) {
+      if (not startFound and not startFoundInThisPacket) {
+        startIdx = idx;
+        copyIntoRingBufFromHere = idx;
+        copyAmount = len - idx;
+      } else {
+        // Maybe print warning, should not happen
+        decodeRingBuf.clear();
+        ErrorInfo info;
+        info.len = idx;
+        prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info);
+        handler(ctx);
+        copyIntoRingBufFromHere = idx;
+        copyAmount = len - idx;
+      }
+      startFound = true;
+      startFoundInThisPacket = true;
+    } else if (data[idx] == DleEncoder::ETX_CHAR) {
+      if (startFoundInThisPacket) {
+        size_t readLen = 0;
+        size_t decodedLen = 0;
+        result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first,
+                                decodedBuf.second, &decodedLen);
+        if (result == HasReturnvaluesIF::RETURN_OK) {
+          ctx.setType(ContextType::PACKET_FOUND);
+          ctx.decodedPacket.first = decodedBuf.first;
+          ctx.decodedPacket.second = decodedLen;
+          this->handler(ctx);
+        } else if (result == DleEncoder::STREAM_TOO_SHORT) {
+          ErrorInfo info;
+          info.res = result;
+          prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
+          handler(ctx);
+        } else {
+          ErrorInfo info;
+          info.res = result;
+          prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
+          handler(ctx);
+        }
+        decodeRingBuf.clear();
+        if ((idx + 1) < len) {
+          copyIntoRingBufFromHere = idx + 1;
+          copyAmount = len - idx - 1;
+        } else {
+          copyAmount = 0;
+        }
+      } else if (startFound) {
+        // ETX found but STX was found in another mini packet. Reconstruct the full packet
+        // to decode it
+        result = decodeRingBuf.writeData(data, idx + 1);
+        if (result != HasReturnvaluesIF::RETURN_OK) {
+          ErrorInfo info;
+          info.res = result;
+          prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info);
+          handler(ctx);
+        }
+        size_t fullEncodedLen = decodeRingBuf.getAvailableReadData();
+        if (fullEncodedLen > encodedBuf.second) {
+          ErrorInfo info;
+          info.len = fullEncodedLen;
+          prepareErrorContext(ErrorTypes::ENCODED_BUF_TOO_SMALL, info);
+          handler(ctx);
+          decodeRingBuf.clear();
+        } else {
+          size_t decodedLen = 0;
+          size_t readLen = 0;
+          decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true);
+          result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first,
+                                  decodedBuf.second, &decodedLen);
+          if (result == HasReturnvaluesIF::RETURN_OK) {
+            if (this->handler != nullptr) {
+              ctx.setType(ContextType::PACKET_FOUND);
+              ctx.decodedPacket.first = decodedBuf.first;
+              ctx.decodedPacket.second = decodedLen;
+              this->handler(ctx);
+            }
+          } else if (result == DleEncoder::STREAM_TOO_SHORT) {
+            ErrorInfo info;
+            info.res = result;
+            prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
+            handler(ctx);
+          } else {
+            ErrorInfo info;
+            info.res = result;
+            prepareErrorContext(ErrorTypes::DECODE_ERROR, info);
+            handler(ctx);
+          }
+          decodeRingBuf.clear();
+          startFound = false;
+          startFoundInThisPacket = false;
+          if ((idx + 1) < len) {
+            copyIntoRingBufFromHere = idx + 1;
+            copyAmount = len - idx - 1;
+          } else {
+            copyAmount = 0;
+          }
+        }
+      } else {
+        // End data without preceeding STX
+        ErrorInfo info;
+        info.len = idx + 1;
+        prepareErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info);
+        handler(ctx);
+        decodeRingBuf.clear();
+        if ((idx + 1) < len) {
+          copyIntoRingBufFromHere = idx + 1;
+          copyAmount = len - idx - 1;
+        } else {
+          copyAmount = 0;
+        }
+      }
+      startFoundInThisPacket = false;
+      startFound = false;
+    }
+  }
+  if (copyAmount > 0) {
+    result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      ErrorInfo info;
+      info.res = result;
+      prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info);
+      handler(ctx);
+    }
+  }
+  return RETURN_OK;
+}
+
+void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::info << "DleParserBase::handleFoundPacket: Detected DLE packet with " << len << " bytes"
+            << std::endl;
+#else
+  sif::printInfo("DleParserBase::handleFoundPacket: Detected DLE packet with %d bytes\n", len);
+#endif
+#endif
+}
+
+void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) {
+  switch (err) {
+    case (ErrorTypes::NONE): {
+      errorPrinter("No error");
+      break;
+    }
+    case (ErrorTypes::DECODE_ERROR): {
+      errorPrinter("Decode Error");
+      break;
+    }
+    case (ErrorTypes::RING_BUF_ERROR): {
+      errorPrinter("Ring Buffer Error");
+      break;
+    }
+    case (ErrorTypes::ENCODED_BUF_TOO_SMALL):
+    case (ErrorTypes::DECODING_BUF_TOO_SMALL): {
+      char opt[64];
+      snprintf(opt, sizeof(opt), ": Too small for packet with length %d", ctx.len);
+      if (err == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
+        errorPrinter("Encoded buf too small", opt);
+      } else {
+        errorPrinter("Decoding buf too small", opt);
+      }
+      break;
+    }
+    case (ErrorTypes::CONSECUTIVE_STX_CHARS): {
+      errorPrinter("Consecutive STX chars detected");
+      break;
+    }
+    case (ErrorTypes::CONSECUTIVE_ETX_CHARS): {
+      errorPrinter("Consecutive ETX chars detected");
+      break;
+    }
+  }
+}
+
+void DleParser::errorPrinter(const char* str, const char* opt) {
+  if (opt == nullptr) {
+    opt = "";
+  }
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::info << "DleParserBase::handleParseError: " << str << opt << std::endl;
+#else
+  sif::printInfo("DleParserBase::handleParseError: %s%s\n", str, opt);
+#endif
+#endif
+}
+
+void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) {
+  ctx.setType(ContextType::ERROR);
+  ctx.error.first = err;
+  ctx.error.second = info;
+}
+
+void DleParser::reset() {
+  startFound = false;
+  decodeRingBuf.clear();
+}
--- a/src/fsfw/globalfunctions/DleParser.h
+++ b/src/fsfw/globalfunctions/DleParser.h
@ -0,0 +1,127 @@
+#ifndef MISSION_DEVICES_DLEPARSER_H_
+#define MISSION_DEVICES_DLEPARSER_H_
+
+#include <fsfw/container/SimpleRingBuffer.h>
+#include <fsfw/globalfunctions/DleEncoder.h>
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+
+#include <cstddef>
+#include <utility>
+
+/**
+ * @brief This base helper class can be used to extract DLE encoded packets from a data stream
+ * @details
+ * The core API of the parser takes received packets which can contains DLE packets. The parser
+ * can deal with DLE packets split across multiple packets. It does so by using a dedicated
+ * decoding ring buffer. The user can process received packets and detect errors by
+ * overriding two provided virtual methods. This also allows detecting multiple DLE packets
+ * inside one passed packet.
+ */
+class DleParser : public HasReturnvaluesIF {
+ public:
+  using BufPair = std::pair<uint8_t*, size_t>;
+
+  enum class ContextType { PACKET_FOUND, ERROR };
+
+  enum class ErrorTypes {
+    NONE,
+    ENCODED_BUF_TOO_SMALL,
+    DECODING_BUF_TOO_SMALL,
+    DECODE_ERROR,
+    RING_BUF_ERROR,
+    CONSECUTIVE_STX_CHARS,
+    CONSECUTIVE_ETX_CHARS
+  };
+
+  union ErrorInfo {
+    size_t len;
+    ReturnValue_t res;
+  };
+
+  using ErrorPair = std::pair<ErrorTypes, ErrorInfo>;
+
+  struct Context {
+   public:
+    Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); }
+
+    void setType(ContextType type) {
+      if (type == ContextType::PACKET_FOUND) {
+        error.first = ErrorTypes::NONE;
+        error.second.len = 0;
+      } else {
+        decodedPacket.first = nullptr;
+        decodedPacket.second = 0;
+      }
+    }
+
+    ContextType getType() const { return type; }
+
+    BufPair decodedPacket = {};
+    ErrorPair error;
+    void* userArgs;
+
+   private:
+    ContextType type;
+  };
+
+  using UserHandler = void (*)(const Context& ctx);
+
+  /**
+   * Base class constructor
+   * @param decodeRingBuf Ring buffer used to store multiple packets to allow detecting DLE packets
+   * split across multiple packets
+   * @param decoder Decoder instance
+   * @param encodedBuf Buffer used to store encoded packets. It has to be large enough to hold
+   * the largest expected encoded DLE packet size
+   * @param decodedBuf Buffer used to store decoded packets. It has to be large enough to hold the
+   * largest expected decoded DLE packet size
+   * @param handler Function which will be called on a found packet
+   * @param args Arbitrary user argument
+   */
+  DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
+            BufPair decodedBuf, UserHandler handler, void* args);
+
+  /**
+   * This function allows to pass new data into the parser. It then scans for DLE packets
+   * automatically and inserts (part of) the packet into a ring buffer if necessary.
+   * @param data
+   * @param len
+   * @return
+   */
+  ReturnValue_t passData(uint8_t* data, size_t len);
+
+  /**
+   * Example found packet handler
+   * function call
+   * @param packet Decoded packet
+   * @param len Length of detected packet
+   */
+  void defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args);
+  /**
+   * Will be called if an error occured in the #passData call
+   * @param err
+   * @param ctx Context information depending on the error type
+   *  - For buffer length errors, will be set to the detected packet length which is too large
+   *  - For decode or ring buffer errors, will be set to the result returned from the failed call
+   */
+  static void defaultErrorHandler(ErrorTypes err, ErrorInfo ctx);
+
+  static void errorPrinter(const char* str, const char* opt = nullptr);
+
+  void prepareErrorContext(ErrorTypes err, ErrorInfo ctx);
+  /**
+   * Resets the parser by resetting the internal states and clearing the decoding ring buffer
+   */
+  void reset();
+
+ private:
+  SimpleRingBuffer& decodeRingBuf;
+  DleEncoder& decoder;
+  BufPair encodedBuf;
+  BufPair decodedBuf;
+  UserHandler handler = nullptr;
+  Context ctx;
+  bool startFound = false;
+};
+
+#endif /* MISSION_DEVICES_DLEPARSER_H_ */
--- a/src/fsfw/globalfunctions/matching/MatchTree.h
+++ b/src/fsfw/globalfunctions/matching/MatchTree.h
@ -179,6 +179,9 @@ class MatchTree : public SerializeableMatcherIF<T>, public BinaryTree<Serializea
  virtual ReturnValue_t cleanUpElement(iterator position) { return HasReturnvaluesIF::RETURN_OK; }

  bool matchSubtree(iterator iter, T number) {
+    if (iter == nullptr) {
+      return false;
+    }
    bool isMatch = iter->match(number);
    if (isMatch) {
      if (iter.left() == this->end()) {
--- a/src/fsfw/health/HasHealthIF.h
+++ b/src/fsfw/health/HasHealthIF.h
@ -23,19 +23,15 @@ class HasHealthIF {
  static const Event HEALTH_INFO = MAKE_EVENT(6, severity::INFO);
  static const Event CHILD_CHANGED_HEALTH = MAKE_EVENT(7, severity::INFO);
  static const Event CHILD_PROBLEMS = MAKE_EVENT(8, severity::LOW);
-  static const Event OVERWRITING_HEALTH =
-      MAKE_EVENT(9, severity::LOW);  //!< Assembly overwrites health information of children to keep
-                                     //!< satellite alive.
-  static const Event TRYING_RECOVERY =
-      MAKE_EVENT(10, severity::MEDIUM);  //!< Someone starts a recovery of a component (typically
-                                         //!< power-cycle). No parameters.
-  static const Event RECOVERY_STEP =
-      MAKE_EVENT(11, severity::MEDIUM);  //!< Recovery is ongoing. Comes twice during recovery. P1:
-                                         //!< 0 for the first, 1 for the second event. P2: 0
-  static const Event RECOVERY_DONE = MAKE_EVENT(
-      12,
-      severity::MEDIUM);  //!< Recovery was completed. Not necessarily successful. No parameters.
-
+  //! Assembly overwrites health information of children to keep satellite alive.
+  static const Event OVERWRITING_HEALTH = MAKE_EVENT(9, severity::LOW);
+  //! Someone starts a recovery of a component (typically power-cycle). No parameters.
+  static const Event TRYING_RECOVERY = MAKE_EVENT(10, severity::MEDIUM);
+  //! Recovery is ongoing. Comes twice during recovery.
+  //! P1: 0 for the first, 1 for the second event. P2: 0
+  static const Event RECOVERY_STEP = MAKE_EVENT(11, severity::MEDIUM);
+  //! Recovery was completed. Not necessarily successful. No parameters.
+  static const Event RECOVERY_DONE = MAKE_EVENT(12, severity::MEDIUM);
  virtual ~HasHealthIF() {}

  virtual MessageQueueId_t getCommandQueue() const = 0;
--- a/src/fsfw/internalerror/InternalErrorReporter.cpp
+++ b/src/fsfw/internalerror/InternalErrorReporter.cpp
@ -7,11 +7,13 @@

 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();
+  auto mqArgs = MqArgs(setObjectId, static_cast<void *>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 InternalErrorReporter::~InternalErrorReporter() { MutexFactory::instance()->deleteMutex(mutex); }
@ -36,15 +38,14 @@ ReturnValue_t InternalErrorReporter::performOperation(uint8_t opCode) {
    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;
+                 << "occured: Queue | TM | Store : " << newQueueHits << " | " << newTmHits << " | "
+                 << 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));
+      sif::printDebug(
+          "InternalErrorReporter::performOperation: Errors occured: Queue | TM | Store: %lu | %lu "
+          "| %lu\n",
+          static_cast<unsigned int>(newQueueHits), static_cast<unsigned int>(newTmHits),
+          static_cast<unsigned int>(newStoreHits));
 #endif
    }
  }
--- a/src/fsfw/ipc/CMakeLists.txt
+++ b/src/fsfw/ipc/CMakeLists.txt
@ -1,6 +1,6 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
+target_sources(${LIB_FSFW_NAME} PRIVATE
    CommandMessage.cpp
    CommandMessageCleaner.cpp
    MessageQueueMessage.cpp
+    MessageQueueBase.cpp
 )
--- a/src/fsfw/ipc/MessageQueueBase.cpp
+++ b/src/fsfw/ipc/MessageQueueBase.cpp
@ -0,0 +1,54 @@
+#include "MessageQueueBase.h"
+
+MessageQueueBase::MessageQueueBase(MessageQueueId_t id, MessageQueueId_t defaultDest, MqArgs* args)
+    : id(id) {
+  this->defaultDest = defaultDest;
+  if (args != nullptr) {
+    this->args = *args;
+  }
+}
+
+MessageQueueBase::~MessageQueueBase() {}
+
+ReturnValue_t MessageQueueBase::sendToDefault(MessageQueueMessageIF* message) {
+  return sendToDefaultFrom(message, this->getId(), false);
+}
+
+ReturnValue_t MessageQueueBase::reply(MessageQueueMessageIF* message) {
+  if (this->last != MessageQueueIF::NO_QUEUE) {
+    return sendMessageFrom(this->last, message, this->getId());
+  } else {
+    return NO_REPLY_PARTNER;
+  }
+}
+
+ReturnValue_t MessageQueueBase::receiveMessage(MessageQueueMessageIF* message,
+                                               MessageQueueId_t* receivedFrom) {
+  ReturnValue_t status = this->receiveMessage(message);
+  *receivedFrom = this->last;
+  return status;
+}
+
+MessageQueueId_t MessageQueueBase::getLastPartner() const { return last; }
+
+MessageQueueId_t MessageQueueBase::getId() const { return id; }
+
+MqArgs& MessageQueueBase::getMqArgs() { return args; }
+
+void MessageQueueBase::setDefaultDestination(MessageQueueId_t defaultDestination) {
+  this->defaultDest = defaultDestination;
+}
+
+MessageQueueId_t MessageQueueBase::getDefaultDestination() const { return defaultDest; }
+
+bool MessageQueueBase::isDefaultDestinationSet() const { return (defaultDest != NO_QUEUE); }
+
+ReturnValue_t MessageQueueBase::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
+                                            bool ignoreFault) {
+  return sendMessageFrom(sendTo, message, this->getId(), false);
+}
+
+ReturnValue_t MessageQueueBase::sendToDefaultFrom(MessageQueueMessageIF* message,
+                                                  MessageQueueId_t sentFrom, bool ignoreFault) {
+  return sendMessageFrom(defaultDest, message, sentFrom, ignoreFault);
+}
--- a/src/fsfw/ipc/MessageQueueBase.h
+++ b/src/fsfw/ipc/MessageQueueBase.h
@ -0,0 +1,40 @@
+#ifndef FSFW_SRC_FSFW_IPC_MESSAGEQUEUEBASE_H_
+#define FSFW_SRC_FSFW_IPC_MESSAGEQUEUEBASE_H_
+
+#include <fsfw/ipc/MessageQueueIF.h>
+#include <fsfw/ipc/definitions.h>
+
+class MessageQueueBase : public MessageQueueIF {
+ public:
+  MessageQueueBase(MessageQueueId_t id, MessageQueueId_t defaultDest, MqArgs* mqArgs);
+  virtual ~MessageQueueBase();
+
+  // Default implementations for MessageQueueIF where possible
+  virtual MessageQueueId_t getLastPartner() const override;
+  virtual MessageQueueId_t getId() const override;
+  virtual MqArgs& getMqArgs() override;
+  virtual void setDefaultDestination(MessageQueueId_t defaultDestination) override;
+  virtual MessageQueueId_t getDefaultDestination() const override;
+  virtual bool isDefaultDestinationSet() const override;
+  virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
+                                    bool ignoreFault) override;
+  virtual ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override;
+  virtual ReturnValue_t reply(MessageQueueMessageIF* message) override;
+  virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
+                                       MessageQueueId_t* receivedFrom) override;
+  virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
+                                          bool ignoreFault = false) override;
+
+  // OSAL specific, forward the abstract function
+  virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message) = 0;
+  virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
+                                        MessageQueueId_t sentFrom, bool ignoreFault = false) = 0;
+
+ protected:
+  MessageQueueId_t id = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t last = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t defaultDest = MessageQueueIF::NO_QUEUE;
+  MqArgs args = {};
+};
+
+#endif /* FSFW_SRC_FSFW_IPC_MESSAGEQUEUEBASE_H_ */
--- a/src/fsfw/ipc/MessageQueueIF.h
+++ b/src/fsfw/ipc/MessageQueueIF.h
@ -1,6 +1,8 @@
 #ifndef FSFW_IPC_MESSAGEQUEUEIF_H_
 #define FSFW_IPC_MESSAGEQUEUEIF_H_

+#include <fsfw/ipc/definitions.h>
+
 #include <cstdint>

 #include "../returnvalues/HasReturnvaluesIF.h"
@ -44,8 +46,8 @@ class MessageQueueIF {
  virtual ReturnValue_t reply(MessageQueueMessageIF* message) = 0;

  /**
-   * @brief	This function reads available messages from the message queue
-   *          and returns the sender.
+   * @brief	This function reads available messages from the message queue and returns the
+   * sender.
   * @details
   * It works identically to the other receiveMessage call, but in addition
   * returns the sender's queue id.
@ -78,19 +80,16 @@ class MessageQueueIF {
   */
  virtual ReturnValue_t flush(uint32_t* count) = 0;
  /**
-   * @brief	This method returns the message queue
-   *          id of the last communication partner.
+   * @brief	This method returns the message queue ID of the last communication partner.
   */
  virtual MessageQueueId_t getLastPartner() const = 0;
  /**
-   * @brief	This method returns the message queue
-   *          id of this class's message queue.
+   * @brief	This method returns the message queue ID  of this class's message queue.
   */
  virtual MessageQueueId_t getId() const = 0;

  /**
-   * @brief	With the sendMessage call, a queue message
-   *          is sent to a receiving queue.
+   * @brief	With the sendMessage call, a queue message is sent to a receiving queue.
   * @details
   * This method takes the message provided, adds the sentFrom information
   * and passes it on to the destination provided with an operating system
@ -129,8 +128,7 @@ class MessageQueueIF {
                                    bool ignoreFault = false) = 0;

  /**
-   * @brief	The sendToDefaultFrom method sends a queue message
-   *          to the default destination.
+   * @brief	The sendToDefaultFrom method sends a queue message to the default destination.
   * @details
   * In all other aspects, it works identical to the sendMessage method.
   * @param message
@ -164,6 +162,8 @@ class MessageQueueIF {
  virtual MessageQueueId_t getDefaultDestination() const = 0;

  virtual bool isDefaultDestinationSet() const = 0;
+
+  virtual MqArgs& getMqArgs() = 0;
 };

 #endif /* FSFW_IPC_MESSAGEQUEUEIF_H_ */
--- a/src/fsfw/ipc/QueueFactory.h
+++ b/src/fsfw/ipc/QueueFactory.h
@ -5,6 +5,7 @@

 #include "MessageQueueIF.h"
 #include "MessageQueueMessage.h"
+#include "definitions.h"

 /**
 * Creates message queues.
@ -22,7 +23,8 @@ class QueueFactory {
  static QueueFactory* instance();

  MessageQueueIF* createMessageQueue(uint32_t messageDepth = 3,
-                                     size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
+                                     size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE,
+                                     MqArgs* args = nullptr);

  void deleteMessageQueue(MessageQueueIF* queue);

--- a/src/fsfw/ipc/definitions.h
+++ b/src/fsfw/ipc/definitions.h
@ -0,0 +1,14 @@
+#ifndef FSFW_SRC_FSFW_IPC_DEFINITIONS_H_
+#define FSFW_SRC_FSFW_IPC_DEFINITIONS_H_
+
+#include <fsfw/objectmanager/SystemObjectIF.h>
+#include <fsfw/objectmanager/frameworkObjects.h>
+
+struct MqArgs {
+  MqArgs(){};
+  MqArgs(object_id_t objectId, void* args = nullptr) : objectId(objectId), args(args) {}
+  object_id_t objectId = objects::NO_OBJECT;
+  void* args = nullptr;
+};
+
+#endif /* FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ */
--- a/src/fsfw/modes/HasModesIF.h
+++ b/src/fsfw/modes/HasModesIF.h
@ -19,32 +19,33 @@ class HasModesIF {
  static const ReturnValue_t INVALID_SUBMODE = MAKE_RETURN_CODE(0x04);

  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER;
-  static const Event CHANGING_MODE =
-      MAKE_EVENT(0, severity::INFO);  //!< An object announces changing the mode. p1: target mode.
-                                      //!< p2: target submode
-  static const Event MODE_INFO = MAKE_EVENT(
-      1,
-      severity::INFO);  //!< An Object announces its mode; parameter1 is mode, parameter2 is submode
+  //! An object announces changing the mode. p1: target mode. p2: target submode
+  static const Event CHANGING_MODE = MAKE_EVENT(0, severity::INFO);
+  //! An Object announces its mode; parameter1 is mode, parameter2 is submode
+  static const Event MODE_INFO = MAKE_EVENT(1, severity::INFO);
  static const Event FALLBACK_FAILED = MAKE_EVENT(2, severity::HIGH);
  static const Event MODE_TRANSITION_FAILED = MAKE_EVENT(3, severity::LOW);
  static const Event CANT_KEEP_MODE = MAKE_EVENT(4, severity::HIGH);
-  static const Event OBJECT_IN_INVALID_MODE =
-      MAKE_EVENT(5, severity::LOW);  //!< Indicates a bug or configuration failure: Object is in a
-                                     //!< mode it should never be in.
-  static const Event FORCING_MODE = MAKE_EVENT(
-      6, severity::MEDIUM);  //!< The mode is changed, but for some reason, the change is forced,
-                             //!< i.e. EXTERNAL_CONTROL ignored. p1: target mode. p2: target submode
-  static const Event MODE_CMD_REJECTED =
-      MAKE_EVENT(7, severity::LOW);  //!< A mode command was rejected by the called object. Par1:
-                                     //!< called object id, Par2: return code.
+  //! Indicates a bug or configuration failure: Object is in a mode it should never be in.
+  static const Event OBJECT_IN_INVALID_MODE = MAKE_EVENT(5, severity::LOW);
+  //! The mode is changed, but for some reason, the change is forced, i.e. EXTERNAL_CONTROL ignored.
+  //! p1: target mode. p2: target submode
+  static const Event FORCING_MODE = MAKE_EVENT(6, severity::MEDIUM);
+  //! A mode command was rejected by the called object. Par1: called object id, Par2: return code.
+  static const Event MODE_CMD_REJECTED = MAKE_EVENT(7, severity::LOW);

-  static const Mode_t MODE_ON =
-      1;  //!< The device is powered and ready to perform operations. In this mode, no commands are
-          //!< sent by the device handler itself, but direct commands van be commanded and will be
-          //!< interpreted
-  static const Mode_t MODE_OFF = 0;  //!< The device is powered off. The only command accepted in
-                                     //!< this mode is a mode change to on.
-  static const Submode_t SUBMODE_NONE = 0;  //!< To avoid checks against magic number "0".
+  //! The device is powered and ready to perform operations. In this mode, no commands are
+  //! sent by the device handler itself, but direct commands van be commanded and will be
+  //! interpreted
+  static constexpr Mode_t MODE_ON = 1;
+  //! The device is powered off. The only command accepted in this mode is a mode change to on.
+  static constexpr Mode_t MODE_OFF = 0;
+
+  static constexpr Mode_t MODE_INVALID = -1;
+  static constexpr Mode_t MODE_UNDEFINED = -2;
+
+  //! To avoid checks against magic number "0".
+  static const Submode_t SUBMODE_NONE = 0;

  virtual ~HasModesIF() {}
  virtual MessageQueueId_t getCommandQueue() const = 0;
--- a/src/fsfw/objectmanager/ObjectManager.cpp
+++ b/src/fsfw/objectmanager/ObjectManager.cpp
@ -95,13 +95,16 @@ void ObjectManager::initialize() {
  for (auto const& it : objectList) {
    result = it.second->initialize();
    if (result != RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #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;
+                 << std::setfill('0') << it.first << " failed to initialize with code 0x" << result
+                 << std::dec << std::setfill(' ') << std::endl;
+#else
+      sif::printError(
+          "ObjectManager::initialize: Object 0x%08x failed to initialize with code 0x%04x\n", var,
+          it.first);
+#endif
 #endif
      errorCount++;
    }
--- a/src/fsfw/osal/freertos/MessageQueue.cpp
+++ b/src/fsfw/osal/freertos/MessageQueue.cpp
@ -4,8 +4,9 @@
 #include "fsfw/osal/freertos/QueueMapManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"

-MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize)
-    : maxMessageSize(maxMessageSize) {
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args)
+    : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args),
+      maxMessageSize(maxMessageSize) {
  handle = xQueueCreate(messageDepth, maxMessageSize);
  if (handle == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@ -15,10 +16,10 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize)
 #else
    sif::printError("MessageQueue::MessageQueue: Creation failed\n");
    sif::printError("Specified Message Depth: %d\n", messageDepth);
-    sif::printError("Specified MAximum Message Size: %d\n", maxMessageSize);
+    sif::printError("Specified Maximum Message Size: %d\n", maxMessageSize);
 #endif
  }
-  QueueMapManager::instance()->addMessageQueue(handle, &queueId);
+  QueueMapManager::instance()->addMessageQueue(handle, &id);
 }

 MessageQueue::~MessageQueue() {
@ -29,28 +30,6 @@ MessageQueue::~MessageQueue() {

 void MessageQueue::switchSystemContext(CallContext callContext) { this->callContext = callContext; }

-ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                        bool ignoreFault) {
-  return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
-}
-
-ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
-  return sendToDefaultFrom(message, this->getId());
-}
-
-ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
-                                              MessageQueueId_t sentFrom, bool ignoreFault) {
-  return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
-}
-
-ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
-  if (this->lastPartner != MessageQueueIF::NO_QUEUE) {
-    return sendMessageFrom(this->lastPartner, message, this->getId());
-  } else {
-    return NO_REPLY_PARTNER;
-  }
-}
-
 ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                            MessageQueueId_t sentFrom, bool ignoreFault) {
  return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault, callContext);
@ -72,27 +51,16 @@ ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault
  return HasReturnvaluesIF::RETURN_OK;
 }

-ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
-                                           MessageQueueId_t* receivedFrom) {
-  ReturnValue_t status = this->receiveMessage(message);
-  if (status == HasReturnvaluesIF::RETURN_OK) {
-    *receivedFrom = this->lastPartner;
-  }
-  return status;
-}
-
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
  BaseType_t result = xQueueReceive(handle, reinterpret_cast<void*>(message->getBuffer()), 0);
  if (result == pdPASS) {
-    this->lastPartner = message->getSender();
+    this->last = message->getSender();
    return HasReturnvaluesIF::RETURN_OK;
  } else {
    return MessageQueueIF::EMPTY;
  }
 }

-MessageQueueId_t MessageQueue::getLastPartner() const { return lastPartner; }
-
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
  // TODO FreeRTOS does not support flushing partially
  // Is always successful
@ -100,17 +68,6 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) {
  return HasReturnvaluesIF::RETURN_OK;
 }

-MessageQueueId_t MessageQueue::getId() const { return queueId; }
-
-void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
-  defaultDestinationSet = true;
-  this->defaultDestination = defaultDestination;
-}
-
-MessageQueueId_t MessageQueue::getDefaultDestination() const { return defaultDestination; }
-
-bool MessageQueue::isDefaultDestinationSet() const { return defaultDestinationSet; }
-
 // static core function to send messages.
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
                                                        MessageQueueMessageIF* message,
--- a/src/fsfw/osal/freertos/MessageQueue.h
+++ b/src/fsfw/osal/freertos/MessageQueue.h
@ -1,12 +1,15 @@
 #ifndef FSFW_OSAL_FREERTOS_MESSAGEQUEUE_H_
 #define FSFW_OSAL_FREERTOS_MESSAGEQUEUE_H_

+#include <fsfw/ipc/MessageQueueBase.h>
+
 #include "FreeRTOS.h"
 #include "TaskManagement.h"
 #include "fsfw/internalerror/InternalErrorReporterIF.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
 #include "fsfw/ipc/MessageQueueMessageIF.h"
+#include "fsfw/ipc/definitions.h"
 #include "queue.h"

 /**
@ -32,7 +35,7 @@
 * @ingroup osal
 * @ingroup message_queue
 */
-class MessageQueue : public MessageQueueIF {
+class MessageQueue : public MessageQueueBase {
  friend class MessageQueueSenderIF;

 public:
@ -53,7 +56,8 @@ class MessageQueue : public MessageQueueIF {
   * This should be left default.
   */
  MessageQueue(size_t messageDepth = 3,
-               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
+               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE,
+               MqArgs* args = nullptr);

  /** Copying message queues forbidden */
  MessageQueue(const MessageQueue&) = delete;
@ -73,40 +77,15 @@ class MessageQueue : public MessageQueueIF {
   */
  void switchSystemContext(CallContext callContext);

-  /** MessageQueueIF implementation */
-  ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                            bool ignoreFault = false) override;
+  QueueHandle_t getNativeQueueHandle();

-  ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override;
-
-  ReturnValue_t reply(MessageQueueMessageIF* message) override;
+  // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase
  virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                        MessageQueueId_t sentFrom = NO_QUEUE,
                                        bool ignoreFault = false) override;
-
-  virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message,
-                                          MessageQueueId_t sentFrom = NO_QUEUE,
-                                          bool ignoreFault = false) override;
-
-  ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
-                               MessageQueueId_t* receivedFrom) override;
-
  ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override;
-
  ReturnValue_t flush(uint32_t* count) override;

-  MessageQueueId_t getLastPartner() const override;
-
-  MessageQueueId_t getId() const override;
-
-  void setDefaultDestination(MessageQueueId_t defaultDestination) override;
-
-  MessageQueueId_t getDefaultDestination() const override;
-
-  bool isDefaultDestinationSet() const override;
-
-  QueueHandle_t getNativeQueueHandle();
-
 protected:
  /**
   * @brief 	Implementation to be called from any send Call within
@ -136,12 +115,8 @@ class MessageQueue : public MessageQueueIF {
  static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);

 private:
-  bool defaultDestinationSet = false;
  QueueHandle_t handle;
-  MessageQueueId_t queueId = MessageQueueIF::NO_QUEUE;

-  MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE;
-  MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE;
  const size_t maxMessageSize;
  //! Stores the current system context
  CallContext callContext = CallContext::TASK;
--- a/src/fsfw/osal/freertos/QueueFactory.cpp
+++ b/src/fsfw/osal/freertos/QueueFactory.cpp
@ -22,8 +22,9 @@ QueueFactory::QueueFactory() {}

 QueueFactory::~QueueFactory() {}

-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) {
-  return new MessageQueue(messageDepth, maxMessageSize);
+MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
+                                                 MqArgs* args) {
+  return new MessageQueue(messageDepth, maxMessageSize, args);
 }

 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; }
--- a/src/fsfw/osal/host/MessageQueue.cpp
+++ b/src/fsfw/osal/host/MessageQueue.cpp
@ -8,10 +8,12 @@
 #include "fsfw/osal/host/QueueMapManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"

-MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize)
-    : messageSize(maxMessageSize), messageDepth(messageDepth) {
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args)
+    : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args),
+      messageSize(maxMessageSize),
+      messageDepth(messageDepth) {
  queueLock = MutexFactory::instance()->createMutex();
-  auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId);
+  auto result = QueueMapManager::instance()->addMessageQueue(this, &id);
  if (result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "MessageQueue::MessageQueue: Could not be created" << std::endl;
@ -23,42 +25,11 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize)

 MessageQueue::~MessageQueue() { MutexFactory::instance()->deleteMutex(queueLock); }

-ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                        bool ignoreFault) {
-  return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
-}
-
-ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
-  return sendToDefaultFrom(message, this->getId());
-}
-
-ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
-                                              MessageQueueId_t sentFrom, bool ignoreFault) {
-  return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
-}
-
-ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
-  if (this->lastPartner != MessageQueueIF::NO_QUEUE) {
-    return sendMessageFrom(this->lastPartner, message, this->getId());
-  } else {
-    return MessageQueueIF::NO_REPLY_PARTNER;
-  }
-}
-
 ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                            MessageQueueId_t sentFrom, bool ignoreFault) {
  return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault);
 }

-ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
-                                           MessageQueueId_t* receivedFrom) {
-  ReturnValue_t status = this->receiveMessage(message);
-  if (status == HasReturnvaluesIF::RETURN_OK) {
-    *receivedFrom = this->lastPartner;
-  }
-  return status;
-}
-
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
  if (messageQueue.empty()) {
    return MessageQueueIF::EMPTY;
@ -68,12 +39,10 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
            message->getBuffer());
  messageQueue.pop();
  // The last partner is the first uint32_t field in the message
-  this->lastPartner = message->getSender();
+  this->last = message->getSender();
  return HasReturnvaluesIF::RETURN_OK;
 }

-MessageQueueId_t MessageQueue::getLastPartner() const { return lastPartner; }
-
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
  *count = messageQueue.size();
  // Clears the queue.
@ -81,17 +50,6 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) {
  return HasReturnvaluesIF::RETURN_OK;
 }

-MessageQueueId_t MessageQueue::getId() const { return mqId; }
-
-void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
-  defaultDestinationSet = true;
-  this->defaultDestination = defaultDestination;
-}
-
-MessageQueueId_t MessageQueue::getDefaultDestination() const { return defaultDestination; }
-
-bool MessageQueue::isDefaultDestinationSet() const { return defaultDestinationSet; }
-
 // static core function to send messages.
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
                                                        MessageQueueMessageIF* message,
--- a/src/fsfw/osal/host/MessageQueue.h
+++ b/src/fsfw/osal/host/MessageQueue.h
@ -5,9 +5,11 @@
 #include <queue>

 #include "fsfw/internalerror/InternalErrorReporterIF.h"
+#include "fsfw/ipc/MessageQueueBase.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
 #include "fsfw/ipc/MutexIF.h"
+#include "fsfw/ipc/definitions.h"
 #include "fsfw/timemanager/Clock.h"

 /**
@ -33,7 +35,7 @@
 * @ingroup osal
 * @ingroup message_queue
 */
-class MessageQueue : public MessageQueueIF {
+class MessageQueue : public MessageQueueBase {
  friend class MessageQueueSenderIF;

 public:
@ -54,7 +56,8 @@ class MessageQueue : public MessageQueueIF {
   * This should be left default.
   */
  MessageQueue(size_t messageDepth = 3,
-               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
+               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE,
+               MqArgs* args = nullptr);

  /** Copying message queues forbidden */
  MessageQueue(const MessageQueue&) = delete;
@ -67,121 +70,12 @@ class MessageQueue : public MessageQueueIF {
   */
  virtual ~MessageQueue();

-  /**
-   * @brief	This operation sends a message to the given destination.
-   * @details	It directly uses the sendMessage call of the MessageQueueSender
-   * parent, but passes its queue id as "sentFrom" parameter.
-   * @param sendTo	This parameter specifies the message queue id of the
-   *  destination message queue.
-   * @param message	A pointer to a previously created message, which is sent.
-   * @param ignoreFault If set to true, the internal software fault counter
-   * is not incremented if queue is full.
-   */
-  ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                            bool ignoreFault = false) override;
-  /**
-   * @brief	This operation sends a message to the default destination.
-   * @details	As in the sendMessage method, this function uses the
-   * sendToDefault call of the MessageQueueSender parent class and adds its
-   * queue id as "sentFrom" information.
-   * @param message	A pointer to a previously created message, which is sent.
-   */
-  ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override;
-  /**
-   * @brief	This operation sends a message to the last communication partner.
-   * @details	This operation simplifies answering an incoming message by using
-   * the stored lastPartner information as destination. If there was no
-   * message received yet (i.e. lastPartner is zero), an error code is returned.
-   * @param message	A pointer to a previously created message, which is sent.
-   */
-  ReturnValue_t reply(MessageQueueMessageIF* message) override;
-
-  /**
-   * @brief	With the sendMessage call, a queue message is sent to a
-   * 			receiving queue.
-   * @details
-   * This method takes the message provided, adds the sentFrom information and
-   * passes it on to the destination provided with an operating system call.
-   *  The OS's return value is returned.
-   * @param sendTo	This parameter specifies the message queue id to send
-   * the message to.
-   * @param message	This is a pointer to a previously created message,
-   * which is sent.
-   * @param sentFrom	The sentFrom information can be set to inject the
-   * sender's queue id into the message. This variable is set to zero by
-   * default.
-   * @param ignoreFault If set to true, the internal software fault counter
-   * is not incremented if queue is full.
-   */
+  // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase
  virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                        MessageQueueId_t sentFrom = NO_QUEUE,
                                        bool ignoreFault = false) override;
-
-  /**
-   * @brief	The sendToDefault method sends a queue message to the default
-   * 			destination.
-   * @details
-   * In all other aspects, it works identical to the sendMessage method.
-   * @param message	This is a pointer to a previously created message,
-   * which is sent.
-   * @param sentFrom	The sentFrom information can be set to inject the
-   * sender's queue id into the message. This variable is set to zero by
-   * default.
-   */
-  virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message,
-                                          MessageQueueId_t sentFrom = NO_QUEUE,
-                                          bool ignoreFault = false) override;
-
-  /**
-   * @brief	This function reads available messages from the message queue
-   * 			and returns the sender.
-   * @details
-   * It works identically to the other receiveMessage call, but in  addition
-   * returns the sender's queue id.
-   * @param message	A pointer to a message in which the received data is stored.
-   * @param receivedFrom	A pointer to a queue id in which the sender's id is stored.
-   */
-  ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
-                               MessageQueueId_t* receivedFrom) override;
-
-  /**
-   * @brief	This function reads available messages from the message queue.
-   * @details
-   * If data is available it is stored in the passed message pointer.
-   * The message's original content is overwritten and the sendFrom
-   * information is stored in the lastPartner attribute. Else, the lastPartner
-   * information remains untouched, the message's content is cleared and the
-   * function returns immediately.
-   * @param message	A pointer to a message in which the received data is stored.
-   */
  ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override;
-  /**
-   * Deletes all pending messages in the queue.
-   * @param count The number of flushed messages.
-   * @return RETURN_OK on success.
-   */
  ReturnValue_t flush(uint32_t* count) override;
-  /**
-   * @brief	This method returns the message queue id of the last
-   * 			communication partner.
-   */
-  MessageQueueId_t getLastPartner() const override;
-  /**
-   * @brief	This method returns the message queue id of this class's
-   * 			message queue.
-   */
-  MessageQueueId_t getId() const override;
-
-  /**
-   * @brief	This method is a simple setter for the default destination.
-   */
-  void setDefaultDestination(MessageQueueId_t defaultDestination) override;
-  /**
-   * @brief	This method is a simple getter for the default destination.
-   */
-  MessageQueueId_t getDefaultDestination() const override;
-
-  bool isDefaultDestinationSet() const override;

  ReturnValue_t lockQueue(MutexIF::TimeoutType timeoutType, dur_millis_t lockTimeout);
  ReturnValue_t unlockQueue();
@ -211,23 +105,14 @@ class MessageQueue : public MessageQueueIF {
                                                   MessageQueueId_t sentFrom = NO_QUEUE,
                                                   bool ignoreFault = false);

-  // static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
-
 private:
  std::queue<std::vector<uint8_t>> messageQueue;
-  /**
-   * @brief	The class stores the queue id it got assigned.
-   * 			If initialization fails, the queue id is set to zero.
-   */
-  MessageQueueId_t mqId = MessageQueueIF::NO_QUEUE;
  size_t messageSize = 0;
  size_t messageDepth = 0;

  MutexIF* queueLock;

-  bool defaultDestinationSet = false;
  MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE;
-  MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE;
 };

 #endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */
--- a/src/fsfw/osal/host/QueueFactory.cpp
+++ b/src/fsfw/osal/host/QueueFactory.cpp
@ -27,12 +27,13 @@ QueueFactory::QueueFactory() {}

 QueueFactory::~QueueFactory() {}

-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) {
+MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
+                                                 MqArgs* args) {
  // A thread-safe queue can be implemented  by using a combination
  // of std::queue and std::mutex. This uses dynamic memory allocation
  // which could be alleviated by using a custom allocator, external library
  // (etl::queue) or simply using std::queue, we're on a host machine anyway.
-  return new MessageQueue(messageDepth, maxMessageSize);
+  return new MessageQueue(messageDepth, maxMessageSize, args);
 }

 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; }
--- a/src/fsfw/osal/linux/Clock.cpp
+++ b/src/fsfw/osal/linux/Clock.cpp
@ -6,6 +6,7 @@
 #include <time.h>
 #include <unistd.h>

+#include <cstring>
 #include <fstream>

 #include "fsfw/serviceinterface/ServiceInterface.h"
@ -13,6 +14,8 @@
 uint16_t Clock::leapSeconds = 0;
 MutexIF* Clock::timeMutex = NULL;

+void handleClockError(const char* func);
+
 uint32_t Clock::getTicksPerSecond(void) {
  uint32_t ticks = sysconf(_SC_CLK_TCK);
  return ticks;
@ -27,7 +30,7 @@ ReturnValue_t Clock::setClock(const TimeOfDay_t* time) {

  int status = clock_settime(CLOCK_REALTIME, &timeUnix);
  if (status != 0) {
-    // TODO errno
+    handleClockError("setClock");
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  return HasReturnvaluesIF::RETURN_OK;
@ -39,7 +42,7 @@ ReturnValue_t Clock::setClock(const timeval* time) {
  timeUnix.tv_nsec = (__syscall_slong_t)time->tv_usec * 1000;
  int status = clock_settime(CLOCK_REALTIME, &timeUnix);
  if (status != 0) {
-    // TODO errno
+    handleClockError("setClock");
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  return HasReturnvaluesIF::RETURN_OK;
@ -49,6 +52,7 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) {
  timespec timeUnix;
  int status = clock_gettime(CLOCK_REALTIME, &timeUnix);
  if (status != 0) {
+    handleClockError("getClock_timeval");
    return HasReturnvaluesIF::RETURN_FAILED;
  }
  time->tv_sec = timeUnix.tv_sec;
@ -151,3 +155,14 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) {
  *JD2000 = (time.tv_sec - 946728000. + time.tv_usec / 1000000.) / 24. / 3600.;
  return HasReturnvaluesIF::RETURN_OK;
 }
+
+void handleClockError(const char* func) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::warning << "Clock::" << func << ": Failed with code " << errno << ": " << strerror(errno)
+               << std::endl;
+#else
+  sif::printWarning("Clock::%s: Failed with code %d: %s\n", func, errno, strerror(errno));
+#endif
+#endif
+}
--- a/src/fsfw/osal/linux/MessageQueue.cpp
+++ b/src/fsfw/osal/linux/MessageQueue.cpp
@ -11,13 +11,10 @@
 #include "fsfw/osal/linux/unixUtility.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"

-MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize)
-    : id(MessageQueueIF::NO_QUEUE),
-      lastPartner(MessageQueueIF::NO_QUEUE),
-      defaultDestination(MessageQueueIF::NO_QUEUE),
+MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args)
+    : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args),
      maxMessageSize(maxMessageSize) {
  mq_attr attributes;
-  this->id = 0;
  // Set attributes
  attributes.mq_curmsgs = 0;
  attributes.mq_maxmsg = messageDepth;
@ -50,30 +47,6 @@ MessageQueue::~MessageQueue() {
  }
 }

-ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                        bool ignoreFault) {
-  return sendMessageFrom(sendTo, message, this->getId(), false);
-}
-
-ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
-  return sendToDefaultFrom(message, this->getId());
-}
-
-ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
-  if (this->lastPartner != 0) {
-    return sendMessageFrom(this->lastPartner, message, this->getId());
-  } else {
-    return NO_REPLY_PARTNER;
-  }
-}
-
-ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
-                                           MessageQueueId_t* receivedFrom) {
-  ReturnValue_t status = this->receiveMessage(message);
-  *receivedFrom = this->lastPartner;
-  return status;
-}
-
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
  if (message == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@ -96,7 +69,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
  int status = mq_receive(id, reinterpret_cast<char*>(message->getBuffer()),
                          message->getMaximumMessageSize(), &messagePriority);
  if (status > 0) {
-    this->lastPartner = message->getSender();
+    this->last = message->getSender();
    // Check size of incoming message.
    if (message->getMessageSize() < message->getMinimumMessageSize()) {
      return HasReturnvaluesIF::RETURN_FAILED;
@ -164,8 +137,6 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
  }
 }

-MessageQueueId_t MessageQueue::getLastPartner() const { return this->lastPartner; }
-
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
  mq_attr attrib;
  int status = mq_getattr(id, &attrib);
@ -212,26 +183,11 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) {
  return HasReturnvaluesIF::RETURN_OK;
 }

-MessageQueueId_t MessageQueue::getId() const { return this->id; }
-
-void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
-  this->defaultDestination = defaultDestination;
-}
-
-ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
-                                              MessageQueueId_t sentFrom, bool ignoreFault) {
-  return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
-}
-
 ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                            MessageQueueId_t sentFrom, bool ignoreFault) {
  return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault);
 }

-MessageQueueId_t MessageQueue::getDefaultDestination() const { return this->defaultDestination; }
-
-bool MessageQueue::isDefaultDestinationSet() const { return (defaultDestination != NO_QUEUE); }
-
 uint16_t MessageQueue::queueCounter = 0;

 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
@ -240,9 +196,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
                                                        bool ignoreFault) {
  if (message == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr!" << std::endl;
+    sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr" << std::endl;
 #else
-    sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr!\n");
+    sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr\n");
 #endif
    return HasReturnvaluesIF::RETURN_FAILED;
  }
@ -256,7 +212,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
    if (!ignoreFault) {
      InternalErrorReporterIF* internalErrorReporter =
          ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
-      if (internalErrorReporter != NULL) {
+      if (internalErrorReporter != nullptr) {
        internalErrorReporter->queueMessageNotSent();
      }
    }
--- a/src/fsfw/osal/linux/MessageQueue.h
+++ b/src/fsfw/osal/linux/MessageQueue.h
@ -1,11 +1,13 @@
 #ifndef FSFW_OSAL_LINUX_MESSAGEQUEUE_H_
 #define FSFW_OSAL_LINUX_MESSAGEQUEUE_H_

+#include <fsfw/ipc/MessageQueueBase.h>
 #include <mqueue.h>

 #include "fsfw/internalerror/InternalErrorReporterIF.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
+#include "fsfw/ipc/definitions.h"
 /**
 * @brief		This class manages sending and receiving of message queue messages.
 *
@ -25,7 +27,7 @@
 * makes use of the operating system calls provided.
 *	@ingroup message_queue
 */
-class MessageQueue : public MessageQueueIF {
+class MessageQueue : public MessageQueueBase {
  friend class MessageQueueSenderIF;

 public:
@ -42,104 +44,24 @@ class MessageQueue : public MessageQueueIF {
   * 							This should be left default.
   */
  MessageQueue(uint32_t messageDepth = 3,
-               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
+               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE,
+               MqArgs* args = nullptr);
+
+  /** Copying message queues forbidden */
+  MessageQueue(const MessageQueue&) = delete;
+  MessageQueue& operator=(const MessageQueue&) = delete;
+
  /**
   * @brief	The destructor deletes the formerly created message queue.
   * @details	This is accomplished by using the delete call provided by the operating system.
   */
  virtual ~MessageQueue();
-  /**
-   * @brief	This operation sends a message to the given destination.
-   * @details	It directly uses the sendMessage call of the MessageQueueSender parent, but passes
-   * its queue id as "sentFrom" parameter.
-   * @param sendTo	This parameter specifies the message queue id of the destination message
-   * queue.
-   * @param message	A pointer to a previously created message, which is sent.
-   * @param ignoreFault If set to true, the internal software fault counter is not incremented if
-   * queue is full.
-   */
-  virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                    bool ignoreFault = false);
-  /**
-   * @brief	This operation sends a message to the default destination.
-   * @details	As in the sendMessage method, this function uses the sendToDefault call of the
-   * 			MessageQueueSender parent class and adds its queue id as "sentFrom"
-   * information.
-   * @param message	A pointer to a previously created message, which is sent.
-   */
-  virtual ReturnValue_t sendToDefault(MessageQueueMessageIF* message);
-  /**
-   * @brief	This operation sends a message to the last communication partner.
-   * @details	This operation simplifies answering an incoming message by using the stored
-   * 			lastParnter information as destination. If there was no message received yet
-   * 			(i.e. lastPartner is zero), an error code is returned.
-   * @param message	A pointer to a previously created message, which is sent.
-   */
-  ReturnValue_t reply(MessageQueueMessageIF* message);

-  /**
-   * @brief	This function reads available messages from the message queue and returns the
-   * sender.
-   * @details	It works identically to the other receiveMessage call, but in addition returns the
-   * 			sender's queue id.
-   * @param message	A pointer to a message in which the received data is stored.
-   * @param receivedFrom	A pointer to a queue id in which the sender's id is stored.
-   */
-  ReturnValue_t receiveMessage(MessageQueueMessageIF* message, MessageQueueId_t* receivedFrom);
-
-  /**
-   * @brief	This function reads available messages from the message queue.
-   * @details	If data is available it is stored in the passed message pointer. The message's
-   * 			original content is overwritten and the sendFrom information is stored in
-   * the lastPartner attribute. Else, the lastPartner information remains untouched, the message's
-   * content is cleared and the function returns immediately.
-   * @param message	A pointer to a message in which the received data is stored.
-   */
-  ReturnValue_t receiveMessage(MessageQueueMessageIF* message);
-  /**
-   * Deletes all pending messages in the queue.
-   * @param count The number of flushed messages.
-   * @return RETURN_OK on success.
-   */
-  ReturnValue_t flush(uint32_t* count);
-  /**
-   * @brief	This method returns the message queue id of the last communication partner.
-   */
-  MessageQueueId_t getLastPartner() const;
-  /**
-   * @brief	This method returns the message queue id of this class's message queue.
-   */
-  MessageQueueId_t getId() const;
-  /**
-   * \brief	With the sendMessage call, a queue message is sent to a receiving queue.
-   * \param sendTo	This parameter specifies the message queue id to send the message to.
-   * \param message	This is a pointer to a previously created message, which is sent.
-   * \param sentFrom	The sentFrom information can be set to inject the sender's queue id into the
-   * message. This variable is set to zero by default. \param ignoreFault If set to true, the
-   * internal software fault counter is not incremented if queue is full.
-   */
-  virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                        MessageQueueId_t sentFrom, bool ignoreFault = false);
-  /**
-   * \brief	The sendToDefault method sends a queue message to the default destination.
-   * \details	In all other aspects, it works identical to the sendMessage method.
-   * \param message	This is a pointer to a previously created message, which is sent.
-   * \param sentFrom	The sentFrom information can be set to inject the sender's queue id into the
-   * message. This variable is set to zero by default.
-   */
-  virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message,
-                                          MessageQueueId_t sentFrom = NO_QUEUE,
-                                          bool ignoreFault = false);
-  /**
-   * \brief	This method is a simple setter for the default destination.
-   */
-  void setDefaultDestination(MessageQueueId_t defaultDestination);
-  /**
-   * \brief	This method is a simple getter for the default destination.
-   */
-  MessageQueueId_t getDefaultDestination() const;
-
-  bool isDefaultDestinationSet() const;
+  // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase
+  ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override;
+  ReturnValue_t flush(uint32_t* count) override;
+  ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
+                                MessageQueueId_t sentFrom, bool ignoreFault = false) override;

 protected:
  /**
@ -158,31 +80,10 @@ class MessageQueue : public MessageQueueIF {
                                                   bool ignoreFault = false);

 private:
-  /**
-   * @brief	The class stores the queue id it got assigned from the operating system in this
-   * attribute. If initialization fails, the queue id is set to zero.
-   */
-  MessageQueueId_t id;
-  /**
-   * @brief	In this attribute, the queue id of the last communication partner is stored
-   * 			to allow for replying.
-   */
-  MessageQueueId_t lastPartner;
-  /**
-   * @brief	The message queue's name -a user specific information for the operating system-  is
-   * 			generated automatically with the help of this static counter.
-   */
-  /**
-   * \brief	This attribute stores a default destination to send messages to.
-   * \details	It is stored to simplify sending to always-the-same receiver. The attribute may
-   * 			be set in the constructor or by a setter call to setDefaultDestination.
-   */
-  MessageQueueId_t defaultDestination;
-
  /**
   * The name of the message queue, stored for unlinking
   */
-  char name[16];
+  char name[16] = {};

  static uint16_t queueCounter;
  const size_t maxMessageSize;
--- a/src/fsfw/osal/linux/QueueFactory.cpp
+++ b/src/fsfw/osal/linux/QueueFactory.cpp
@ -28,8 +28,9 @@ QueueFactory::QueueFactory() {}

 QueueFactory::~QueueFactory() {}

-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) {
-  return new MessageQueue(messageDepth, maxMessageSize);
+MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
+                                                 MqArgs* args) {
+  return new MessageQueue(messageDepth, maxMessageSize, args);
 }

 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; }
--- a/src/fsfw/osal/rtems/MessageQueue.cpp
+++ b/src/fsfw/osal/rtems/MessageQueue.cpp
@ -6,8 +6,9 @@
 #include "fsfw/osal/rtems/RtemsBasic.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"

-MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size)
-    : id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(nullptr) {
+MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size, MqArgs* args)
+    : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args),
+      internalErrorReporter(nullptr) {
  rtems_name name = ('Q' << 24) + (queueCounter++ << 8);
  rtems_status_code status =
      rtems_message_queue_create(name, message_depth, max_message_size, 0, &(this->id));
@ -16,43 +17,19 @@ MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size)
    sif::error << "MessageQueue::MessageQueue: Creating Queue " << std::hex << name << std::dec
               << " failed with status:" << (uint32_t)status << std::endl;
 #endif
-    this->id = 0;
+    this->id = MessageQueueIF::NO_QUEUE;
  }
 }

 MessageQueue::~MessageQueue() { rtems_message_queue_delete(id); }

-ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                        bool ignoreFault) {
-  return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
-}
-
-ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
-  return sendToDefaultFrom(message, this->getId());
-}
-
-ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
-  if (this->lastPartner != 0) {
-    return sendMessage(this->lastPartner, message, this->getId());
-  } else {
-    return NO_REPLY_PARTNER;
-  }
-}
-
-ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
-                                           MessageQueueId_t* receivedFrom) {
-  ReturnValue_t status = this->receiveMessage(message);
-  *receivedFrom = this->lastPartner;
-  return status;
-}
-
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
  size_t size = 0;
  rtems_status_code status =
      rtems_message_queue_receive(id, message->getBuffer(), &size, RTEMS_NO_WAIT, 1);
  if (status == RTEMS_SUCCESSFUL) {
    message->setMessageSize(size);
-    this->lastPartner = message->getSender();
+    this->last = message->getSender();
    // Check size of incoming message.
    if (message->getMessageSize() < message->getMinimumMessageSize()) {
      return HasReturnvaluesIF::RETURN_FAILED;
@ -65,19 +42,11 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
  return convertReturnCode(status);
 }

-MessageQueueId_t MessageQueue::getLastPartner() const { return this->lastPartner; }
-
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
  rtems_status_code status = rtems_message_queue_flush(id, count);
  return convertReturnCode(status);
 }

-MessageQueueId_t MessageQueue::getId() const { return this->id; }
-
-void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
-  this->defaultDestination = defaultDestination;
-}
-
 ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                            MessageQueueId_t sentFrom, bool ignoreFault) {
  message->setSender(sentFrom);
@ -103,15 +72,6 @@ ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueu
  return returnCode;
 }

-ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
-                                              MessageQueueId_t sentFrom, bool ignoreFault) {
-  return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
-}
-
-MessageQueueId_t MessageQueue::getDefaultDestination() const { return this->defaultDestination; }
-
-bool MessageQueue::isDefaultDestinationSet() const { return (defaultDestination != NO_QUEUE); }
-
 ReturnValue_t MessageQueue::convertReturnCode(rtems_status_code inValue) {
  switch (inValue) {
    case RTEMS_SUCCESSFUL:
--- a/src/fsfw/osal/rtems/MessageQueue.h
+++ b/src/fsfw/osal/rtems/MessageQueue.h
@ -1,10 +1,13 @@
 #ifndef FSFW_OSAL_RTEMS_MESSAGEQUEUE_H_
 #define FSFW_OSAL_RTEMS_MESSAGEQUEUE_H_

+#include <fsfw/ipc/MessageQueueBase.h>
+
 #include "RtemsBasic.h"
 #include "fsfw/internalerror/InternalErrorReporterIF.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
+#include "fsfw/ipc/definitions.h"

 /**
 *	@brief		This class manages sending and receiving of message queue messages.
@ -19,7 +22,7 @@
 *as well as sending and receiving messages, the class makes use of the operating system calls
 *provided. \ingroup message_queue
 */
-class MessageQueue : public MessageQueueIF {
+class MessageQueue : public MessageQueueBase {
 public:
  /**
   * @brief	The constructor initializes and configures the message queue.
@ -34,131 +37,26 @@ class MessageQueue : public MessageQueueIF {
   * 							This should be left default.
   */
  MessageQueue(size_t message_depth = 3,
-               size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE);
+               size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE,
+               MqArgs* args = nullptr);
+
+  /** Copying message queues forbidden */
+  MessageQueue(const MessageQueue&) = delete;
+  MessageQueue& operator=(const MessageQueue&) = delete;
+
  /**
   * @brief	The destructor deletes the formerly created message queue.
   * @details	This is accomplished by using the delete call provided by the operating system.
   */
  virtual ~MessageQueue();
-  /**
-   * @brief	This operation sends a message to the given destination.
-   * @details	It directly uses the sendMessage call of the MessageQueueSender parent, but passes
-   * its queue id as "sentFrom" parameter.
-   * @param sendTo	This parameter specifies the message queue id of the destination message
-   * queue.
-   * @param message	A pointer to a previously created message, which is sent.
-   * @param ignoreFault If set to true, the internal software fault counter is not incremented if
-   * queue is full.
-   */
-  ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                            bool ignoreFault = false);
-  /**
-   * @brief	This operation sends a message to the default destination.
-   * @details	As in the sendMessage method, this function uses the sendToDefault call of the
-   * 			MessageQueueSender parent class and adds its queue id as "sentFrom"
-   * information.
-   * @param message	A pointer to a previously created message, which is sent.
-   */
-  ReturnValue_t sendToDefault(MessageQueueMessageIF* message);
-  /**
-   * @brief	This operation sends a message to the last communication partner.
-   * @details	This operation simplifies answering an incoming message by using the stored
-   * 			lastParnter information as destination. If there was no message received yet
-   * 			(i.e. lastPartner is zero), an error code is returned.
-   * @param message	A pointer to a previously created message, which is sent.
-   */
-  ReturnValue_t reply(MessageQueueMessageIF* message);

-  /**
-   * @brief	This function reads available messages from the message queue and returns the
-   * sender.
-   * @details	It works identically to the other receiveMessage call, but in addition returns the
-   * 			sender's queue id.
-   * @param message	A pointer to a message in which the received data is stored.
-   * @param receivedFrom	A pointer to a queue id in which the sender's id is stored.
-   */
-  ReturnValue_t receiveMessage(MessageQueueMessageIF* message, MessageQueueId_t* receivedFrom);
-
-  /**
-   * @brief	This function reads available messages from the message queue.
-   * @details	If data is available it is stored in the passed message pointer. The message's
-   * 			original content is overwritten and the sendFrom information is stored in
-   * the lastPartner attribute. Else, the lastPartner information remains untouched, the message's
-   * content is cleared and the function returns immediately.
-   * @param message	A pointer to a message in which the received data is stored.
-   */
-  ReturnValue_t receiveMessage(MessageQueueMessageIF* message);
-  /**
-   * Deletes all pending messages in the queue.
-   * @param count The number of flushed messages.
-   * @return RETURN_OK on success.
-   */
-  ReturnValue_t flush(uint32_t* count);
-  /**
-   * @brief	This method returns the message queue id of the last communication partner.
-   */
-  MessageQueueId_t getLastPartner() const;
-  /**
-   * @brief	This method returns the message queue id of this class's message queue.
-   */
-  MessageQueueId_t getId() const;
-  /**
-   * \brief	With the sendMessage call, a queue message is sent to a receiving queue.
-   * \details	This method takes the message provided, adds the sentFrom information and passes
-   * 			it on to the destination provided with an operating system call. The OS's
-   * return value is returned.
-   * \param sendTo	This parameter specifies the message queue id to send the message to.
-   * \param message	This is a pointer to a previously created message, which is sent.
-   * \param sentFrom	The sentFrom information can be set to inject the sender's queue id into the
-   * message. This variable is set to zero by default. \param ignoreFault If set to true, the
-   * internal software fault counter is not incremented if queue is full.
-   */
-  virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
+  // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase
+  ReturnValue_t flush(uint32_t* count) override;
+  ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                MessageQueueId_t sentFrom = NO_QUEUE,
-                                        bool ignoreFault = false);
-  /**
-   * \brief	The sendToDefault method sends a queue message to the default destination.
-   * \details	In all other aspects, it works identical to the sendMessage method.
-   * \param message	This is a pointer to a previously created message, which is sent.
-   * \param sentFrom	The sentFrom information can be set to inject the sender's queue id into the
-   * message. This variable is set to zero by default.
-   */
-  virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message,
-                                          MessageQueueId_t sentFrom = NO_QUEUE,
-                                          bool ignoreFault = false);
-  /**
-   * \brief	This method is a simple setter for the default destination.
-   */
-  void setDefaultDestination(MessageQueueId_t defaultDestination);
-  /**
-   * \brief	This method is a simple getter for the default destination.
-   */
-  MessageQueueId_t getDefaultDestination() const;
-
-  bool isDefaultDestinationSet() const;
+                                bool ignoreFault = false) override;

 private:
-  /**
-   * @brief	The class stores the queue id it got assigned from the operating system in this
-   * attribute. If initialization fails, the queue id is set to zero.
-   */
-  MessageQueueId_t id;
-  /**
-   * @brief	In this attribute, the queue id of the last communication partner is stored
-   * 			to allow for replying.
-   */
-  MessageQueueId_t lastPartner;
-  /**
-   * @brief	The message queue's name -a user specific information for the operating system-  is
-   * 			generated automatically with the help of this static counter.
-   */
-  /**
-   * \brief	This attribute stores a default destination to send messages to.
-   * \details	It is stored to simplify sending to always-the-same receiver. The attribute may
-   * 			be set in the constructor or by a setter call to setDefaultDestination.
-   */
-  MessageQueueId_t defaultDestination;
-
  /**
   * \brief	This attribute stores a reference to the internal error reporter for reporting full
   * queues. \details	In the event of a full destination queue, the reporter will be notified. The
--- a/src/fsfw/osal/rtems/QueueFactory.cpp
+++ b/src/fsfw/osal/rtems/QueueFactory.cpp
@ -49,8 +49,9 @@ QueueFactory::QueueFactory() {}

 QueueFactory::~QueueFactory() {}

-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) {
-  return new MessageQueue(messageDepth, maxMessageSize);
+MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
+                                                 MqArgs* args) {
+  return new MessageQueue(messageDepth, maxMessageSize, args);
 }

 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; }
--- a/src/fsfw/parameters/HasParametersIF.h
+++ b/src/fsfw/parameters/HasParametersIF.h
@ -66,7 +66,7 @@ class HasParametersIF {
   * @param newValues
   * @param startAtIndex Linear index, runs left to right, top to bottom for
   * matrix indexes.
-   * @return
+   * @return RETURN_OK if parameter is valid and a set function of the parameter wrapper was called.
   */
  virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
                                     ParameterWrapper *parameterWrapper,
--- a/src/fsfw/parameters/ParameterWrapper.cpp
+++ b/src/fsfw/parameters/ParameterWrapper.cpp
@ -211,9 +211,13 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
  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;
+    sif::warning << "ParameterWrapper::copyFrom: Called on read-only variable or "
+                    "data pointer not set"
+                 << std::endl;
 #else
-    sif::printWarning("ParameterWrapper::copyFrom: Called on read-only variable!\n");
+    sif::printWarning(
+        "ParameterWrapper::copyFrom: Called on read-only variable "
+        "or data pointer not set\n");
 #endif
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
    return READONLY;
@ -222,9 +226,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
  if (from->readonlyData == nullptr) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "ParameterWrapper::copyFrom: Source not set!" << std::endl;
+    sif::warning << "ParameterWrapper::copyFrom: Source not set" << std::endl;
 #else
-    sif::printWarning("ParameterWrapper::copyFrom: Source not set!\n");
+    sif::printWarning("ParameterWrapper::copyFrom: Source not set\n");
 #endif
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
    return SOURCE_NOT_SET;
@ -233,9 +237,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
  if (type != from->type) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "ParameterWrapper::copyFrom: Datatype missmatch!" << std::endl;
+    sif::warning << "ParameterWrapper::copyFrom: Datatype missmatch" << std::endl;
 #else
-    sif::printWarning("ParameterWrapper::copyFrom: Datatype missmatch!\n");
+    sif::printWarning("ParameterWrapper::copyFrom: Datatype missmatch\n");
 #endif
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
    return DATATYPE_MISSMATCH;
@ -245,9 +249,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
  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;
+    sif::warning << "ParameterWrapper::copyFrom: Columns or rows zero" << std::endl;
 #else
-    sif::printWarning("ParameterWrapper::copyFrom: Columns or rows zero!\n");
+    sif::printWarning("ParameterWrapper::copyFrom: Columns or rows zero\n");
 #endif
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
    return COLUMN_OR_ROWS_ZERO;
--- a/src/fsfw/power/CMakeLists.txt
+++ b/src/fsfw/power/CMakeLists.txt
@ -1,7 +1,8 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
+target_sources(${LIB_FSFW_NAME} PRIVATE
    Fuse.cpp
    PowerComponent.cpp
    PowerSensor.cpp
    PowerSwitcher.cpp
+    DummyPowerSwitcher.cpp
+    PowerSwitcherComponent.cpp
 )
--- a/src/fsfw/power/DummyPowerSwitcher.cpp
+++ b/src/fsfw/power/DummyPowerSwitcher.cpp
@ -0,0 +1,46 @@
+#include "DummyPowerSwitcher.h"
+
+DummyPowerSwitcher::DummyPowerSwitcher(object_id_t objectId, size_t numberOfSwitches,
+                                       size_t numberOfFuses, uint32_t switchDelayMs)
+    : SystemObject(objectId),
+      switcherList(numberOfSwitches),
+      fuseList(numberOfFuses),
+      switchDelayMs(switchDelayMs) {}
+
+void DummyPowerSwitcher::setInitialSwitcherList(std::vector<ReturnValue_t> switcherList) {
+  this->switcherList = switcherList;
+}
+
+void DummyPowerSwitcher::setInitialFusesList(std::vector<ReturnValue_t> fuseList) {
+  this->fuseList = fuseList;
+}
+
+ReturnValue_t DummyPowerSwitcher::sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) {
+  if (switchNr < switcherList.capacity()) {
+    switcherList[switchNr] = onOff;
+  }
+  return RETURN_FAILED;
+}
+
+ReturnValue_t DummyPowerSwitcher::sendFuseOnCommand(uint8_t fuseNr) {
+  if (fuseNr < fuseList.capacity()) {
+    fuseList[fuseNr] = FUSE_ON;
+  }
+  return RETURN_FAILED;
+}
+
+ReturnValue_t DummyPowerSwitcher::getSwitchState(power::Switch_t switchNr) const {
+  if (switchNr < switcherList.capacity()) {
+    return switcherList[switchNr];
+  }
+  return HasReturnvaluesIF::RETURN_FAILED;
+}
+
+ReturnValue_t DummyPowerSwitcher::getFuseState(uint8_t fuseNr) const {
+  if (fuseNr < fuseList.capacity()) {
+    return fuseList[fuseNr];
+  }
+  return HasReturnvaluesIF::RETURN_FAILED;
+}
+
+uint32_t DummyPowerSwitcher::getSwitchDelayMs(void) const { return switchDelayMs; }
--- a/src/fsfw/power/DummyPowerSwitcher.h
+++ b/src/fsfw/power/DummyPowerSwitcher.h
@ -0,0 +1,31 @@
+#ifndef FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_
+#define FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_
+
+#include <cstddef>
+#include <vector>
+
+#include "PowerSwitchIF.h"
+#include "definitions.h"
+#include "fsfw/objectmanager/SystemObject.h"
+
+class DummyPowerSwitcher : public SystemObject, public PowerSwitchIF {
+ public:
+  DummyPowerSwitcher(object_id_t objectId, size_t numberOfSwitches, size_t numberOfFuses,
+                     uint32_t switchDelayMs = 5000);
+
+  void setInitialSwitcherList(std::vector<ReturnValue_t> switcherList);
+  void setInitialFusesList(std::vector<ReturnValue_t> switcherList);
+
+  virtual ReturnValue_t sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) override;
+  virtual ReturnValue_t sendFuseOnCommand(uint8_t fuseNr) override;
+  virtual ReturnValue_t getSwitchState(power::Switch_t switchNr) const override;
+  virtual ReturnValue_t getFuseState(uint8_t fuseNr) const override;
+  virtual uint32_t getSwitchDelayMs(void) const override;
+
+ private:
+  std::vector<ReturnValue_t> switcherList;
+  std::vector<ReturnValue_t> fuseList;
+  uint32_t switchDelayMs = 5000;
+};
+
+#endif /* FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ */
--- a/src/fsfw/power/Fuse.h
+++ b/src/fsfw/power/Fuse.h
@ -34,14 +34,14 @@ class Fuse : public SystemObject,
  };

  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_1;
-  static const Event FUSE_CURRENT_HIGH = MAKE_EVENT(
-      1, severity::LOW);  //!< PSS detected that current on a fuse is totally out of bounds.
-  static const Event FUSE_WENT_OFF =
-      MAKE_EVENT(2, severity::LOW);  //!< PSS detected a fuse that went off.
-  static const Event POWER_ABOVE_HIGH_LIMIT =
-      MAKE_EVENT(4, severity::LOW);  //!< PSS detected a fuse that violates its limits.
-  static const Event POWER_BELOW_LOW_LIMIT =
-      MAKE_EVENT(5, severity::LOW);  //!< PSS detected a fuse that violates its limits.
+  //! PSS detected that current on a fuse is totally out of bounds.
+  static const Event FUSE_CURRENT_HIGH = MAKE_EVENT(1, severity::LOW);
+  //! PSS detected a fuse that went off.
+  static const Event FUSE_WENT_OFF = MAKE_EVENT(2, severity::LOW);
+  //! PSS detected a fuse that violates its limits.
+  static const Event POWER_ABOVE_HIGH_LIMIT = MAKE_EVENT(4, severity::LOW);
+  //! PSS detected a fuse that violates its limits.
+  static const Event POWER_BELOW_LOW_LIMIT = MAKE_EVENT(5, severity::LOW);

  typedef std::list<PowerComponentIF *> DeviceList;
  Fuse(object_id_t fuseObjectId, uint8_t fuseId, sid_t variableSet, VariableIds ids,
--- a/src/fsfw/power/PowerSensor.cpp
+++ b/src/fsfw/power/PowerSensor.cpp
@ -15,7 +15,9 @@ PowerSensor::PowerSensor(object_id_t objectId, sid_t setId, VariableIds ids, Def
                   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();
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 PowerSensor::~PowerSensor() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
--- a/src/fsfw/power/PowerSwitchIF.h
+++ b/src/fsfw/power/PowerSwitchIF.h
@ -3,6 +3,7 @@

 #include "../events/Event.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
+#include "definitions.h"
 /**
 *
 * @brief   This interface defines a connection to a device that is capable of
@ -37,11 +38,11 @@ class PowerSwitchIF : public HasReturnvaluesIF {
   * @param switchNr
   * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON
   */
-  virtual void sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const = 0;
+  virtual ReturnValue_t sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) = 0;
  /**
   * Sends a command to the Power Unit to enable a certain fuse.
   */
-  virtual void sendFuseOnCommand(uint8_t fuseNr) const = 0;
+  virtual ReturnValue_t sendFuseOnCommand(uint8_t fuseNr) = 0;

  /**
   * get the state of the Switches.
@ -51,7 +52,7 @@ class PowerSwitchIF : public HasReturnvaluesIF {
   *     - @c SWITCH_OFF if the specified switch is off.
   *     - @c RETURN_FAILED if an error occured
   */
-  virtual ReturnValue_t getSwitchState(uint8_t switchNr) const = 0;
+  virtual ReturnValue_t getSwitchState(power::Switch_t switchNr) const = 0;
  /**
   * get state of a fuse.
   * @param fuseNr
--- a/src/fsfw/power/PowerSwitcher.cpp
+++ b/src/fsfw/power/PowerSwitcher.cpp
@ -1,19 +1,12 @@
 #include "fsfw/power/PowerSwitcher.h"

+#include "definitions.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/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;
-}
+PowerSwitcher::PowerSwitcher(PowerSwitchIF* switcher, power::Switch_t setSwitch1,
+                             power::Switch_t setSwitch2, PowerSwitcher::State_t setStartState)
+    : power(switcher), state(setStartState), firstSwitch(setSwitch1), secondSwitch(setSwitch2) {}

 ReturnValue_t PowerSwitcher::getStateOfSwitches() {
  SwitchReturn_t result = howManySwitches();
@ -52,18 +45,37 @@ void PowerSwitcher::commandSwitches(ReturnValue_t onOff) {
  return;
 }

-void PowerSwitcher::turnOn() {
+void PowerSwitcher::turnOn(bool checkCurrentState) {
+  if (checkCurrentState) {
+    if (getStateOfSwitches() == PowerSwitchIF::SWITCH_ON) {
+      state = SWITCH_IS_ON;
+      return;
+    }
+  }
  commandSwitches(PowerSwitchIF::SWITCH_ON);
  state = WAIT_ON;
 }

-void PowerSwitcher::turnOff() {
+void PowerSwitcher::turnOff(bool checkCurrentState) {
+  if (checkCurrentState) {
+    if (getStateOfSwitches() == PowerSwitchIF::SWITCH_OFF) {
+      state = SWITCH_IS_OFF;
+      return;
+    }
+  }
  commandSwitches(PowerSwitchIF::SWITCH_OFF);
  state = WAIT_OFF;
 }

+bool PowerSwitcher::active() {
+  if (state == WAIT_OFF or state == WAIT_ON) {
+    return true;
+  }
+  return false;
+}
+
 PowerSwitcher::SwitchReturn_t PowerSwitcher::howManySwitches() {
-  if (secondSwitch == NO_SWITCH) {
+  if (secondSwitch == power::NO_SWITCH) {
    return ONE_SWITCH;
  } else {
    return TWO_SWITCHES;
--- a/src/fsfw/power/PowerSwitcher.h
+++ b/src/fsfw/power/PowerSwitcher.h
@ -14,28 +14,28 @@ class PowerSwitcher : public HasReturnvaluesIF {
    SWITCH_IS_OFF,
    SWITCH_IS_ON,
  };
-  State_t state;
+
  static const uint8_t INTERFACE_ID = CLASS_ID::POWER_SWITCHER;
  static const ReturnValue_t IN_POWER_TRANSITION = MAKE_RETURN_CODE(1);
  static const ReturnValue_t SWITCH_STATE_MISMATCH = MAKE_RETURN_CODE(2);
-  PowerSwitcher(uint8_t setSwitch1, uint8_t setSwitch2 = NO_SWITCH,
+  PowerSwitcher(PowerSwitchIF* switcher, uint8_t setSwitch1, uint8_t setSwitch2 = power::NO_SWITCH,
                State_t setStartState = SWITCH_IS_OFF);
-  ReturnValue_t initialize(object_id_t powerSwitchId);
-  void turnOn();
-  void turnOff();
+  void turnOn(bool checkCurrentState = true);
+  void turnOff(bool checkCurrentState = true);
+  bool active();
  void doStateMachine();
  State_t getState();
  ReturnValue_t checkSwitchState();
  uint32_t getSwitchDelay();
-  uint8_t getFirstSwitch() const;
-  uint8_t getSecondSwitch() const;
+  power::Switch_t getFirstSwitch() const;
+  power::Switch_t getSecondSwitch() const;

 private:
-  uint8_t firstSwitch;
-  uint8_t secondSwitch;
  PowerSwitchIF* power = nullptr;
+  State_t state;
+  power::Switch_t firstSwitch = power::NO_SWITCH;
+  power::Switch_t secondSwitch = power::NO_SWITCH;

-  static const uint8_t NO_SWITCH = 0xFF;
  enum SwitchReturn_t { ONE_SWITCH = 1, TWO_SWITCHES = 2 };
  ReturnValue_t getStateOfSwitches();
  void commandSwitches(ReturnValue_t onOff);
--- a/src/fsfw/power/PowerSwitcherComponent.cpp
+++ b/src/fsfw/power/PowerSwitcherComponent.cpp
@ -0,0 +1,107 @@
+#include "PowerSwitcherComponent.h"
+
+#include <fsfw/ipc/QueueFactory.h>
+#include <fsfw/power/PowerSwitchIF.h>
+
+PowerSwitcherComponent::PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF *pwrSwitcher,
+                                               power::Switch_t pwrSwitch)
+    : SystemObject(objectId),
+      switcher(pwrSwitcher, pwrSwitch),
+      modeHelper(this),
+      healthHelper(this, objectId) {
+  queue = QueueFactory::instance()->createMessageQueue();
+}
+
+ReturnValue_t PowerSwitcherComponent::performOperation(uint8_t opCode) {
+  ReturnValue_t result;
+  CommandMessage command;
+
+  for (result = queue->receiveMessage(&command); result == RETURN_OK;
+       result = queue->receiveMessage(&command)) {
+    result = healthHelper.handleHealthCommand(&command);
+    if (result == RETURN_OK) {
+      continue;
+    }
+
+    result = modeHelper.handleModeCommand(&command);
+    if (result == RETURN_OK) {
+      continue;
+    }
+  }
+  if (switcher.active()) {
+    switcher.doStateMachine();
+    auto currState = switcher.getState();
+    if (currState == PowerSwitcher::SWITCH_IS_OFF) {
+      setMode(MODE_OFF, 0);
+    } else if (currState == PowerSwitcher::SWITCH_IS_ON) {
+      setMode(MODE_ON, 0);
+    }
+  }
+  return RETURN_OK;
+}
+
+ReturnValue_t PowerSwitcherComponent::initialize() {
+  ReturnValue_t result = modeHelper.initialize();
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  result = healthHelper.initialize();
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  return SystemObject::initialize();
+}
+
+MessageQueueId_t PowerSwitcherComponent::getCommandQueue() const { return queue->getId(); }
+
+void PowerSwitcherComponent::getMode(Mode_t *mode, Submode_t *submode) {
+  *mode = this->mode;
+  *submode = this->submode;
+}
+
+ReturnValue_t PowerSwitcherComponent::setHealth(HealthState health) {
+  healthHelper.setHealth(health);
+  return RETURN_OK;
+}
+
+ReturnValue_t PowerSwitcherComponent::checkModeCommand(Mode_t mode, Submode_t submode,
+                                                       uint32_t *msToReachTheMode) {
+  *msToReachTheMode = 5000;
+  if (mode != MODE_ON and mode != MODE_OFF) {
+    return TRANS_NOT_ALLOWED;
+  }
+  return RETURN_OK;
+}
+
+void PowerSwitcherComponent::startTransition(Mode_t mode, Submode_t submode) {
+  if (mode == MODE_OFF) {
+    switcher.turnOff(true);
+    switcher.doStateMachine();
+    if (switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) {
+      setMode(MODE_OFF, 0);
+    }
+  } else if (mode == MODE_ON) {
+    switcher.turnOn(true);
+    switcher.doStateMachine();
+    if (switcher.getState() == PowerSwitcher::SWITCH_IS_ON) {
+      setMode(MODE_ON, 0);
+    }
+  }
+}
+
+void PowerSwitcherComponent::setToExternalControl() {
+  healthHelper.setHealth(HasHealthIF::EXTERNAL_CONTROL);
+}
+
+void PowerSwitcherComponent::announceMode(bool recursive) {
+  triggerEvent(MODE_INFO, mode, submode);
+}
+
+void PowerSwitcherComponent::setMode(Mode_t newMode, Submode_t newSubmode) {
+  this->mode = newMode;
+  this->submode = newSubmode;
+  modeHelper.modeChanged(mode, submode);
+  announceMode(false);
+}
+
+HasHealthIF::HealthState PowerSwitcherComponent::getHealth() { return healthHelper.getHealth(); }
--- a/src/fsfw/power/PowerSwitcherComponent.h
+++ b/src/fsfw/power/PowerSwitcherComponent.h
@ -0,0 +1,62 @@
+#ifndef _FSFW_POWER_POWERSWITCHERCOMPONENT_H_
+#define _FSFW_POWER_POWERSWITCHERCOMPONENT_H_
+
+#include <fsfw/health/HasHealthIF.h>
+#include <fsfw/health/HealthHelper.h>
+#include <fsfw/modes/HasModesIF.h>
+#include <fsfw/modes/ModeHelper.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/power/PowerSwitcher.h>
+#include <fsfw/power/definitions.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+
+class PowerSwitchIF;
+
+/**
+ * @brief Allows to create an power switch object with its own mode and health
+ * @details
+ * This basic component allows to create an object which is solely responsible for managing a
+ * switch. It also has a mode and a health by implementing the respective interface components
+ * which allows integrating this component into a system mode tree.
+ *
+ * Commanding this component to MODE_OFF will cause the switcher to turn the switch off while
+ * commanding in to MODE_ON will cause the switcher to turn the switch on.
+ */
+class PowerSwitcherComponent : public SystemObject,
+                               public HasReturnvaluesIF,
+                               public ExecutableObjectIF,
+                               public HasModesIF,
+                               public HasHealthIF {
+ public:
+  PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF *pwrSwitcher,
+                         power::Switch_t pwrSwitch);
+
+ private:
+  MessageQueueIF *queue = nullptr;
+  PowerSwitcher switcher;
+
+  Mode_t mode = MODE_OFF;
+  Submode_t submode = 0;
+
+  ModeHelper modeHelper;
+  HealthHelper healthHelper;
+
+  void setMode(Mode_t newMode, Submode_t newSubmode);
+
+  virtual ReturnValue_t performOperation(uint8_t opCode) override;
+
+  ReturnValue_t initialize() override;
+
+  MessageQueueId_t getCommandQueue() const override;
+  void getMode(Mode_t *mode, Submode_t *submode) override;
+  ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
+                                 uint32_t *msToReachTheMode) override;
+  void startTransition(Mode_t mode, Submode_t submode) override;
+  void setToExternalControl() override;
+  void announceMode(bool recursive) override;
+
+  ReturnValue_t setHealth(HealthState health) override;
+  HasHealthIF::HealthState getHealth() override;
+};
+
+#endif /* _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ */
--- a/src/fsfw/power/definitions.h
+++ b/src/fsfw/power/definitions.h
@ -0,0 +1,13 @@
+#ifndef FSFW_SRC_FSFW_POWER_DEFINITIONS_H_
+#define FSFW_SRC_FSFW_POWER_DEFINITIONS_H_
+
+#include <cstdint>
+
+namespace power {
+
+using Switch_t = uint8_t;
+static constexpr Switch_t NO_SWITCH = 0xFF;
+
+}  // namespace power
+
+#endif /* FSFW_SRC_FSFW_POWER_DEFINITIONS_H_ */
--- a/src/fsfw/pus/Service1TelecommandVerification.cpp
+++ b/src/fsfw/pus/Service1TelecommandVerification.cpp
@ -16,7 +16,9 @@ Service1TelecommandVerification::Service1TelecommandVerification(object_id_t obj
      apid(apid),
      serviceId(serviceId),
      targetDestination(targetDestination) {
-  tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  tmQueue = QueueFactory::instance()->createMessageQueue(
+      messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 Service1TelecommandVerification::~Service1TelecommandVerification() {
--- a/src/fsfw/pus/Service3Housekeeping.cpp
+++ b/src/fsfw/pus/Service3Housekeeping.cpp
@ -214,11 +214,11 @@ ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply,
    default:
 #if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::warning << "Service3Housekeeping::handleReply: Invalid reply with "
-                   << "reply command " << command << "!" << std::endl;
+                   << "reply command " << command << std::endl;
 #else
      sif::printWarning(
          "Service3Housekeeping::handleReply: Invalid reply with "
-          "reply command %hu!\n",
+          "reply command %hu\n",
          command);
 #endif
      return CommandingServiceBase::INVALID_REPLY;
--- a/src/fsfw/pus/Service5EventReporting.cpp
+++ b/src/fsfw/pus/Service5EventReporting.cpp
@ -12,7 +12,9 @@ Service5EventReporting::Service5EventReporting(object_id_t objectId, uint16_t ap
                                               uint32_t messageQueueDepth)
    : PusServiceBase(objectId, apid, serviceId),
      maxNumberReportsPerCycle(maxNumberReportsPerCycle) {
-  eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  eventQueue = QueueFactory::instance()->createMessageQueue(
+      messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 Service5EventReporting::~Service5EventReporting() {
@ -36,9 +38,6 @@ ReturnValue_t Service5EventReporting::performService() {
      }
    }
  }
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-  sif::warning << "Service5EventReporting::generateEventReport: Too many events" << std::endl;
-#endif
  return HasReturnvaluesIF::RETURN_OK;
 }

@ -87,7 +86,7 @@ ReturnValue_t Service5EventReporting::handleRequest(uint8_t subservice) {
 // to be registered to the event manager to listen for events.
 ReturnValue_t Service5EventReporting::initialize() {
  EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
-  if (manager == NULL) {
+  if (manager == nullptr) {
    return RETURN_FAILED;
  }
  // register Service 5 as listener for events
--- a/src/fsfw/pus/Service5EventReporting.h
+++ b/src/fsfw/pus/Service5EventReporting.h
@ -41,7 +41,7 @@
 class Service5EventReporting : public PusServiceBase {
 public:
  Service5EventReporting(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                         size_t maxNumberReportsPerCycle = 10, uint32_t messageQueueDepth = 10);
+                         size_t maxNumberReportsPerCycle, uint32_t messageQueueDepth);
  virtual ~Service5EventReporting();

  /***
--- a/src/fsfw/pus/Service9TimeManagement.h
+++ b/src/fsfw/pus/Service9TimeManagement.h
@ -6,10 +6,10 @@
 class Service9TimeManagement : public PusServiceBase {
 public:
  static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9;
-  static constexpr Event CLOCK_SET =
-      MAKE_EVENT(0, severity::INFO);  //!< Clock has been set. P1: New Uptime. P2: Old Uptime
-  static constexpr Event CLOCK_SET_FAILURE =
-      MAKE_EVENT(1, severity::LOW);  //!< Clock could not be set. P1: Returncode.
+  //!< Clock has been set. P1: New Uptime. P2: Old Uptime
+  static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO);
+  //!< Clock could not be set. P1: Returncode.
+  static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(1, severity::LOW);

  static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9;

--- a/src/fsfw/subsystem/Subsystem.cpp
+++ b/src/fsfw/subsystem/Subsystem.cpp
@ -30,11 +30,11 @@ ReturnValue_t Subsystem::checkSequence(HybridIterator<ModeListEntry> iter,
    return FALLBACK_SEQUENCE_DOES_NOT_EXIST;
  }

-  if (iter.value == NULL) {
+  if (iter.value == nullptr) {
    return NO_TARGET_TABLE;
  }

-  for (; iter.value != NULL; ++iter) {
+  for (; iter.value != nullptr; ++iter) {
    if (!existsModeTable(iter->getTableId())) {
      return TABLE_DOES_NOT_EXIST;
    } else {
@ -66,13 +66,18 @@ HybridIterator<ModeListEntry> Subsystem::getCurrentTable() {
 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 (currentSequenceIterator.value == nullptr) {  // 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());
+          Mode_t tableId = 0;
+          auto seq = getSequence(targetMode);
+          if (seq.value != nullptr) {
+            tableId = seq->getTableId();
+          }
+          transitionFailed(TARGET_TABLE_NOT_REACHED, tableId);
          return;
        }
      }
@ -248,10 +253,13 @@ ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) {
    case ModeSequenceMessage::READ_TABLE: {
      ReturnValue_t result;
      Mode_t table = ModeSequenceMessage::getSequenceId(message);
-      EntryPointer *entry = NULL;
+      EntryPointer *entry = nullptr;
      result = modeTables.find(table, &entry);
-      if (result != RETURN_OK) {
+      if (result != RETURN_OK or entry == nullptr) {
        replyToCommand(result, 0);
+        if (entry == nullptr) {
+          return result;
+        }
      }

      SerializeIF *elements[2];
--- a/src/fsfw/subsystem/Subsystem.h
+++ b/src/fsfw/subsystem/Subsystem.h
@ -1,19 +1,22 @@
 #ifndef FSFW_SUBSYSTEM_SUBSYSTEM_H_
 #define FSFW_SUBSYSTEM_SUBSYSTEM_H_

-#include <FSFWConfig.h>
-
 #include "../container/FixedArrayList.h"
 #include "../container/FixedMap.h"
 #include "../container/HybridIterator.h"
 #include "../container/SinglyLinkedList.h"
 #include "../serialize/SerialArrayListAdapter.h"
 #include "SubsystemBase.h"
+#include "fsfw/FSFW.h"
 #include "modes/ModeDefinitions.h"

 /**
- * @brief   TODO: documentation missing
+ * @brief   This class extends the SubsystemBase to perform the management of mode tables
+ *          and mode sequences
 * @details
+ * This class is able to use mode tables and sequences to command all its children into the
+ * right mode. Fallback sequences can be used to handle failed transitions or have a fallback
+ * in case a component can't keep its current mode.
 */
 class Subsystem : public SubsystemBase, public HasModeSequenceIF {
 public:
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/src/fsfw/subsystem/SubsystemBase.cpp
@ -8,11 +8,13 @@ SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t
                             uint16_t commandQueueDepth)
    : SystemObject(setObjectId),
      mode(initialMode),
-      commandQueue(QueueFactory::instance()->createMessageQueue(commandQueueDepth,
-                                                                CommandMessage::MAX_MESSAGE_SIZE)),
      healthHelper(this, setObjectId),
      modeHelper(this),
-      parentId(parent) {}
+      parentId(parent) {
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE, &mqArgs);
+}

 SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }

@ -31,8 +33,9 @@ ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
      info.mode = MODE_OFF;
    }
  } else {
+    // intentional to force an initial command during system startup
    info.commandQueue = child->getCommandQueue();
-    info.mode = -1;  // intentional to force an initial command during system startup
+    info.mode = HasModesIF::MODE_UNDEFINED;
  }

  info.submode = SUBMODE_NONE;
--- a/src/fsfw/subsystem/SubsystemBase.h
+++ b/src/fsfw/subsystem/SubsystemBase.h
@ -15,7 +15,14 @@

 /**
 * @defgroup subsystems Subsystem Objects
- * Contains all Subsystem and Assemblies
+ * All Subsystem and Assemblies can derive from this class. It contains helper classes to
+ * perform mode and health handling, which allows OBSW developers to build a mode tree for
+ * the whole satellite.
+ *
+ * Aside from setting up a mode tree and being able to executing mode tables, this class does not
+ * provide an implementation on what to do with the features. To build a mode tree, helper classes
+ * like the #AssemblyBase or the #Subsystem class extend and use the functionality of the base
+ * class.
 */
 class SubsystemBase : public SystemObject,
                      public HasModesIF,
@ -96,6 +103,7 @@ class SubsystemBase : public SystemObject,
                                       Submode_t targetSubmode);

  /**
+   * This function takes care of sending all according mode commands specified inside a mode table.
   * We need to know the target Submode, as children are able to inherit the submode
   * Still, we have a default for all child implementations which do not use submode inheritance
   */
@ -123,11 +131,11 @@ class SubsystemBase : public SystemObject,
  virtual void performChildOperation() = 0;

  virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
-                                         uint32_t *msToReachTheMode) = 0;
+                                         uint32_t *msToReachTheMode) override = 0;

-  virtual void startTransition(Mode_t mode, Submode_t submode) = 0;
+  virtual void startTransition(Mode_t mode, Submode_t submode) override = 0;

-  virtual void getMode(Mode_t *mode, Submode_t *submode);
+  virtual void getMode(Mode_t *mode, Submode_t *submode) override;

  virtual void setToExternalControl();

--- a/src/fsfw/tcdistribution/TcDistributor.cpp
+++ b/src/fsfw/tcdistribution/TcDistributor.cpp
@ -5,7 +5,9 @@
 #include "fsfw/tmtcservices/TmTcMessage.h"

 TcDistributor::TcDistributor(object_id_t objectId) : SystemObject(objectId) {
-  tcQueue = QueueFactory::instance()->createMessageQueue(DISTRIBUTER_MAX_PACKETS);
+  auto mqArgs = MqArgs(objectId);
+  tcQueue = QueueFactory::instance()->createMessageQueue(
+      DISTRIBUTER_MAX_PACKETS, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 TcDistributor::~TcDistributor() { QueueFactory::instance()->deleteMessageQueue(tcQueue); }
--- a/src/fsfw/thermal/AbstractTemperatureSensor.cpp
+++ b/src/fsfw/thermal/AbstractTemperatureSensor.cpp
@ -4,14 +4,13 @@

 AbstractTemperatureSensor::AbstractTemperatureSensor(object_id_t setObjectid,
                                                     ThermalModuleIF *thermalModule)
-    : SystemObject(setObjectid),
-      commandQueue(NULL),
-      healthHelper(this, setObjectid),
-      parameterHelper(this) {
-  if (thermalModule != NULL) {
+    : SystemObject(setObjectid), healthHelper(this, setObjectid), parameterHelper(this) {
+  if (thermalModule != nullptr) {
    thermalModule->registerSensor(this);
  }
-  commandQueue = QueueFactory::instance()->createMessageQueue();
+  auto mqArgs = MqArgs(setObjectid, static_cast<void *>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 AbstractTemperatureSensor::~AbstractTemperatureSensor() {
--- a/src/fsfw/thermal/AbstractTemperatureSensor.h
+++ b/src/fsfw/thermal/AbstractTemperatureSensor.h
@ -51,7 +51,7 @@ class AbstractTemperatureSensor : public HasHealthIF,
  HasHealthIF::HealthState getHealth();

 protected:
-  MessageQueueIF* commandQueue;
+  MessageQueueIF* commandQueue = nullptr;
  HealthHelper healthHelper;
  ParameterHelper parameterHelper;

--- a/src/fsfw/thermal/Heater.cpp
+++ b/src/fsfw/thermal/Heater.cpp
@ -12,7 +12,9 @@ Heater::Heater(uint32_t objectId, uint8_t switch0, uint8_t switch1)
      switch1(switch1),
      heaterOnCountdown(10800000) /*about two orbits*/,
      parameterHelper(this) {
-  eventQueue = QueueFactory::instance()->createMessageQueue();
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  eventQueue = QueueFactory::instance()->createMessageQueue(
+      3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 Heater::~Heater() { QueueFactory::instance()->deleteMessageQueue(eventQueue); }
--- a/src/fsfw/timemanager/Clock.h
+++ b/src/fsfw/timemanager/Clock.h
@ -99,6 +99,13 @@ class Clock {
   */
  static ReturnValue_t getDateAndTime(TimeOfDay_t *time);

+  /**
+   * Convert to time of day struct given the POSIX timeval struct
+   * @param from
+   * @param to
+   * @return
+   */
+  static ReturnValue_t convertTimevalToTimeOfDay(const timeval *from, TimeOfDay_t *to);
  /**
   * Converts a time of day struct to POSIX seconds.
   * @param time The time of day as input
--- a/src/fsfw/timemanager/ClockCommon.cpp
+++ b/src/fsfw/timemanager/ClockCommon.cpp
@ -1,3 +1,5 @@
+#include <ctime>
+
 #include "fsfw/ipc/MutexGuard.h"
 #include "fsfw/timemanager/Clock.h"

@ -42,6 +44,19 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
  return HasReturnvaluesIF::RETURN_OK;
 }

+ReturnValue_t Clock::convertTimevalToTimeOfDay(const timeval* from, TimeOfDay_t* to) {
+  struct tm* timeInfo;
+  timeInfo = gmtime(&from->tv_sec);
+  to->year = timeInfo->tm_year + 1900;
+  to->month = timeInfo->tm_mon + 1;
+  to->day = timeInfo->tm_mday;
+  to->hour = timeInfo->tm_hour;
+  to->minute = timeInfo->tm_min;
+  to->second = timeInfo->tm_sec;
+  to->usecond = from->tv_usec;
+  return HasReturnvaluesIF::RETURN_OK;
+}
+
 ReturnValue_t Clock::checkOrCreateClockMutex() {
  if (timeMutex == nullptr) {
    MutexFactory* mutexFactory = MutexFactory::instance();
--- a/src/fsfw/tmstorage/TmStoreBackendIF.h
+++ b/src/fsfw/tmstorage/TmStoreBackendIF.h
@ -33,50 +33,47 @@ class TmStoreBackendIF : public HasParametersIF {
  static const ReturnValue_t INVALID_REQUEST = MAKE_RETURN_CODE(15);

  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MEMORY;
-  static const Event STORE_SEND_WRITE_FAILED =
-      MAKE_EVENT(0, severity::LOW);  //!< Initiating sending data to store failed. Low, par1:
-                                     //!< returnCode, par2: integer (debug info)
-  static const Event STORE_WRITE_FAILED = MAKE_EVENT(
-      1, severity::LOW);  //!< Data was sent, but writing failed. Low, par1: returnCode, par2: 0
-  static const Event STORE_SEND_READ_FAILED =
-      MAKE_EVENT(2, severity::LOW);  //!< Initiating reading data from store failed. Low, par1:
-                                     //!< returnCode, par2: 0
-  static const Event STORE_READ_FAILED = MAKE_EVENT(
-      3, severity::LOW);  //!< Data was requested, but access failed. Low, par1: returnCode, par2: 0
-  static const Event UNEXPECTED_MSG =
-      MAKE_EVENT(4, severity::LOW);  //!< An unexpected TM packet or data message occurred. Low,
-                                     //!< par1: 0, par2: integer (debug info)
-  static const Event STORING_FAILED = MAKE_EVENT(
-      5, severity::LOW);  //!< Storing data failed. May simply be a full store. Low, par1:
-                          //!< returnCode, par2: integer (sequence count of failed packet).
-  static const Event TM_DUMP_FAILED =
-      MAKE_EVENT(6, severity::LOW);  //!< Dumping retrieved data failed. Low, par1: returnCode,
-                                     //!< par2: integer (sequence count of failed packet).
-  static const Event STORE_INIT_FAILED =
-      MAKE_EVENT(7, severity::LOW);  //!< Corrupted init data or read error. Low, par1: returnCode,
-                                     //!< par2: integer (debug info)
-  static const Event STORE_INIT_EMPTY = MAKE_EVENT(
-      8, severity::INFO);  //!< Store was not initialized. Starts empty. Info, parameters both zero.
-  static const Event STORE_CONTENT_CORRUPTED =
-      MAKE_EVENT(9, severity::LOW);  //!< Data was read out, but it is inconsistent. Low par1:
-                                     //!< Memory address of corruption, par2: integer (debug info)
-  static const Event STORE_INITIALIZE =
-      MAKE_EVENT(10, severity::INFO);  //!< Info event indicating the store will be initialized,
-                                       //!< either at boot or after IOB switch. Info. pars: 0
-  static const Event INIT_DONE = MAKE_EVENT(
-      11, severity::INFO);  //!< Info event indicating the store was successfully initialized,
-                            //!< either at boot or after IOB switch. Info. pars: 0
-  static const Event DUMP_FINISHED = MAKE_EVENT(
-      12, severity::INFO);  //!< Info event indicating that dumping finished successfully. par1:
-                            //!< Number of dumped packets. par2: APID/SSC (16bits each)
-  static const Event DELETION_FINISHED = MAKE_EVENT(
-      13, severity::INFO);  //!< Info event indicating that deletion finished successfully. par1:
-                            //!< Number of deleted packets. par2: APID/SSC (16bits each)
-  static const Event DELETION_FAILED = MAKE_EVENT(
-      14,
-      severity::LOW);  //!< Info event indicating that something went wrong during deletion. pars: 0
-  static const Event AUTO_CATALOGS_SENDING_FAILED =
-      MAKE_EVENT(15, severity::INFO);  //!< Info that the a auto catalog report failed
+  //! Initiating sending data to store failed. Low, par1:
+  //! returnCode, par2: integer (debug info)
+  static const Event STORE_SEND_WRITE_FAILED = MAKE_EVENT(0, severity::LOW);
+  //! Data was sent, but writing failed. Low, par1: returnCode, par2: 0
+  static const Event STORE_WRITE_FAILED = MAKE_EVENT(1, severity::LOW);
+  //! Initiating reading data from store failed. Low, par1: returnCode, par2: 0
+  static const Event STORE_SEND_READ_FAILED = MAKE_EVENT(2, severity::LOW);
+  //! Data was requested, but access failed. Low, par1: returnCode, par2: 0
+  static const Event STORE_READ_FAILED = MAKE_EVENT(3, severity::LOW);
+  //! An unexpected TM packet or data message occurred. Low, par1: 0, par2: integer (debug info)
+  static const Event UNEXPECTED_MSG = MAKE_EVENT(4, severity::LOW);
+  //! Storing data failed. May simply be a full store. Low, par1: returnCode,
+  //! par2: integer (sequence count of failed packet).
+  static const Event STORING_FAILED = MAKE_EVENT(5, severity::LOW);
+  //! Dumping retrieved data failed. Low, par1: returnCode,
+  //! par2: integer (sequence count of failed packet).
+  static const Event TM_DUMP_FAILED = MAKE_EVENT(6, severity::LOW);
+  //! Corrupted init data or read error. Low, par1: returnCode, par2: integer (debug info)
+  //! Store was not initialized. Starts empty. Info, parameters both zero.
+  static const Event STORE_INIT_FAILED = MAKE_EVENT(7, severity::LOW);
+  //! Data was read out, but it is inconsistent. Low par1:
+  //! Memory address of corruption, par2: integer (debug info)
+  static const Event STORE_INIT_EMPTY = MAKE_EVENT(8, severity::INFO);
+
+  static const Event STORE_CONTENT_CORRUPTED = MAKE_EVENT(9, severity::LOW);
+  //! Info event indicating the store will be initialized, either at boot or after IOB switch.
+  //! Info. pars: 0
+  static const Event STORE_INITIALIZE = MAKE_EVENT(10, severity::INFO);
+  //! Info event indicating the store was successfully initialized, either at boot or after
+  //! IOB switch. Info. pars: 0
+  static const Event INIT_DONE = MAKE_EVENT(11, severity::INFO);
+  //! Info event indicating that dumping finished successfully.
+  //! par1: Number of dumped packets. par2: APID/SSC (16bits each)
+  static const Event DUMP_FINISHED = MAKE_EVENT(12, severity::INFO);
+  //! Info event indicating that deletion finished successfully.
+  //! par1:Number of deleted packets. par2: APID/SSC (16bits each)
+  static const Event DELETION_FINISHED = MAKE_EVENT(13, severity::INFO);
+  //! Info event indicating that something went wrong during deletion. pars: 0
+  static const Event DELETION_FAILED = MAKE_EVENT(14, severity::LOW);
+  //! Info that the a auto catalog report failed
+  static const Event AUTO_CATALOGS_SENDING_FAILED = MAKE_EVENT(15, severity::INFO);

  virtual ~TmStoreBackendIF() {}

--- a/src/fsfw/tmtcpacket/SpacePacket.h
+++ b/src/fsfw/tmtcpacket/SpacePacket.h
@ -25,7 +25,7 @@ class SpacePacket : public SpacePacketBase {
   * @param apid Sets the packet's APID field. The default value describes an idle packet.
   * @param sequenceCount ets the packet's Source Sequence Count field.
   */
-  SpacePacket(uint16_t packetDataLength, bool isTelecommand = false,
+  SpacePacket(uint16_t packetDataLength = 0, bool isTelecommand = false,
              uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0);
  /**
   * The class's default destructor.
--- a/src/fsfw/tmtcservices/CommandingServiceBase.cpp
+++ b/src/fsfw/tmtcservices/CommandingServiceBase.cpp
@ -20,8 +20,10 @@ CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, uint16_t a
      service(service),
      timeoutSeconds(commandTimeoutSeconds),
      commandMap(numberOfParallelCommands) {
-  commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
-  requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  size_t mqSz = MessageQueueMessage::MAX_MESSAGE_SIZE;
+  commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth, mqSz, &mqArgs);
+  requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth, mqSz, &mqArgs);
 }

 void CommandingServiceBase::setPacketSource(object_id_t packetSource) {
--- a/src/fsfw/tmtcservices/PusServiceBase.cpp
+++ b/src/fsfw/tmtcservices/PusServiceBase.cpp
@ -13,7 +13,9 @@ object_id_t PusServiceBase::packetDestination = 0;

 PusServiceBase::PusServiceBase(object_id_t setObjectId, uint16_t setApid, uint8_t setServiceId)
    : SystemObject(setObjectId), apid(setApid), serviceId(setServiceId) {
-  requestQueue = QueueFactory::instance()->createMessageQueue(PUS_SERVICE_MAX_RECEPTION);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  requestQueue = QueueFactory::instance()->createMessageQueue(
+      PUS_SERVICE_MAX_RECEPTION, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 PusServiceBase::~PusServiceBase() { QueueFactory::instance()->deleteMessageQueue(requestQueue); }
--- a/src/fsfw/tmtcservices/SourceSequenceCounter.h
+++ b/src/fsfw/tmtcservices/SourceSequenceCounter.h
@ -19,6 +19,19 @@ class SourceSequenceCounter {
  void reset(uint16_t toValue = 0) {
    sequenceCount = toValue % (SpacePacketBase::LIMIT_SEQUENCE_COUNT);
  }
+  SourceSequenceCounter& operator++(int) {
+    this->increment();
+    return *this;
+  }
+  SourceSequenceCounter& operator--(int) {
+    this->decrement();
+    return *this;
+  }
+  SourceSequenceCounter& operator=(const uint16_t& newCount) {
+    sequenceCount = newCount;
+    return *this;
+  }
+  operator uint16_t() { return this->get(); }
 };

 #endif /* FSFW_TMTCSERVICES_SOURCESEQUENCECOUNTER_H_ */
--- a/src/fsfw/tmtcservices/TmTcBridge.cpp
+++ b/src/fsfw/tmtcservices/TmTcBridge.cpp
@ -15,7 +15,9 @@ TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination, object_i
      tcDestination(tcDestination)

 {
-  tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(
+      TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }

 TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); }
@ -172,6 +174,7 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) {
  }

  if (tmFifo->full()) {
+    if (warningSwitch) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number "
                      "of stored packet IDs reached!"
@ -181,6 +184,8 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) {
          "TmTcBridge::storeDownlinkData: TM downlink max. number "
          "of stored packet IDs reached!\n");
 #endif
+      warningSwitch = true;
+    }
    if (overwriteOld) {
      tmFifo->retrieve(&storeId);
      tmStore->deleteData(storeId);
--- a/src/fsfw/tmtcservices/TmTcBridge.h
+++ b/src/fsfw/tmtcservices/TmTcBridge.h
@ -72,6 +72,8 @@ class TmTcBridge : public AcceptsTelemetryIF,
  virtual uint16_t getIdentifier() override;
  virtual MessageQueueId_t getRequestQueue() override;

+  bool warningSwitch = true;
+
 protected:
  //! Cached for initialize function.
  object_id_t tmStoreId = objects::NO_OBJECT;
--- a/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolManagerTest.cpp
+++ b/tests/src/fsfw_tests/unit/datapoollocal/LocalPoolManagerTest.cpp
@ -21,8 +21,10 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
  REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK);
  REQUIRE(poolOwner->initializeHkManagerAfterTaskCreation() == retval::CATCH_OK);

-  MessageQueueMockBase* mqMock = poolOwner->getMockQueueHandle();
-  REQUIRE(mqMock != nullptr);
+  MessageQueueMockBase* poolOwnerMock = poolOwner->getMockQueueHandle();
+  REQUIRE(poolOwnerMock != nullptr);
+
+  // MessageQueueIF* hkCommander = QueueFactory::instance()->createMessageQueue();
  CommandMessage messageSent;
  uint8_t messagesSent = 0;

@ -41,9 +43,9 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    poolOwner->dataset.setChanged(true);
    /* Now the update message should be generated. */
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent() == true);
+    REQUIRE(poolOwnerMock->wasMessageSent() == true);

-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() ==
          static_cast<int>(HousekeepingMessage::UPDATE_NOTIFICATION_SET));

@ -53,9 +55,9 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    poolOwner->dataset.setChanged(true);
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);

-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() ==
          static_cast<int>(HousekeepingMessage::UPDATE_NOTIFICATION_SET));

@ -63,15 +65,15 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    REQUIRE(poolOwner->subscribeWrapperSetUpdateHk() == retval::CATCH_OK);
    poolOwner->dataset.setChanged(true);
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 2);
    /* first message sent should be the update notification, considering
    the internal list is a vector checked in insertion order. */
-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() ==
          static_cast<int>(HousekeepingMessage::UPDATE_NOTIFICATION_SET));

-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() == static_cast<int>(HousekeepingMessage::HK_REPORT));
    /* Clear message to avoid memory leak, our mock won't do it for us (yet) */
    CommandMessageCleaner::clearCommandMessage(&messageSent);
@ -99,9 +101,9 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {

    /* Trigger generation of snapshot */
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    /* Check that snapshot was generated */
    CHECK(messageSent.getCommand() == static_cast<int>(HousekeepingMessage::UPDATE_SNAPSHOT_SET));
    /* Now we deserialize the snapshot into a new dataset instance */
@ -162,12 +164,12 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);

    /* Check update snapshot was sent. */
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);

    /* Should have been reset. */
    CHECK(poolVar->hasChanged() == false);
-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() ==
          static_cast<int>(HousekeepingMessage::UPDATE_SNAPSHOT_VARIABLE));
    /* Now we deserialize the snapshot into a new dataset instance */
@ -209,11 +211,11 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);

    /* Check update notification was sent. */
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
    /* Should have been reset. */
    CHECK(poolVar->hasChanged() == false);
-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() ==
          static_cast<int>(HousekeepingMessage::UPDATE_NOTIFICATION_VARIABLE));
    /* Now subscribe for the dataset update (HK and update) again with subscription interface */
@ -225,26 +227,26 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    poolOwner->dataset.setChanged(true);
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);
    /* Now two messages should be sent. */
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 2);
-    mqMock->clearMessages(true);
+    poolOwnerMock->clearMessages(true);

    poolOwner->dataset.setChanged(true);
    poolVar->setChanged(true);
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);
    /* Now three messages should be sent. */
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 3);
-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() ==
          static_cast<int>(HousekeepingMessage::UPDATE_NOTIFICATION_VARIABLE));
-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() ==
          static_cast<int>(HousekeepingMessage::UPDATE_NOTIFICATION_SET));
-    REQUIRE(mqMock->receiveMessage(&messageSent) == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == retval::CATCH_OK);
    CHECK(messageSent.getCommand() == static_cast<int>(HousekeepingMessage::HK_REPORT));
    CommandMessageCleaner::clearCommandMessage(&messageSent);
-    REQUIRE(mqMock->receiveMessage(&messageSent) == static_cast<int>(MessageQueueIF::EMPTY));
+    REQUIRE(poolOwnerMock->receiveMessage(&messageSent) == static_cast<int>(MessageQueueIF::EMPTY));
  }

  SECTION("PeriodicHKAndMessaging") {
@ -255,38 +257,38 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    REQUIRE(poolOwner->subscribePeriodicHk(true) == retval::CATCH_OK);
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);
    /* Now HK packet should be sent as message immediately. */
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    LocalPoolDataSetBase* setHandle = poolOwner->getDataSetHandle(lpool::testSid);
    REQUIRE(setHandle != nullptr);
    CHECK(poolOwner->poolManager.generateHousekeepingPacket(lpool::testSid, setHandle, false) ==
          retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    CHECK(setHandle->getReportingEnabled() == true);
    CommandMessage hkCmd;
    HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, false, false);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
    CHECK(setHandle->getReportingEnabled() == false);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, true, false);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
    CHECK(setHandle->getReportingEnabled() == true);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, false, false);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
    CHECK(setHandle->getReportingEnabled() == false);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setCollectionIntervalModificationCommand(&hkCmd, lpool::testSid, 0.4,
                                                                  false);
@ -294,23 +296,23 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    /* For non-diagnostics and a specified minimum frequency of 0.2 seconds, the
    resulting collection interval should be 1.0 second */
    CHECK(poolOwner->dataset.getCollectionInterval() == 1.0);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setStructureReportingCommand(&hkCmd, lpool::testSid, false);
    REQUIRE(poolOwner->poolManager.performHkOperation() == retval::CATCH_OK);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
    /* Now HK packet should be sent as message. */
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setOneShotReportCommand(&hkCmd, lpool::testSid, false);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setUpdateNotificationSetCommand(&hkCmd, lpool::testSid);
    sid_t sidToCheck;
@ -326,62 +328,62 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) ==
          static_cast<int>(LocalDataPoolManager::WRONG_HK_PACKET_TYPE));
    /* We still expect a failure message being sent */
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setCollectionIntervalModificationCommand(&hkCmd, lpool::testSid, 0.4,
                                                                  false);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) ==
          static_cast<int>(LocalDataPoolManager::WRONG_HK_PACKET_TYPE));
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setStructureReportingCommand(&hkCmd, lpool::testSid, false);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) ==
          static_cast<int>(LocalDataPoolManager::WRONG_HK_PACKET_TYPE));
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setStructureReportingCommand(&hkCmd, lpool::testSid, true);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setCollectionIntervalModificationCommand(&hkCmd, lpool::testSid, 0.4,
                                                                  true);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, true, true);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setToggleReportingCommand(&hkCmd, lpool::testSid, false, true);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setOneShotReportCommand(&hkCmd, lpool::testSid, false);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) ==
          static_cast<int>(LocalDataPoolManager::WRONG_HK_PACKET_TYPE));
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setOneShotReportCommand(&hkCmd, lpool::testSid, true);
    CHECK(poolOwner->poolManager.handleHousekeepingMessage(&hkCmd) == retval::CATCH_OK);
-    REQUIRE(mqMock->wasMessageSent(&messagesSent) == true);
+    REQUIRE(poolOwnerMock->wasMessageSent(&messagesSent) == true);
    CHECK(messagesSent == 1);
-    CHECK(mqMock->popMessage() == retval::CATCH_OK);
+    CHECK(poolOwnerMock->popMessage() == retval::CATCH_OK);

    HousekeepingMessage::setUpdateNotificationVariableCommand(&hkCmd, lpool::uint8VarGpid);
    gp_id_t gpidToCheck;
@ -407,5 +409,5 @@ TEST_CASE("LocalPoolManagerTest", "[LocManTest]") {
  /* we need to reset the subscription list because the pool owner
  is a global object. */
  CHECK(poolOwner->reset() == retval::CATCH_OK);
-  mqMock->clearMessages(true);
+  poolOwnerMock->clearMessages(true);
 }
--- a/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp
+++ b/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp
@ -73,7 +73,7 @@ TEST_CASE("Command Executor", "[cmd-exec]") {
    REQUIRE(result != CommandExecutor::COMMAND_ERROR);
    // This ensures that the tests do not block indefinitely
    usleep(500);
-    REQUIRE(limitIdx < 500);
+    REQUIRE(limitIdx < 50000);
  }
  limitIdx = 0;
  CHECK(bytesHaveBeenRead == true);
--- a/tests/src/fsfw_tests/unit/mocks/MessageQueueMockBase.h
+++ b/tests/src/fsfw_tests/unit/mocks/MessageQueueMockBase.h
@ -5,15 +5,17 @@
 #include <queue>

 #include "fsfw/ipc/CommandMessage.h"
+#include "fsfw/ipc/MessageQueueBase.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
 #include "fsfw_tests/unit/CatchDefinitions.h"

-class MessageQueueMockBase : public MessageQueueIF {
+class MessageQueueMockBase : public MessageQueueBase {
 public:
-  MessageQueueId_t myQueueId = tconst::testQueueId;
+  MessageQueueMockBase()
+      : MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, nullptr) {}
+
  uint8_t messageSentCounter = 0;
-  bool defaultDestSet = false;
  bool messageSent = false;

  bool wasMessageSent(uint8_t* messageSentCounter = nullptr, bool resetCounter = true) {
@ -38,39 +40,19 @@ class MessageQueueMockBase : public MessageQueueIF {
    return receiveMessage(&message);
  }

-  virtual ReturnValue_t reply(MessageQueueMessageIF* message) {
-    return sendMessage(myQueueId, message);
-  };
-  virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
-                                       MessageQueueId_t* receivedFrom) {
-    return receiveMessage(message);
-  }
-
-  virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message) {
+  virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override {
    if (messagesSentQueue.empty()) {
      return MessageQueueIF::EMPTY;
    }
-
+    this->last = message->getSender();
    std::memcpy(message->getBuffer(), messagesSentQueue.front().getBuffer(),
                message->getMessageSize());
    messagesSentQueue.pop();
    return HasReturnvaluesIF::RETURN_OK;
  }
  virtual ReturnValue_t flush(uint32_t* count) { return HasReturnvaluesIF::RETURN_OK; }
-  virtual MessageQueueId_t getLastPartner() const { return myQueueId; }
-  virtual MessageQueueId_t getId() const { return myQueueId; }
  virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                        MessageQueueId_t sentFrom, bool ignoreFault = false) {
-    return sendMessage(sendTo, message);
-  }
-  virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
-                                          bool ignoreFault = false) {
-    return sendMessage(myQueueId, message);
-  }
-  virtual ReturnValue_t sendToDefault(MessageQueueMessageIF* message) {
-    return sendMessage(myQueueId, message);
-  }
-  virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
+                                        MessageQueueId_t sentFrom,
                                        bool ignoreFault = false) override {
    messageSent = true;
    messageSentCounter++;
@ -78,13 +60,10 @@ class MessageQueueMockBase : public MessageQueueIF {
    messagesSentQueue.push(messageRef);
    return HasReturnvaluesIF::RETURN_OK;
  }
-  virtual void setDefaultDestination(MessageQueueId_t defaultDestination) {
-    myQueueId = defaultDestination;
-    defaultDestSet = true;
-  }

-  virtual MessageQueueId_t getDefaultDestination() const { return myQueueId; }
-  virtual bool isDefaultDestinationSet() const { return defaultDestSet; }
+  virtual ReturnValue_t reply(MessageQueueMessageIF* message) override {
+    return sendMessageFrom(MessageQueueIF::NO_QUEUE, message, this->getId(), false);
+  }

  void clearMessages(bool clearCommandMessages = true) {
    while (not messagesSentQueue.empty()) {