diff --git a/src/fsfw/container/FixedMap.h b/src/fsfw/container/FixedMap.h
index def219d1..6ed6278d 100644
--- a/src/fsfw/container/FixedMap.h
+++ b/src/fsfw/container/FixedMap.h
@@ -7,6 +7,13 @@
 #include "../returnvalues/returnvalue.h"
 #include "ArrayList.h"
 
+namespace mapdefs {
+static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP;
+static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01);
+static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02);
+static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03);
+}  // namespace mapdefs
+
 /**
  * @brief    Map implementation for maps with a pre-defined size.
  * @details
@@ -24,11 +31,6 @@ class FixedMap : public SerializeIF {
                 "derived class from SerializeIF to be serialize-able");
 
  public:
-  static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP;
-  static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01);
-  static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02);
-  static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03);
-
  private:
   static const key_t EMPTY_SLOT = -1;
   ArrayList<std::pair<key_t, T>, uint32_t> theMap;
@@ -76,10 +78,10 @@ class FixedMap : public SerializeIF {
 
   ReturnValue_t insert(key_t key, T value, Iterator* storedValue = nullptr) {
     if (exists(key) == returnvalue::OK) {
-      return KEY_ALREADY_EXISTS;
+      return mapdefs::KEY_ALREADY_EXISTS;
     }
     if (_size == theMap.maxSize()) {
-      return MAP_FULL;
+      return mapdefs::MAP_FULL;
     }
     theMap[_size].first = key;
     theMap[_size].second = value;
@@ -93,7 +95,7 @@ class FixedMap : public SerializeIF {
   ReturnValue_t insert(std::pair<key_t, T> pair) { return insert(pair.first, pair.second); }
 
   ReturnValue_t exists(key_t key) const {
-    ReturnValue_t result = KEY_DOES_NOT_EXIST;
+    ReturnValue_t result = mapdefs::KEY_DOES_NOT_EXIST;
     if (findIndex(key) < _size) {
       result = returnvalue::OK;
     }
@@ -103,7 +105,7 @@ class FixedMap : public SerializeIF {
   ReturnValue_t erase(Iterator* iter) {
     uint32_t i;
     if ((i = findIndex((*iter).value->first)) >= _size) {
-      return KEY_DOES_NOT_EXIST;
+      return mapdefs::KEY_DOES_NOT_EXIST;
     }
     theMap[i] = theMap[_size - 1];
     --_size;
@@ -114,7 +116,7 @@ class FixedMap : public SerializeIF {
   ReturnValue_t erase(key_t key) {
     uint32_t i;
     if ((i = findIndex(key)) >= _size) {
-      return KEY_DOES_NOT_EXIST;
+      return mapdefs::KEY_DOES_NOT_EXIST;
     }
     theMap[i] = theMap[_size - 1];
     --_size;
diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp
index 80c08590..98907210 100644
--- a/src/fsfw/controller/ControllerBase.cpp
+++ b/src/fsfw/controller/ControllerBase.cpp
@@ -4,11 +4,10 @@
 #include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/subsystem/SubsystemBase.h"
+#include "fsfw/subsystem/helper.h"
 
-ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
-                               size_t commandQueueDepth)
+ControllerBase::ControllerBase(object_id_t setObjectId, size_t commandQueueDepth)
     : SystemObject(setObjectId),
-      parentId(parentId),
       mode(MODE_OFF),
       submode(SUBMODE_NONE),
       modeHelper(this),
@@ -21,33 +20,15 @@ ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
 ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
 
 ReturnValue_t ControllerBase::initialize() {
-  ReturnValue_t result = SystemObject::initialize();
+  ReturnValue_t result = modeHelper.initialize();
   if (result != returnvalue::OK) {
     return result;
   }
-
-  MessageQueueId_t parentQueue = 0;
-  if (parentId != objects::NO_OBJECT) {
-    auto* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
-    if (parent == nullptr) {
-      return returnvalue::FAILED;
-    }
-    parentQueue = parent->getCommandQueue();
-
-    parent->registerChild(getObjectId());
-  }
-
-  result = healthHelper.initialize(parentQueue);
+  result = healthHelper.initialize();
   if (result != returnvalue::OK) {
     return result;
   }
-
-  result = modeHelper.initialize(parentQueue);
-  if (result != returnvalue::OK) {
-    return result;
-  }
-
-  return returnvalue::OK;
+  return SystemObject::initialize();
 }
 
 MessageQueueId_t ControllerBase::getCommandQueue() const { return commandQueue->getId(); }
@@ -120,3 +101,13 @@ void ControllerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
 void ControllerBase::changeHK(Mode_t mode_, Submode_t submode_, bool enable) {}
 
 ReturnValue_t ControllerBase::initializeAfterTaskCreation() { return returnvalue::OK; }
+
+const HasHealthIF* ControllerBase::getOptHealthIF() const { return this; }
+
+const HasModesIF& ControllerBase::getModeIF() const { return *this; }
+
+ModeTreeChildIF& ControllerBase::getModeTreeChildIF() { return *this; }
+
+ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
+}
diff --git a/src/fsfw/controller/ControllerBase.h b/src/fsfw/controller/ControllerBase.h
index 56c28585..da140e49 100644
--- a/src/fsfw/controller/ControllerBase.h
+++ b/src/fsfw/controller/ControllerBase.h
@@ -6,6 +6,9 @@
 #include "fsfw/modes/HasModesIF.h"
 #include "fsfw/modes/ModeHelper.h"
 #include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
+#include "fsfw/subsystem/ModeTreeChildIF.h"
+#include "fsfw/subsystem/ModeTreeConnectionIF.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw/tasks/PeriodicTaskIF.h"
 
@@ -18,13 +21,18 @@
 class ControllerBase : public HasModesIF,
                        public HasHealthIF,
                        public ExecutableObjectIF,
+                       public ModeTreeChildIF,
+                       public ModeTreeConnectionIF,
                        public SystemObject {
  public:
   static const Mode_t MODE_NORMAL = 2;
 
-  ControllerBase(object_id_t setObjectId, object_id_t parentId, size_t commandQueueDepth = 3);
+  ControllerBase(object_id_t setObjectId, size_t commandQueueDepth = 3);
   ~ControllerBase() override;
 
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
+  ModeTreeChildIF &getModeTreeChildIF() override;
+
   /** SystemObject override */
   ReturnValue_t initialize() override;
 
@@ -38,6 +46,8 @@ class ControllerBase : public HasModesIF,
   ReturnValue_t performOperation(uint8_t opCode) override;
   void setTaskIF(PeriodicTaskIF *task) override;
   ReturnValue_t initializeAfterTaskCreation() override;
+  const HasHealthIF *getOptHealthIF() const override;
+  const HasModesIF &getModeIF() const override;
 
  protected:
   /**
@@ -56,8 +66,6 @@ class ControllerBase : public HasModesIF,
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
                                  uint32_t *msToReachTheMode) override = 0;
 
-  const object_id_t parentId;
-
   Mode_t mode;
 
   Submode_t submode;
diff --git a/src/fsfw/controller/ExtendedControllerBase.cpp b/src/fsfw/controller/ExtendedControllerBase.cpp
index 4d5c90c8..58db3563 100644
--- a/src/fsfw/controller/ExtendedControllerBase.cpp
+++ b/src/fsfw/controller/ExtendedControllerBase.cpp
@@ -1,8 +1,7 @@
 #include "fsfw/controller/ExtendedControllerBase.h"
 
-ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, object_id_t parentId,
-                                               size_t commandQueueDepth)
-    : ControllerBase(objectId, parentId, commandQueueDepth),
+ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, size_t commandQueueDepth)
+    : ControllerBase(objectId, commandQueueDepth),
       poolManager(this, commandQueue),
       actionHelper(this, commandQueue) {}
 
diff --git a/src/fsfw/controller/ExtendedControllerBase.h b/src/fsfw/controller/ExtendedControllerBase.h
index b5583a88..04a79528 100644
--- a/src/fsfw/controller/ExtendedControllerBase.h
+++ b/src/fsfw/controller/ExtendedControllerBase.h
@@ -17,7 +17,7 @@ class ExtendedControllerBase : public ControllerBase,
                                public HasActionsIF,
                                public HasLocalDataPoolIF {
  public:
-  ExtendedControllerBase(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth = 3);
+  ExtendedControllerBase(object_id_t objectId, size_t commandQueueDepth = 3);
   ~ExtendedControllerBase() override;
 
   /* SystemObjectIF overrides */
diff --git a/src/fsfw/devicehandlers/AssemblyBase.cpp b/src/fsfw/devicehandlers/AssemblyBase.cpp
index c943a4cf..14575e69 100644
--- a/src/fsfw/devicehandlers/AssemblyBase.cpp
+++ b/src/fsfw/devicehandlers/AssemblyBase.cpp
@@ -1,7 +1,7 @@
 #include "fsfw/devicehandlers/AssemblyBase.h"
 
-AssemblyBase::AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth)
-    : SubsystemBase(objectId, parentId, MODE_OFF, commandQueueDepth),
+AssemblyBase::AssemblyBase(object_id_t objectId, uint16_t commandQueueDepth)
+    : SubsystemBase(objectId, MODE_OFF, commandQueueDepth),
       internalState(STATE_NONE),
       recoveryState(RECOVERY_IDLE),
       recoveringDevice(childrenMap.end()),
diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h
index fe1c4218..5e0d826f 100644
--- a/src/fsfw/devicehandlers/AssemblyBase.h
+++ b/src/fsfw/devicehandlers/AssemblyBase.h
@@ -41,7 +41,7 @@ class AssemblyBase : public SubsystemBase {
   static const ReturnValue_t NEED_TO_CHANGE_HEALTH = MAKE_RETURN_CODE(0x05);
   static const ReturnValue_t NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE = MAKE_RETURN_CODE(0xa1);
 
-  AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth = 8);
+  AssemblyBase(object_id_t objectId, uint16_t commandQueueDepth = 8);
   virtual ~AssemblyBase();
 
  protected:
diff --git a/src/fsfw/devicehandlers/ChildHandlerBase.cpp b/src/fsfw/devicehandlers/ChildHandlerBase.cpp
index ecd4cfc8..37af39dc 100644
--- a/src/fsfw/devicehandlers/ChildHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/ChildHandlerBase.cpp
@@ -3,17 +3,12 @@
 #include "fsfw/subsystem/SubsystemBase.h"
 
 ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication,
-                                   CookieIF* cookie, object_id_t hkDestination,
-                                   uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId,
-                                   object_id_t parent, FailureIsolationBase* customFdir,
-                                   size_t cmdQueueSize)
+                                   CookieIF* cookie, HasModeTreeChildrenIF& parent,
+                                   FailureIsolationBase* customFdir, size_t cmdQueueSize)
     : DeviceHandlerBase(setObjectId, deviceCommunication, cookie,
                         (customFdir == nullptr ? &childHandlerFdir : customFdir), cmdQueueSize),
-      parentId(parent),
-      childHandlerFdir(setObjectId) {
-  this->setHkDestination(hkDestination);
-  this->setThermalStateRequestPoolIds(thermalStatePoolId, thermalRequestPoolId);
-}
+      parent(parent),
+      childHandlerFdir(setObjectId) {}
 
 ChildHandlerBase::~ChildHandlerBase() {}
 
@@ -23,21 +18,5 @@ ReturnValue_t ChildHandlerBase::initialize() {
     return result;
   }
 
-  MessageQueueId_t parentQueue = 0;
-
-  if (parentId != objects::NO_OBJECT) {
-    SubsystemBase* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
-    if (parent == NULL) {
-      return returnvalue::FAILED;
-    }
-    parentQueue = parent->getCommandQueue();
-
-    parent->registerChild(getObjectId());
-  }
-
-  healthHelper.setParentQueue(parentQueue);
-
-  modeHelper.setParentQueue(parentQueue);
-
-  return returnvalue::OK;
+  return DeviceHandlerBase::connectModeTreeParent(parent);
 }
diff --git a/src/fsfw/devicehandlers/ChildHandlerBase.h b/src/fsfw/devicehandlers/ChildHandlerBase.h
index 19d48a24..c98baf3d 100644
--- a/src/fsfw/devicehandlers/ChildHandlerBase.h
+++ b/src/fsfw/devicehandlers/ChildHandlerBase.h
@@ -1,22 +1,23 @@
 #ifndef FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_
 #define FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_
 
+#include <fsfw/subsystem/HasModeTreeChildrenIF.h>
+
 #include "ChildHandlerFDIR.h"
 #include "DeviceHandlerBase.h"
 
 class ChildHandlerBase : public DeviceHandlerBase {
  public:
   ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF* cookie,
-                   object_id_t hkDestination, uint32_t thermalStatePoolId,
-                   uint32_t thermalRequestPoolId, object_id_t parent = objects::NO_OBJECT,
-                   FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20);
+                   HasModeTreeChildrenIF& parent, FailureIsolationBase* customFdir = nullptr,
+                   size_t cmdQueueSize = 20);
 
   virtual ~ChildHandlerBase();
 
   virtual ReturnValue_t initialize();
 
  protected:
-  const uint32_t parentId;
+  HasModeTreeChildrenIF& parent;
   ChildHandlerFDIR childHandlerFdir;
 };
 
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 2e157efa..63124dc8 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -1,4 +1,4 @@
-#include "fsfw/devicehandlers/DeviceHandlerBase.h"
+#include "DeviceHandlerBase.h"
 
 #include "fsfw/datapoollocal/LocalPoolVariable.h"
 #include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h"
@@ -12,6 +12,7 @@
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/storagemanager/StorageManagerIF.h"
 #include "fsfw/subsystem/SubsystemBase.h"
+#include "fsfw/subsystem/helper.h"
 #include "fsfw/thermal/ThermalComponentIF.h"
 
 object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT;
@@ -132,14 +133,6 @@ ReturnValue_t DeviceHandlerBase::initialize() {
         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);
   if (communicationInterface == nullptr) {
@@ -1578,8 +1571,6 @@ MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyI
 
 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;
 }
@@ -1602,6 +1593,16 @@ void DeviceHandlerBase::disableCommandsAndReplies() {
   }
 }
 
+ReturnValue_t DeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
+}
+
+const HasHealthIF* DeviceHandlerBase::getOptHealthIF() const { return this; }
+
+const HasModesIF& DeviceHandlerBase::getModeIF() const { return *this; }
+
+ModeTreeChildIF& DeviceHandlerBase::getModeTreeChildIF() { return *this; }
+
 ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action,
                                               ReturnValue_t result) {
   auto commandIter = deviceCommandMap.find(action);
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h
index 92788ca3..300dd747 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h
@@ -21,6 +21,7 @@
 #include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
+#include "fsfw/subsystem/ModeTreeConnectionIF.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw/tasks/PeriodicTaskIF.h"
 #include "fsfw/util/dataWrapper.h"
@@ -84,6 +85,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
                           public HasModesIF,
                           public HasHealthIF,
                           public HasActionsIF,
+                          public ModeTreeChildIF,
+                          public ModeTreeConnectionIF,
                           public ReceivesParameterMessagesIF,
                           public HasLocalDataPoolIF {
   friend void(Factory::setStaticFrameworkObjectIds)();
@@ -104,7 +107,6 @@ class DeviceHandlerBase : public DeviceHandlerIF,
                     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);
 
@@ -121,6 +123,10 @@ class DeviceHandlerBase : public DeviceHandlerIF,
       lp_id_t thermalStatePoolId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID,
       lp_id_t thermalRequestPoolId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID,
       uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID);
+
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
+  ModeTreeChildIF &getModeTreeChildIF() override;
+
   /**
    * @brief   Helper function to ease device handler development.
    * This will instruct the transition to MODE_ON immediately
@@ -166,7 +172,7 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * @param counter Specifies which Action to perform
    * @return returnvalue::OK for successful execution
    */
-  virtual ReturnValue_t performOperation(uint8_t counter) override;
+  ReturnValue_t performOperation(uint8_t counter) override;
 
   /**
    * @brief  Initializes the device handler
@@ -176,14 +182,14 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * Calls fillCommandAndReplyMap().
    * @return
    */
-  virtual ReturnValue_t initialize() override;
+  ReturnValue_t initialize() override;
 
   /**
    * @brief   Intialization steps performed after all tasks have been created.
    *          This function will be called by the executing task.
    * @return
    */
-  virtual ReturnValue_t initializeAfterTaskCreation() override;
+  ReturnValue_t initializeAfterTaskCreation() override;
 
   /** Destructor. */
   virtual ~DeviceHandlerBase();
@@ -200,6 +206,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   virtual object_id_t getObjectId() const override;
 
   /**
+   * This is a helper method for classes which are parent nodes in the mode tree.
+   * It registers the passed queue as the destination for mode and health messages.
    * @param parentQueueId
    */
   virtual void setParentQueue(MessageQueueId_t parentQueueId);
@@ -961,6 +969,9 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    */
   LocalDataPoolManager *getHkManagerHandle() override;
 
+  const HasHealthIF *getOptHealthIF() const override;
+  const HasModesIF &getModeIF() const override;
+
   /**
    * Returns the delay cycle count of a reply.
    * A count != 0 indicates that the command is already executed.
diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp
index ecc52c94..efe6d534 100644
--- a/src/fsfw/modes/ModeMessage.cpp
+++ b/src/fsfw/modes/ModeMessage.cpp
@@ -24,3 +24,13 @@ void ModeMessage::setCantReachMode(CommandMessage* message, ReturnValue_t reason
   message->setParameter(reason);
   message->setParameter2(0);
 }
+
+void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive) {
+  Command_t cmd;
+  if (recursive) {
+    cmd = CMD_MODE_ANNOUNCE_RECURSIVELY;
+  } else {
+    cmd = CMD_MODE_ANNOUNCE;
+  }
+  message.setCommand(cmd);
+}
diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h
index 84429e84..1582ccd4 100644
--- a/src/fsfw/modes/ModeMessage.h
+++ b/src/fsfw/modes/ModeMessage.h
@@ -1,43 +1,42 @@
 #ifndef FSFW_MODES_MODEMESSAGE_H_
 #define FSFW_MODES_MODEMESSAGE_H_
 
-#include "../ipc/CommandMessage.h"
+#include "fsfw/ipc/CommandMessage.h"
 
 typedef uint32_t Mode_t;
 typedef uint8_t Submode_t;
 
 class ModeMessage {
- private:
-  ModeMessage();
-
  public:
   static const uint8_t MESSAGE_ID = messagetypes::MODE_COMMAND;
-  static const Command_t CMD_MODE_COMMAND =
-      MAKE_COMMAND_ID(0x01);  //!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY,
-                              //! REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies,
-                              //! as this will break the subsystem mode machine!!
-  static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(
-      0xF1);  //!> Command to set the specified Mode, regardless of external control flag, replies
-              //! are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any
-              //! replies, as this will break the subsystem mode machine!!
-  static const Command_t REPLY_MODE_REPLY =
-      MAKE_COMMAND_ID(0x02);  //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
-  static const Command_t REPLY_MODE_INFO =
-      MAKE_COMMAND_ID(0x03);  //!> Unrequested info about the current mode (used for composites to
-                              //! inform their container of a changed mode)
-  static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(
-      0x04);  //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
-  static const Command_t REPLY_WRONG_MODE_REPLY =
-      MAKE_COMMAND_ID(0x05);  //!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded
-                              //! and a transition started but was aborted; the parameters contain
-                              //! the mode that was reached
-  static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(
-      0x06);  //!> Command to read the current mode and reply with a REPLY_MODE_REPLY
-  static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(
-      0x07);  //!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
-  static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY =
-      MAKE_COMMAND_ID(0x08);  //!> Command to trigger an ModeInfo Event and to send this command to
-                              //! every child. This command does NOT have a reply.
+  //!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY,
+  //! REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies,
+  //! as this will break the subsystem mode machine!!
+  static const Command_t CMD_MODE_COMMAND = MAKE_COMMAND_ID(0x01);
+  //!> Command to set the specified Mode, regardless of external control flag, replies
+  //! are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any
+  //! replies, as this will break the subsystem mode machine!!
+  static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(0xF1);
+  //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
+  static const Command_t REPLY_MODE_REPLY = MAKE_COMMAND_ID(0x02);
+  //!> Unrequested info about the current mode (used for composites to
+  //! inform their container of a changed mode)
+  static const Command_t REPLY_MODE_INFO = MAKE_COMMAND_ID(0x03);
+  //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
+  static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(0x04);
+  //!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded
+  //! and a transition started but was aborted; the parameters contain
+  //! the mode that was reached
+  static const Command_t REPLY_WRONG_MODE_REPLY = MAKE_COMMAND_ID(0x05);
+  //!> Command to read the current mode and reply with a REPLY_MODE_REPLY
+  static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(0x06);
+  //!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
+  static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(0x07);
+  //!> Command to trigger an ModeInfo Event and to send this command to
+  //! every child. This command does NOT have a reply.
+  static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY = MAKE_COMMAND_ID(0x08);
+
+  ModeMessage() = delete;
 
   static Mode_t getMode(const CommandMessage* message);
   static Submode_t getSubmode(const CommandMessage* message);
@@ -45,6 +44,7 @@ class ModeMessage {
 
   static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
                              Submode_t submode);
+  static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
   static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
   static void clear(CommandMessage* message);
 };
diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp
index d28df59b..f5928be7 100644
--- a/src/fsfw/pus/CService200ModeCommanding.cpp
+++ b/src/fsfw/pus/CService200ModeCommanding.cpp
@@ -20,6 +20,7 @@ ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
     case (Subservice::COMMAND_MODE_COMMAND):
     case (Subservice::COMMAND_MODE_READ):
     case (Subservice::COMMAND_MODE_ANNCOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY):
       return returnvalue::OK;
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
@@ -53,15 +54,26 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
                                                         const uint8_t *tcData, size_t tcDataLen,
                                                         uint32_t *state, object_id_t objectId) {
-  ModePacket modeCommandPacket;
-  ReturnValue_t result =
-      modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
-  if (result != returnvalue::OK) {
-    return result;
+  ReturnValue_t result = returnvalue::OK;
+  if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE or
+      subservice == Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY) {
+    bool recursive = true;
+    if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
+      recursive = false;
+    }
+    ModeMessage::setModeAnnounceMessage(*message, recursive);
+  } else {
+    ModePacket modeCommandPacket;
+    ReturnValue_t result =
+        modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+
+    ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
+                                modeCommandPacket.getSubmode());
   }
 
-  ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
-                              modeCommandPacket.getSubmode());
   return result;
 }
 
diff --git a/src/fsfw/pus/servicepackets/Service200Packets.h b/src/fsfw/pus/servicepackets/Service200Packets.h
index 701e3f09..3e1df55a 100644
--- a/src/fsfw/pus/servicepackets/Service200Packets.h
+++ b/src/fsfw/pus/servicepackets/Service200Packets.h
@@ -1,9 +1,9 @@
 #ifndef FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_
 #define FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_
 
-#include "../../modes/ModeMessage.h"
-#include "../../serialize/SerialLinkedListAdapter.h"
-#include "../../serialize/SerializeIF.h"
+#include "fsfw/modes/ModeMessage.h"
+#include "fsfw/serialize/SerialLinkedListAdapter.h"
+#include "fsfw/serialize/SerializeIF.h"
 
 /**
  * @brief Subservice 1, 2, 3, 4, 5
diff --git a/src/fsfw/subsystem/CMakeLists.txt b/src/fsfw/subsystem/CMakeLists.txt
index 164c90f7..95dce8a5 100644
--- a/src/fsfw/subsystem/CMakeLists.txt
+++ b/src/fsfw/subsystem/CMakeLists.txt
@@ -1,3 +1,4 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp)
+target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp
+                                        helper.cpp)
 
 add_subdirectory(modes)
diff --git a/src/fsfw/subsystem/HasModeTreeChildrenIF.h b/src/fsfw/subsystem/HasModeTreeChildrenIF.h
new file mode 100644
index 00000000..3458d008
--- /dev/null
+++ b/src/fsfw/subsystem/HasModeTreeChildrenIF.h
@@ -0,0 +1,13 @@
+#ifndef FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_
+#define FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_
+
+#include "ModeTreeChildIF.h"
+
+class HasModeTreeChildrenIF {
+ public:
+  virtual ~HasModeTreeChildrenIF() = default;
+  virtual ReturnValue_t registerChild(const ModeTreeChildIF& child) = 0;
+  virtual MessageQueueId_t getCommandQueue() const = 0;
+};
+
+#endif  // FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_
diff --git a/src/fsfw/subsystem/ModeTreeChildIF.h b/src/fsfw/subsystem/ModeTreeChildIF.h
new file mode 100644
index 00000000..6f8d8d86
--- /dev/null
+++ b/src/fsfw/subsystem/ModeTreeChildIF.h
@@ -0,0 +1,15 @@
+#ifndef FSFW_SUBSYSTEM_MODETREECHILDIF_H_
+#define FSFW_SUBSYSTEM_MODETREECHILDIF_H_
+
+#include <fsfw/health/HasHealthIF.h>
+#include <fsfw/modes/HasModesIF.h>
+
+class ModeTreeChildIF {
+ public:
+  virtual ~ModeTreeChildIF() = default;
+  virtual object_id_t getObjectId() const = 0;
+  virtual const HasHealthIF* getOptHealthIF() const = 0;
+  virtual const HasModesIF& getModeIF() const = 0;
+};
+
+#endif /* FSFW_SUBSYSTEM_MODETREECHILDIF_H_ */
diff --git a/src/fsfw/subsystem/ModeTreeConnectionIF.h b/src/fsfw/subsystem/ModeTreeConnectionIF.h
new file mode 100644
index 00000000..4fe54ada
--- /dev/null
+++ b/src/fsfw/subsystem/ModeTreeConnectionIF.h
@@ -0,0 +1,13 @@
+#ifndef FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_
+#define FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_
+
+#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
+
+class ModeTreeConnectionIF {
+ public:
+  virtual ~ModeTreeConnectionIF() = default;
+  virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) = 0;
+  virtual ModeTreeChildIF& getModeTreeChildIF() = 0;
+};
+
+#endif /* FSFW_SRC_FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_ */
diff --git a/src/fsfw/subsystem/Subsystem.cpp b/src/fsfw/subsystem/Subsystem.cpp
index b2af5ac3..ad3afb2b 100644
--- a/src/fsfw/subsystem/Subsystem.cpp
+++ b/src/fsfw/subsystem/Subsystem.cpp
@@ -9,9 +9,9 @@
 #include "fsfw/serialize/SerialLinkedListAdapter.h"
 #include "fsfw/serialize/SerializeElement.h"
 
-Subsystem::Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
+Subsystem::Subsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
                      uint32_t maxNumberOfTables)
-    : SubsystemBase(setObjectId, parent, 0),
+    : SubsystemBase(setObjectId, 0),
       isInTransition(false),
       childrenChangedHealth(false),
       currentTargetTable(),
@@ -36,6 +36,13 @@ ReturnValue_t Subsystem::checkSequence(HybridIterator<ModeListEntry> iter,
 
   for (; iter.value != nullptr; ++iter) {
     if (!existsModeTable(iter->getTableId())) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      using namespace std;
+      sif::warning << "Subsystem::checkSequence: "
+                   << "Object " << setfill('0') << hex << "0x" << setw(8) << getObjectId()
+                   << setw(0) << ": Mode table for mode ID "
+                   << "0x" << setw(8) << iter->getTableId() << " does not exist" << dec << endl;
+#endif
       return TABLE_DOES_NOT_EXIST;
     } else {
       ReturnValue_t result = checkTable(getTable(iter->getTableId()));
diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h
index f4e73117..20925821 100644
--- a/src/fsfw/subsystem/Subsystem.h
+++ b/src/fsfw/subsystem/Subsystem.h
@@ -66,8 +66,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF {
    * @param maxNumberOfSequences
    * @param maxNumberOfTables
    */
-  Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
-            uint32_t maxNumberOfTables);
+  Subsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
   virtual ~Subsystem();
 
   ReturnValue_t addSequence(SequenceEntry sequence);
diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index 2ae2cb77..d14e3539 100644
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/src/fsfw/subsystem/SubsystemBase.cpp
@@ -1,16 +1,17 @@
 #include "fsfw/subsystem/SubsystemBase.h"
 
+#include "fsfw/FSFW.h"
 #include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/objectmanager/ObjectManager.h"
-#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/serviceinterface.h"
+#include "fsfw/subsystem/helper.h"
 
-SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode,
+SubsystemBase::SubsystemBase(object_id_t setObjectId, Mode_t initialMode,
                              uint16_t commandQueueDepth)
     : SystemObject(setObjectId),
       mode(initialMode),
       healthHelper(this, setObjectId),
-      modeHelper(this),
-      parentId(parent) {
+      modeHelper(this) {
   auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
   commandQueue = QueueFactory::instance()->createMessageQueue(
       commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE, &mqArgs);
@@ -18,36 +19,6 @@ SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t
 
 SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
 
-ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
-  ChildInfo info;
-
-  HasModesIF* child = ObjectManager::instance()->get<HasModesIF>(objectId);
-  // This is a rather ugly hack to have the changedHealth info for all
-  // children available.
-  HasHealthIF* healthChild = ObjectManager::instance()->get<HasHealthIF>(objectId);
-  if (child == nullptr) {
-    if (healthChild == nullptr) {
-      return CHILD_DOESNT_HAVE_MODES;
-    } else {
-      info.commandQueue = healthChild->getCommandQueue();
-      info.mode = MODE_OFF;
-    }
-  } else {
-    // intentional to force an initial command during system startup
-    info.commandQueue = child->getCommandQueue();
-    info.mode = HasModesIF::MODE_UNDEFINED;
-  }
-
-  info.submode = SUBMODE_NONE;
-  info.healthChanged = false;
-
-  auto resultPair = childrenMap.emplace(objectId, info);
-  if (not resultPair.second) {
-    return COULD_NOT_INSERT_CHILD;
-  }
-  return returnvalue::OK;
-}
-
 ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry> tableIter,
                                                     Submode_t targetSubmode) {
   std::map<object_id_t, ChildInfo>::iterator childIter;
@@ -87,7 +58,8 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
     if ((iter = childrenMap.find(object)) == childrenMap.end()) {
       // illegal table entry, should only happen due to misconfigured mode table
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::debug << std::hex << getObjectId() << ": invalid mode table entry" << std::endl;
+      sif::debug << std::hex << SystemObject::getObjectId() << ": invalid mode table entry"
+                 << std::endl;
 #endif
       continue;
     }
@@ -158,36 +130,15 @@ ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bo
 MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->getId(); }
 
 ReturnValue_t SubsystemBase::initialize() {
-  MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE;
-  ReturnValue_t result = SystemObject::initialize();
-
+  ReturnValue_t result = modeHelper.initialize();
   if (result != returnvalue::OK) {
     return result;
   }
-
-  if (parentId != objects::NO_OBJECT) {
-    SubsystemBase* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
-    if (parent == nullptr) {
-      return returnvalue::FAILED;
-    }
-    parentQueue = parent->getCommandQueue();
-
-    parent->registerChild(getObjectId());
-  }
-
-  result = healthHelper.initialize(parentQueue);
-
+  result = healthHelper.initialize();
   if (result != returnvalue::OK) {
     return result;
   }
-
-  result = modeHelper.initialize(parentQueue);
-
-  if (result != returnvalue::OK) {
-    return result;
-  }
-
-  return returnvalue::OK;
+  return SystemObject::initialize();
 }
 
 ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) {
@@ -240,8 +191,14 @@ ReturnValue_t SubsystemBase::handleModeReply(CommandMessage* message) {
 }
 
 ReturnValue_t SubsystemBase::checkTable(HybridIterator<ModeListEntry> tableIter) {
-  for (; tableIter.value != NULL; ++tableIter) {
+  for (; tableIter.value != nullptr; ++tableIter) {
     if (childrenMap.find(tableIter.value->getObject()) == childrenMap.end()) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      using namespace std;
+      sif::warning << "SubsystemBase::checkTable: Could not find Object " << setfill('0') << hex
+                   << "0x" << setw(8) << tableIter.value->getObject() << " in object " << setw(8)
+                   << setw(0) << "0x" << setw(8) << SystemObject::getObjectId() << dec << std::endl;
+#endif
       return TABLE_CONTAINS_INVALID_OBJECT_ID;
     }
   }
@@ -326,4 +283,33 @@ ReturnValue_t SubsystemBase::setHealth(HealthState health) {
 
 HasHealthIF::HealthState SubsystemBase::getHealth() { return healthHelper.getHealth(); }
 
+ReturnValue_t SubsystemBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper);
+}
+
+object_id_t SubsystemBase::getObjectId() const { return SystemObject::getObjectId(); }
+
 void SubsystemBase::modeChanged() {}
+
+ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) {
+  ChildInfo info;
+
+  const HasModesIF& modeChild = child.getModeIF();
+  // intentional to force an initial command during system startup
+  info.commandQueue = modeChild.getCommandQueue();
+  info.mode = HasModesIF::MODE_UNDEFINED;
+  info.submode = SUBMODE_NONE;
+  info.healthChanged = false;
+
+  auto resultPair = childrenMap.emplace(child.getObjectId(), info);
+  if (not resultPair.second) {
+    return COULD_NOT_INSERT_CHILD;
+  }
+  return returnvalue::OK;
+}
+
+const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; }
+
+const HasModesIF& SubsystemBase::getModeIF() const { return *this; }
+
+ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { return *this; }
diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h
index 69bdfd37..0fbf9f4a 100644
--- a/src/fsfw/subsystem/SubsystemBase.h
+++ b/src/fsfw/subsystem/SubsystemBase.h
@@ -3,14 +3,16 @@
 
 #include <map>
 
-#include "../container/HybridIterator.h"
-#include "../health/HasHealthIF.h"
-#include "../health/HealthHelper.h"
-#include "../ipc/MessageQueueIF.h"
-#include "../modes/HasModesIF.h"
-#include "../objectmanager/SystemObject.h"
-#include "../returnvalues/returnvalue.h"
-#include "../tasks/ExecutableObjectIF.h"
+#include "fsfw/container/HybridIterator.h"
+#include "fsfw/health/HasHealthIF.h"
+#include "fsfw/health/HealthHelper.h"
+#include "fsfw/ipc/MessageQueueIF.h"
+#include "fsfw/modes/HasModesIF.h"
+#include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/returnvalues/returnvalue.h"
+#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
+#include "fsfw/subsystem/ModeTreeConnectionIF.h"
+#include "fsfw/tasks/ExecutableObjectIF.h"
 #include "modes/HasModeSequenceIF.h"
 
 /**
@@ -27,6 +29,9 @@
 class SubsystemBase : public SystemObject,
                       public HasModesIF,
                       public HasHealthIF,
+                      public HasModeTreeChildrenIF,
+                      public ModeTreeConnectionIF,
+                      public ModeTreeChildIF,
                       public ExecutableObjectIF {
  public:
   static const uint8_t INTERFACE_ID = CLASS_ID::SUBSYSTEM_BASE;
@@ -36,32 +41,34 @@ class SubsystemBase : public SystemObject,
   static const ReturnValue_t COULD_NOT_INSERT_CHILD = MAKE_RETURN_CODE(0x04);
   static const ReturnValue_t TABLE_CONTAINS_INVALID_OBJECT_ID = MAKE_RETURN_CODE(0x05);
 
-  SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode = 0,
-                uint16_t commandQueueDepth = 8);
+  SubsystemBase(object_id_t setObjectId, Mode_t initialMode = 0, uint16_t commandQueueDepth = 8);
   virtual ~SubsystemBase();
 
   virtual MessageQueueId_t getCommandQueue() const override;
 
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
+  ModeTreeChildIF &getModeTreeChildIF() override;
+
   /**
    * Function to register the child objects.
    * Performs a checks if the child does implement HasHealthIF and/or HasModesIF
    *
-   * 	Also adds them to the internal childrenMap.
+   *    Also adds them to the internal childrenMap.
    *
    * @param objectId
    * @return returnvalue::OK if successful
-   * 		   CHILD_DOESNT_HAVE_MODES if Child is no HasHealthIF and no HasModesIF
-   * 		   COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap
+   *           CHILD_DOESNT_HAVE_MODES if Child is no HasHealthIF and no HasModesIF
+   *           COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap
    */
-  ReturnValue_t registerChild(object_id_t objectId);
+  ReturnValue_t registerChild(const ModeTreeChildIF &child) override;
 
-  virtual ReturnValue_t initialize() override;
+  ReturnValue_t initialize() override;
 
-  virtual ReturnValue_t performOperation(uint8_t opCode) override;
+  ReturnValue_t performOperation(uint8_t opCode) override;
 
-  virtual ReturnValue_t setHealth(HealthState health) override;
+  ReturnValue_t setHealth(HealthState health) override;
 
-  virtual HasHealthIF::HealthState getHealth() override;
+  HasHealthIF::HealthState getHealth() override;
 
  protected:
   struct ChildInfo {
@@ -88,8 +95,6 @@ class SubsystemBase : public SystemObject,
 
   ModeHelper modeHelper;
 
-  const object_id_t parentId;
-
   typedef std::map<object_id_t, ChildInfo> ChildrenMap;
   ChildrenMap childrenMap;
 
@@ -136,6 +141,10 @@ class SubsystemBase : public SystemObject,
 
   virtual void getMode(Mode_t *mode, Submode_t *submode) override;
 
+  object_id_t getObjectId() const override;
+  const HasHealthIF *getOptHealthIF() const override;
+  const HasModesIF &getModeIF() const override;
+
   virtual void setToExternalControl() override;
 
   virtual void announceMode(bool recursive) override;
diff --git a/src/fsfw/subsystem/helper.cpp b/src/fsfw/subsystem/helper.cpp
new file mode 100644
index 00000000..e65ef5a9
--- /dev/null
+++ b/src/fsfw/subsystem/helper.cpp
@@ -0,0 +1,13 @@
+#include "helper.h"
+
+ReturnValue_t modetree::connectModeTreeParent(HasModeTreeChildrenIF& parent,
+                                              const ModeTreeChildIF& child,
+                                              HealthHelper& healthHelper, ModeHelper& modeHelper) {
+  ReturnValue_t result = parent.registerChild(child);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  healthHelper.setParentQueue(parent.getCommandQueue());
+  modeHelper.setParentQueue(parent.getCommandQueue());
+  return returnvalue::OK;
+}
diff --git a/src/fsfw/subsystem/helper.h b/src/fsfw/subsystem/helper.h
new file mode 100644
index 00000000..71a5563f
--- /dev/null
+++ b/src/fsfw/subsystem/helper.h
@@ -0,0 +1,14 @@
+#ifndef FSFW_SUBSYSTEM_HELPER_H_
+#define FSFW_SUBSYSTEM_HELPER_H_
+
+#include "HasModeTreeChildrenIF.h"
+#include "fsfw/health/HealthHelper.h"
+
+namespace modetree {
+
+ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent, const ModeTreeChildIF& child,
+                                    HealthHelper& healthHelper, ModeHelper& modeHelper);
+
+}
+
+#endif /* FSFW_SRC_FSFW_SUBSYSTEM_HELPER_H_ */
diff --git a/src/fsfw_tests/integration/assemblies/TestAssembly.cpp b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp
index 668120c6..761d0d3c 100644
--- a/src/fsfw_tests/integration/assemblies/TestAssembly.cpp
+++ b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp
@@ -2,19 +2,17 @@
 
 #include <fsfw/objectmanager/ObjectManager.h>
 
-TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId, object_id_t testDevice0,
-                           object_id_t testDevice1)
-    : AssemblyBase(objectId, parentId),
-      deviceHandler0Id(testDevice0),
-      deviceHandler1Id(testDevice1) {
+TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId, ModeTreeChildIF& testDevice0,
+                           ModeTreeChildIF& testDevice1)
+    : AssemblyBase(objectId, parentId), deviceHandler0(testDevice0), deviceHandler1(testDevice1) {
   ModeListEntry newModeListEntry;
-  newModeListEntry.setObject(testDevice0);
+  newModeListEntry.setObject(testDevice0.getObjectId());
   newModeListEntry.setMode(MODE_OFF);
   newModeListEntry.setSubmode(SUBMODE_NONE);
 
   commandTable.insert(newModeListEntry);
 
-  newModeListEntry.setObject(testDevice1);
+  newModeListEntry.setObject(testDevice1.getObjectId());
   newModeListEntry.setMode(MODE_OFF);
   newModeListEntry.setSubmode(SUBMODE_NONE);
 
@@ -43,8 +41,8 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
       commandTable[1].setMode(MODE_OFF);
       commandTable[1].setSubmode(SUBMODE_NONE);
       // We try to prefer 0 here but we try to switch to 1 even if it might fail
-      if (isDeviceAvailable(deviceHandler0Id)) {
-        if (childrenMap[deviceHandler0Id].mode == MODE_ON) {
+      if (isDeviceAvailable(deviceHandler0.getObjectId())) {
+        if (childrenMap[deviceHandler0.getObjectId()].mode == MODE_ON) {
           commandTable[0].setMode(mode);
           commandTable[0].setSubmode(SUBMODE_NONE);
         } else {
@@ -53,7 +51,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
           result = NEED_SECOND_STEP;
         }
       } else {
-        if (childrenMap[deviceHandler1Id].mode == MODE_ON) {
+        if (childrenMap[deviceHandler1.getObjectId()].mode == MODE_ON) {
           commandTable[1].setMode(mode);
           commandTable[1].setSubmode(SUBMODE_NONE);
         } else {
@@ -64,7 +62,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
       }
     } else {
       // Dual Mode Normal
-      if (childrenMap[deviceHandler0Id].mode == MODE_ON) {
+      if (childrenMap[deviceHandler0.getObjectId()].mode == MODE_ON) {
         commandTable[0].setMode(mode);
         commandTable[0].setSubmode(SUBMODE_NONE);
       } else {
@@ -72,7 +70,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
         commandTable[0].setSubmode(SUBMODE_NONE);
         result = NEED_SECOND_STEP;
       }
-      if (childrenMap[deviceHandler1Id].mode == MODE_ON) {
+      if (childrenMap[deviceHandler1.getObjectId()].mode == MODE_ON) {
         commandTable[1].setMode(mode);
         commandTable[1].setSubmode(SUBMODE_NONE);
       } else {
@@ -89,7 +87,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) {
       commandTable[1].setMode(MODE_OFF);
       commandTable[1].setSubmode(SUBMODE_NONE);
       // We try to prefer 0 here but we try to switch to 1 even if it might fail
-      if (isDeviceAvailable(deviceHandler0Id)) {
+      if (isDeviceAvailable(deviceHandler0.getObjectId())) {
         commandTable[0].setMode(MODE_ON);
         commandTable[0].setSubmode(SUBMODE_NONE);
       } else {
@@ -133,23 +131,14 @@ ReturnValue_t TestAssembly::initialize() {
   if (result != returnvalue::OK) {
     return result;
   }
-  handler0 = ObjectManager::instance()->get<TestDevice>(deviceHandler0Id);
-  handler1 = ObjectManager::instance()->get<TestDevice>(deviceHandler1Id);
+  auto* handler0 = ObjectManager::instance()->get<TestDevice>(deviceHandler0.getObjectId());
+  auto* handler1 = ObjectManager::instance()->get<TestDevice>(deviceHandler1.getObjectId());
   if ((handler0 == nullptr) or (handler1 == nullptr)) {
     return returnvalue::FAILED;
   }
 
-  handler0->setParentQueue(this->getCommandQueue());
-  handler1->setParentQueue(this->getCommandQueue());
-
-  result = registerChild(deviceHandler0Id);
-  if (result != returnvalue::OK) {
-    return result;
-  }
-  result = registerChild(deviceHandler1Id);
-  if (result != returnvalue::OK) {
-    return result;
-  }
+  handler0->connectModeTreeParent(*this);
+  handler1->connectModeTreeParent(*this);
   return result;
 }
 
diff --git a/src/fsfw_tests/integration/assemblies/TestAssembly.h b/src/fsfw_tests/integration/assemblies/TestAssembly.h
index 91ffbf60..86b4fb07 100644
--- a/src/fsfw_tests/integration/assemblies/TestAssembly.h
+++ b/src/fsfw_tests/integration/assemblies/TestAssembly.h
@@ -7,8 +7,8 @@
 
 class TestAssembly : public AssemblyBase {
  public:
-  TestAssembly(object_id_t objectId, object_id_t parentId, object_id_t testDevice0,
-               object_id_t testDevice1);
+  TestAssembly(object_id_t objectId, object_id_t parentId, ModeTreeChildIF& testDevice0,
+               ModeTreeChildIF& testDevice1);
   virtual ~TestAssembly();
   ReturnValue_t initialize() override;
 
@@ -41,10 +41,8 @@ class TestAssembly : public AssemblyBase {
 
  private:
   FixedArrayList<ModeListEntry, 2> commandTable;
-  object_id_t deviceHandler0Id = 0;
-  object_id_t deviceHandler1Id = 0;
-  TestDevice* handler0 = nullptr;
-  TestDevice* handler1 = nullptr;
+  ModeTreeChildIF& deviceHandler0;
+  ModeTreeChildIF& deviceHandler1;
 
   bool isDeviceAvailable(object_id_t object);
 };
diff --git a/src/fsfw_tests/integration/controller/TestController.cpp b/src/fsfw_tests/integration/controller/TestController.cpp
index 2ee4d5fe..c489f336 100644
--- a/src/fsfw_tests/integration/controller/TestController.cpp
+++ b/src/fsfw_tests/integration/controller/TestController.cpp
@@ -4,8 +4,8 @@
 #include <fsfw/objectmanager/ObjectManager.h>
 #include <fsfw/serviceinterface/ServiceInterface.h>
 
-TestController::TestController(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth)
-    : ExtendedControllerBase(objectId, parentId, commandQueueDepth) {}
+TestController::TestController(object_id_t objectId, size_t commandQueueDepth)
+    : ExtendedControllerBase(objectId, commandQueueDepth) {}
 
 TestController::~TestController() {}
 
diff --git a/src/fsfw_tests/integration/controller/TestController.h b/src/fsfw_tests/integration/controller/TestController.h
index 9898371f..9577bedf 100644
--- a/src/fsfw_tests/integration/controller/TestController.h
+++ b/src/fsfw_tests/integration/controller/TestController.h
@@ -7,7 +7,7 @@
 
 class TestController : public ExtendedControllerBase {
  public:
-  TestController(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth = 10);
+  TestController(object_id_t objectId, size_t commandQueueDepth = 10);
   virtual ~TestController();
 
  protected: