diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3d28c5c6c..84fb2ca1d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -12,6 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Changes
 
+- Bump C++ required version to C++17. Every project which uses the FSFW and every modern
+  compiler supports it
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/622
 - HAL Linux SPI: Set the Clock Default State when setting new SPI speed
   and mode
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/573
@@ -49,9 +52,18 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/559
 - Added ETL dependency and improved library dependency management
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/592
+- `Subsystem`: New API to add table and sequence entries
 
 ## Fixed
 
+- TCP TMTC Server: `MutexGuard` was not created properly in
+  `TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent)` call.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/618
+- Fix infinite recursion in `prepareHealthSetReply` of PUS Health Service 201.
+  Is not currently used right now but might be used in the future
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/617
+- Move some CMake directives further up top so they are not ignored
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/621
 - Small bugfix in STM32 HAL for SPI
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/599
 - HAL GPIO: Improved error checking in `LinuxLibgpioIF::configureGpios(...)`. If a GPIO
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 979a71702..7e2393ed6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,15 @@
 cmake_minimum_required(VERSION 3.13)
 
+set(LIB_FSFW_NAME fsfw)
+project(${LIB_FSFW_NAME})
+
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+    set(CMAKE_CXX_STANDARD_REQUIRED True)
+elseif(${CMAKE_CXX_STANDARD} LESS 17)
+    message(FATAL_ERROR "Compiling the FSFW requires a minimum of C++17 support")
+endif()
+
 set(FSFW_VERSION 4)
 set(FSFW_SUBVERSION 0)
 set(FSFW_REVISION 0)
@@ -69,11 +79,10 @@ option(FSFW_ADD_TMSTORAGE "Compile with tm storage components" OFF)
 # Contrib sources
 option(FSFW_ADD_SGP4_PROPAGATOR "Add SGP4 propagator code" OFF)
 
-set(LIB_FSFW_NAME fsfw)
+
 set(FSFW_TEST_TGT fsfw-tests)
 set(FSFW_DUMMY_TGT fsfw-dummy)
 
-project(${LIB_FSFW_NAME})
 add_library(${LIB_FSFW_NAME})
 
 if(IPO_SUPPORTED AND FSFW_ENABLE_IPO)
@@ -191,13 +200,6 @@ target_include_directories(${LIB_FSFW_NAME} INTERFACE
   ${CMAKE_CURRENT_BINARY_DIR}
   )
 
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 11)
-  set(CMAKE_CXX_STANDARD_REQUIRED True)
-elseif(${CMAKE_CXX_STANDARD} LESS 11)
-  message(FATAL_ERROR "Compiling the FSFW requires a minimum of C++11 support")
-endif()
-
 # Backwards comptability
 if(OS_FSFW AND NOT FSFW_OSAL)
   message(WARNING "Please pass the FSFW OSAL as FSFW_OSAL instead of OS_FSFW")
diff --git a/src/fsfw/container/FixedArrayList.h b/src/fsfw/container/FixedArrayList.h
index 26a739212..fc8be393a 100644
--- a/src/fsfw/container/FixedArrayList.h
+++ b/src/fsfw/container/FixedArrayList.h
@@ -2,6 +2,7 @@
 #define FIXEDARRAYLIST_H_
 
 #include <cmath>
+#include <limits>
 
 #include "ArrayList.h"
 /**
@@ -9,10 +10,9 @@
  */
 template <typename T, size_t MAX_SIZE, typename count_t = uint8_t>
 class FixedArrayList : public ArrayList<T, count_t> {
-#if !defined(_MSC_VER) && !defined(__clang__)
-  static_assert(MAX_SIZE <= (std::pow(2, sizeof(count_t) * 8) - 1),
+  static_assert(MAX_SIZE <= std::numeric_limits<count_t>::max(),
                 "count_t is not large enough to hold MAX_SIZE");
-#endif
+
  private:
   T data[MAX_SIZE];
 
diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp
index ab32d73ff..81e8149e8 100644
--- a/src/fsfw/osal/common/TcpTmTcServer.cpp
+++ b/src/fsfw/osal/common/TcpTmTcServer.cpp
@@ -19,6 +19,8 @@
 #include <ws2tcpip.h>
 #elif defined(PLATFORM_UNIX)
 #include <netdb.h>
+
+#include <utility>
 #endif
 
 const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT;
@@ -29,7 +31,7 @@ TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
     : SystemObject(objectId),
       tmtcBridgeId(tmtcTcpBridge),
       receptionMode(receptionMode),
-      tcpConfig(customTcpServerPort),
+      tcpConfig(std::move(customTcpServerPort)),
       receptionBuffer(receptionBufferSize),
       ringBuffer(ringBufferSize, true) {}
 
@@ -99,7 +101,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
 
 TcpTmTcServer::~TcpTmTcServer() { closeSocket(listenerTcpSocket); }
 
-ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
+[[noreturn]] ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
   using namespace tcpip;
   // If a connection is accepted, the corresponding socket will be assigned to the new socket
   socket_t connSocket = 0;
@@ -134,7 +136,6 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
     closeSocket(connSocket);
     connSocket = 0;
   }
-  return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() {
@@ -155,8 +156,8 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) {
 #endif
 
   while (true) {
-    int retval = recv(connSocket, reinterpret_cast<char*>(receptionBuffer.data()),
-                      receptionBuffer.capacity(), tcpConfig.tcpFlags);
+    ssize_t retval = recv(connSocket, reinterpret_cast<char*>(receptionBuffer.data()),
+                          receptionBuffer.capacity(), tcpConfig.tcpFlags);
     if (retval == 0) {
       size_t availableReadData = ringBuffer.getAvailableReadData();
       if (availableReadData > lastRingBufferSize) {
@@ -224,7 +225,7 @@ ReturnValue_t TcpTmTcServer::handleTcReception(uint8_t* spacePacket, size_t pack
   return result;
 }
 
-std::string TcpTmTcServer::getTcpPort() const { return tcpConfig.tcpPort; }
+const std::string& TcpTmTcServer::getTcpPort() const { return tcpConfig.tcpPort; }
 
 void TcpTmTcServer::setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds_) {
   this->validPacketIds = std::move(validPacketIds_);
@@ -290,6 +291,9 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
   size_t foundSize = 0;
   size_t readLen = 0;
   while (readLen < readAmount) {
+    if (spacePacketParser == nullptr) {
+      return HasReturnvaluesIF::RETURN_FAILED;
+    }
     result =
         spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, startIdx, foundSize, readLen);
     switch (result) {
diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h
index d062a0ce0..faf6e0a17 100644
--- a/src/fsfw/osal/common/TcpTmTcServer.h
+++ b/src/fsfw/osal/common/TcpTmTcServer.h
@@ -17,6 +17,7 @@
 #endif
 
 #include <string>
+#include <utility>
 #include <vector>
 
 class TcpTmTcBridge;
@@ -44,7 +45,7 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
 
   struct TcpConfig {
    public:
-    TcpConfig(std::string tcpPort) : tcpPort(tcpPort) {}
+    explicit TcpConfig(std::string tcpPort) : tcpPort(std::move(tcpPort)) {}
 
     /**
      * Passed to the recv call
@@ -84,7 +85,7 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
                 size_t ringBufferSize = RING_BUFFER_SIZE,
                 std::string customTcpServerPort = DEFAULT_SERVER_PORT,
                 ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS);
-  virtual ~TcpTmTcServer();
+  ~TcpTmTcServer() override;
 
   void enableWiretapping(bool enable);
 
@@ -97,10 +98,10 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
   void setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds);
 
   ReturnValue_t initialize() override;
-  ReturnValue_t performOperation(uint8_t opCode) override;
+  [[noreturn]] ReturnValue_t performOperation(uint8_t opCode) override;
   ReturnValue_t initializeAfterTaskCreation() override;
 
-  std::string getTcpPort() const;
+  [[nodiscard]] const std::string& getTcpPort() const;
 
  protected:
   StorageManagerIF* tcStore = nullptr;
@@ -115,7 +116,7 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
 
   ReceptionModes receptionMode;
   TcpConfig tcpConfig;
-  struct sockaddr tcpAddress;
+  struct sockaddr tcpAddress = {};
   socket_t listenerTcpSocket = 0;
 
   MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
diff --git a/src/fsfw/pus/CService201HealthCommanding.cpp b/src/fsfw/pus/CService201HealthCommanding.cpp
index d196e19b6..f736e5980 100644
--- a/src/fsfw/pus/CService201HealthCommanding.cpp
+++ b/src/fsfw/pus/CService201HealthCommanding.cpp
@@ -13,8 +13,6 @@ CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, u
     : CommandingServiceBase(objectId, apid, serviceId, numParallelCommands, commandTimeoutSeconds) {
 }
 
-CService201HealthCommanding::~CService201HealthCommanding() {}
-
 ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) {
   switch (subservice) {
     case (Subservice::COMMAND_SET_HEALTH):
@@ -75,6 +73,10 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag
       HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE_ALL);
       break;
     }
+    default: {
+      // Should never happen, subservice was already checked
+      result = RETURN_FAILED;
+    }
   }
   return result;
 }
@@ -93,7 +95,8 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep
 }
 
 // Not used for now, health state already reported by event
-ReturnValue_t CService201HealthCommanding::prepareHealthSetReply(const CommandMessage *reply) {
+[[maybe_unused]] ReturnValue_t CService201HealthCommanding::prepareHealthSetReply(
+    const CommandMessage *reply) {
   auto health = static_cast<uint8_t>(HealthMessage::getHealth(reply));
   auto oldHealth = static_cast<uint8_t>(HealthMessage::getOldHealth(reply));
   HealthSetReply healthSetReply(health, oldHealth);
diff --git a/src/fsfw/pus/CService201HealthCommanding.h b/src/fsfw/pus/CService201HealthCommanding.h
index 0c901ca9a..71b7caa05 100644
--- a/src/fsfw/pus/CService201HealthCommanding.h
+++ b/src/fsfw/pus/CService201HealthCommanding.h
@@ -1,7 +1,7 @@
 #ifndef FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_
 #define FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_
 
-#include "../tmtcservices/CommandingServiceBase.h"
+#include "fsfw/tmtcservices/CommandingServiceBase.h"
 
 /**
  * @brief   Custom PUS service to set health of all objects
@@ -21,7 +21,7 @@ class CService201HealthCommanding : public CommandingServiceBase {
  public:
   CService201HealthCommanding(object_id_t objectId, uint16_t apid, uint8_t serviceId,
                               uint8_t numParallelCommands = 4, uint16_t commandTimeoutSeconds = 60);
-  virtual ~CService201HealthCommanding();
+  ~CService201HealthCommanding() override = default;
 
  protected:
   /* CSB abstract function implementations */
@@ -38,12 +38,10 @@ class CService201HealthCommanding : public CommandingServiceBase {
                             bool *isStep) override;
 
  private:
-  ReturnValue_t checkAndAcquireTargetID(object_id_t *objectIdToSet, const uint8_t *tcData,
-                                        size_t tcDataLen);
-  ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet,
-                                                     const object_id_t *objectId);
+  static ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet,
+                                                            const object_id_t *objectId);
 
-  ReturnValue_t prepareHealthSetReply(const CommandMessage *reply);
+  [[maybe_unused]] ReturnValue_t prepareHealthSetReply(const CommandMessage *reply);
 
   enum Subservice {
     //! [EXPORT] : [TC] Set health of target object
diff --git a/src/fsfw/subsystem/Subsystem.cpp b/src/fsfw/subsystem/Subsystem.cpp
index ab8c1b168..27e6ae8e0 100644
--- a/src/fsfw/subsystem/Subsystem.cpp
+++ b/src/fsfw/subsystem/Subsystem.cpp
@@ -307,6 +307,11 @@ void Subsystem::replyToCommand(ReturnValue_t status, uint32_t parameter) {
   }
 }
 
+ReturnValue_t Subsystem::addSequence(SequenceEntry sequence) {
+  return addSequence(sequence.table, sequence.mode, sequence.fallbackMode, sequence.inStore,
+                     sequence.preInit);
+}
+
 ReturnValue_t Subsystem::addSequence(ArrayList<ModeListEntry> *sequence, Mode_t id,
                                      Mode_t fallbackSequence, bool inStore, bool preInit) {
   ReturnValue_t result;
@@ -350,6 +355,10 @@ ReturnValue_t Subsystem::addSequence(ArrayList<ModeListEntry> *sequence, Mode_t
   return result;
 }
 
+ReturnValue_t Subsystem::addTable(TableEntry table) {
+  return addTable(table.table, table.mode, table.inStore, table.preInit);
+}
+
 ReturnValue_t Subsystem::addTable(ArrayList<ModeListEntry> *table, Mode_t id, bool inStore,
                                   bool preInit) {
   ReturnValue_t result;
@@ -458,6 +467,7 @@ ReturnValue_t Subsystem::initialize() {
   }
 
   mode = initialMode;
+  submode = initSubmode;
 
   return RETURN_OK;
 }
@@ -595,7 +605,10 @@ ReturnValue_t Subsystem::checkObjectConnections() {
   return RETURN_OK;
 }
 
-void Subsystem::setInitialMode(Mode_t mode) { initialMode = mode; }
+void Subsystem::setInitialMode(Mode_t mode, Submode_t submode) {
+  this->initialMode = mode;
+  this->initSubmode = submode;
+}
 
 void Subsystem::cantKeepMode() {
   ReturnValue_t result;
diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h
index f1e7228d0..855c8f7ab 100644
--- a/src/fsfw/subsystem/Subsystem.h
+++ b/src/fsfw/subsystem/Subsystem.h
@@ -10,6 +10,28 @@
 #include "fsfw/FSFW.h"
 #include "modes/ModeDefinitions.h"
 
+struct TableSequenceBase {
+ public:
+  TableSequenceBase(Mode_t mode, ArrayList<ModeListEntry> *table) : mode(mode), table(table){};
+  Mode_t mode;
+  ArrayList<ModeListEntry> *table;
+  bool inStore = false;
+  bool preInit = true;
+};
+
+struct TableEntry : public TableSequenceBase {
+ public:
+  TableEntry(Mode_t mode, ArrayList<ModeListEntry> *table) : TableSequenceBase(mode, table){};
+};
+
+struct SequenceEntry : public TableSequenceBase {
+ public:
+  SequenceEntry(Mode_t mode, ArrayList<ModeListEntry> *table, Mode_t fallbackMode)
+      : TableSequenceBase(mode, table), fallbackMode(fallbackMode) {}
+
+  Mode_t fallbackMode;
+};
+
 /**
  * @brief   TODO: documentation missing
  * @details
@@ -44,13 +66,15 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF {
             uint32_t maxNumberOfTables);
   virtual ~Subsystem();
 
+  ReturnValue_t addSequence(SequenceEntry sequence);
   ReturnValue_t addSequence(ArrayList<ModeListEntry> *sequence, Mode_t id, Mode_t fallbackSequence,
                             bool inStore = true, bool preInit = true);
 
+  ReturnValue_t addTable(TableEntry table);
   ReturnValue_t addTable(ArrayList<ModeListEntry> *table, Mode_t id, bool inStore = true,
                          bool preInit = true);
 
-  void setInitialMode(Mode_t mode);
+  void setInitialMode(Mode_t mode, Submode_t submode = SUBMODE_NONE);
 
   virtual ReturnValue_t initialize() override;
 
@@ -89,6 +113,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF {
   Submode_t targetSubmode;
 
   Mode_t initialMode = 0;
+  Submode_t initSubmode = SUBMODE_NONE;
 
   HybridIterator<ModeListEntry> currentSequenceIterator;
 
diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.h b/src/fsfw/tmtcservices/CommandingServiceBase.h
index 867fc2874..4dcad024e 100644
--- a/src/fsfw/tmtcservices/CommandingServiceBase.h
+++ b/src/fsfw/tmtcservices/CommandingServiceBase.h
@@ -166,7 +166,7 @@ class CommandingServiceBase : public SystemObject,
    * @param objectId Target object ID
    * @return
    * - @c RETURN_OK to generate a verification start message
-   * - @c EXECUTION_COMPELTE Fire-and-forget command. Generate a completion
+   * - @c EXECUTION_COMPLETE Fire-and-forget command. Generate a completion
    *      verification message.
    * - @c Anything else rejects the packets and generates a start failure
    *      verification.