diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3d28c5c6..87c912ac 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -24,6 +24,20 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - `oneShotAction` flag in the `TestTask` class is not static anymore
 - HAL Linux Uart: Baudrate and bits per word are enums now, avoiding misconfigurations
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/585
+- Major update for version handling, using `git describe` to fetch version information with git.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/601
+  - Place `Version` class outside of `fsfw` namespace. It is generic
+  - Add helper functions provided by [`cmake-modules`](https://github.com/bilke/cmake-modules)
+    manually now. Those should not change too often and only a small subset is needed
+  - Separate folder for easier update and for distinction
+  - LICENSE file included
+  - use `int` for version numbers to allow unset or uninitialized version
+  - Initialize Version object with numbers set to -1
+  - Instead of hardcoding the git hash, it is now retrieved from git
+  - `Version` now allows specifying additional version information like the git SHA1 hash and the
+    versions since the last tag
+  - Additional information is set to the last part of the git describe output for `FSFW_VERSION` now.
+  - Version still need to be hand-updated if the FSFW is not included as a submodule for now.
 - IPC Message Queue Handling: Allow passing an optional `MqArgs` argument into the MessageQueue
   creation call. It allows passing context information and an arbitrary user argument into
   the message queue. Also streamlined and simplified `MessageQueue` implementation for all OSALs
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d8ca812e..337a56f4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,16 +1,19 @@
 cmake_minimum_required(VERSION 3.13)
+project(${LIB_FSFW_NAME})
 
-set(FSFW_VERSION 4)
-set(FSFW_SUBVERSION 0)
-set(FSFW_REVISION 0)
+set(FSFW_VERSION_IF_GIT_FAILS 4)
+set(FSFW_SUBVERSION_IF_GIT_FAILS 0)
+set(FSFW_REVISION_IF_GIT_FAILS 0)
 
 # Add the cmake folder so the FindSphinx module is found
-set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" )
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake-modules")
+set(MSG_PREFIX "fsfw |")
 
 set(FSFW_ETL_LIB_MAJOR_VERSION 20 CACHE STRING
     "ETL library major version requirement"
 )
-set(FSFW_ETL_LIB_VERSION ${FSFW_ETL_LIB_MAJOR_VERSION}.27.2 CACHE STRING
+set(FSFW_ETL_LIB_VERSION ${FSFW_ETL_LIB_MAJOR_VERSION}.27.3 CACHE STRING
     "ETL library exact version requirement"
 )
 
@@ -58,16 +61,47 @@ 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})
 
+set(FSFW_GIT_VER_HANDLING_OK FALSE)
+# Version handling
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/.git)
+    message(STATUS "${MSG_PREFIX} Determining version information with git")
+    include(FsfwHelpers)
+    determine_version_with_git("--exclude" "docker_*")
+    if(GIT_INFO)
+        set(FSFW_GIT_INFO ${GIT_INFO} CACHE STRING "Version information retrieved with git describe")
+        list(GET FSFW_GIT_INFO 1 FSFW_VERSION)
+        list(GET FSFW_GIT_INFO 2 FSFW_SUBVERSION)
+        list(GET FSFW_GIT_INFO 3 FSFW_REVISION)
+        list(GET FSFW_GIT_INFO 4 FSFW_VERSION_CST_GIT_SHA1)
+        if(NOT FSFW_VERSION)
+            set(FSFW_VERSION ${FSFW_VERSION_IF_GIT_FAILS})
+        endif()
+        if(NOT FSFW_SUBVERSION)
+            set(FSFW_SUBVERSION ${FSFW_SUBVERSION_IF_GIT_FAILS})
+        endif()
+        if(NOT FSFW_REVISION)
+            set(FSFW_REVISION ${FSFW_REVISION_IF_GIT_FAILS})
+        endif()
+        set(FSFW_GIT_VER_HANDLING_OK TRUE)
+   else()
+        set(FSFW_GIT_VER_HANDLING_OK FALSE)
+   endif()
+endif()
+if(NOT FSFW_GIT_VER_HANDLING_OK)
+    set(FSFW_VERSION ${FSFW_VERSION_IF_GIT_FAILS})
+    set(FSFW_SUBVERSION ${FSFW_SUBVERSION_IF_GIT_FAILS})
+    set(FSFW_REVISION ${FSFW_REVISION_IF_GIT_FAILS})
+endif()
+
 if(FSFW_BUILD_UNITTESTS)
-    message(STATUS "Building the FSFW unittests in addition to the static library")
+    message(STATUS "${MSG_PREFIX} Building the FSFW unittests in addition to the static library")
     # Check whether the user has already installed Catch2 first
-    find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION})
+    find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION} CONFIG QUIET)
     # Not installed, so use FetchContent to download and provide Catch2
     if(NOT Catch2_FOUND)
-        message(STATUS "Catch2 installation not found. Downloading Catch2 library with FetchContent")
+        message(STATUS "${MSG_PREFIX} Catch2 installation not found. Downloading Catch2 library with FetchContent")
         include(FetchContent)
 
         FetchContent_Declare(
@@ -89,42 +123,35 @@ if(FSFW_BUILD_UNITTESTS)
     add_executable(${FSFW_TEST_TGT})
 
     if(FSFW_TESTS_GEN_COV)
-        message(STATUS "Generating coverage data for the library")
-        message(STATUS "Targets linking against ${LIB_FSFW_NAME} "
+        message(STATUS "${MSG_PREFIX} Generating coverage data for the library")
+        message(STATUS "${MSG_PREFIX} Targets linking against ${LIB_FSFW_NAME} "
             "will be compiled with coverage data as well"
         )
-        include(FetchContent)
-        FetchContent_Declare(
-            cmake-modules
-            GIT_REPOSITORY https://github.com/bilke/cmake-modules.git
-        )
-        FetchContent_MakeAvailable(cmake-modules)
         set(CMAKE_BUILD_TYPE "Debug")
-        list(APPEND CMAKE_MODULE_PATH ${cmake-modules_SOURCE_DIR})
         include(CodeCoverage)
     endif()
 endif()
 
-message(STATUS "Finding and/or providing ETL library")
+message(STATUS "Finding and/or providing etl library with version ${FSFW_ETL_LIB_MAJOR_VERSION}")
 
 # Check whether the user has already installed ETL first
-find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} QUIET)
+# find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} CONFIG QUIET)
 # Not installed, so use FetchContent to download and provide etl
-if(NOT ${FSFW_ETL_LIB_NAME}_FOUND)
-    message(STATUS
-        "No ETL installation was found with find_package. Installing and providing "
-        "etl with FindPackage"
-    )
-    include(FetchContent)
+# if(NOT ${FSFW_ETL_LIB_NAME}_FOUND)
+message(STATUS
+    "No etl installation was found with find_package. Installing and providing "
+    "etl with FetchContent"
+)
+include(FetchContent)
 
-    FetchContent_Declare(
-        ${FSFW_ETL_LIB_NAME}
-        GIT_REPOSITORY https://github.com/ETLCPP/etl
-        GIT_TAG        ${FSFW_ETL_LIB_VERSION}
-    )
+FetchContent_Declare(
+    ${FSFW_ETL_LIB_NAME}
+    GIT_REPOSITORY https://github.com/ETLCPP/etl
+    GIT_TAG        ${FSFW_ETL_LIB_VERSION}
+)
 
-    FetchContent_MakeAvailable(etl)
-endif()
+FetchContent_MakeAvailable(${FSFW_ETL_LIB_NAME})
+# endif()
 
 set(FSFW_CORE_INC_PATH "inc")
 
@@ -142,17 +169,17 @@ 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")
+    message(FATAL_ERROR "${MSG_PREFIX} 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")
+    message(WARNING "${MSG_PREFIX} Please pass the FSFW OSAL as FSFW_OSAL instead of OS_FSFW")
     set(FSFW_OSAL OS_FSFW)
 endif()
 
 if(NOT FSFW_OSAL)
-	message(STATUS "No OS for FSFW via FSFW_OSAL set. Assuming host OS")
+	message(STATUS "${MSG_PREFIX} No OS for FSFW via FSFW_OSAL set. Assuming host OS")
 	# Assume host OS and autodetermine from OS_FSFW
 	if(UNIX)
 		set(FSFW_OSAL  "linux" 
@@ -186,7 +213,7 @@ elseif(FSFW_OSAL STREQUAL rtems)
     set(FSFW_OSAL_RTEMS ON)
 else()
     message(WARNING
-        "Invalid operating system for FSFW specified! Setting to host.."
+        "${MSG_PREFIX} Invalid operating system for FSFW specified! Setting to host.."
     )
     set(FSFW_OS_NAME "Host")
     set(OS_FSFW "host")
@@ -195,7 +222,7 @@ endif()
 configure_file(src/fsfw/FSFW.h.in fsfw/FSFW.h)
 configure_file(src/fsfw/FSFWVersion.h.in fsfw/FSFWVersion.h)
 
-message(STATUS "Compiling FSFW for the ${FSFW_OS_NAME} operating system.")
+message(STATUS "${MSG_PREFIX} Compiling FSFW for the ${FSFW_OS_NAME} operating system")
 
 add_subdirectory(src)
 add_subdirectory(tests)
@@ -279,8 +306,8 @@ endif()
 if(NOT FSFW_CONFIG_PATH)
     set(DEF_CONF_PATH misc/defaultcfg/fsfwconfig)
     if(NOT FSFW_BUILD_DOCS)
-        message(WARNING "Flight Software Framework configuration path not set!")
-        message(WARNING "Setting default configuration from ${DEF_CONF_PATH} ..")
+        message(WARNING "${MSG_PREFIX} Flight Software Framework configuration path not set")
+        message(WARNING "${MSG_PREFIX} Setting default configuration from ${DEF_CONF_PATH} ..")
     endif()
     add_subdirectory(${DEF_CONF_PATH})
     set(FSFW_CONFIG_PATH ${DEF_CONF_PATH})
@@ -387,7 +414,7 @@ target_compile_options(${LIB_FSFW_NAME} PRIVATE
 )
 
 target_link_libraries(${LIB_FSFW_NAME} PRIVATE
-    ${FSFW_ETL_LIB_NAME}
+    etl
     ${FSFW_ADDITIONAL_LINK_LIBS}
 )
 
diff --git a/cmake/FsfwHelpers.cmake b/cmake/FsfwHelpers.cmake
new file mode 100644
index 00000000..3f22ebde
--- /dev/null
+++ b/cmake/FsfwHelpers.cmake
@@ -0,0 +1,28 @@
+# Determines the git version with git describe and returns it by setting
+# the GIT_INFO list in the parent scope. The list has the following entries
+# 1. Full version string
+# 2. Major version
+# 3. Minor version
+# 4. Revision
+# 5. git SHA hash and commits since tag
+function(determine_version_with_git)
+    include(GetGitRevisionDescription)
+    git_describe(VERSION ${ARGN})
+    string(FIND ${VERSION} "." VALID_VERSION)
+    if(VALID_VERSION EQUAL -1)
+        message(WARNING "Version string ${VERSION} retrieved with git describe is invalid")
+        return()
+    endif()
+    # Parse the version information into pieces.
+    string(REGEX REPLACE "^v([0-9]+)\\..*" "\\1" _VERSION_MAJOR "${VERSION}")
+    string(REGEX REPLACE "^v[0-9]+\\.([0-9]+).*" "\\1" _VERSION_MINOR "${VERSION}")
+    string(REGEX REPLACE "^v[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" _VERSION_PATCH "${VERSION}")
+    string(REGEX REPLACE "^v[0-9]+\\.[0-9]+\\.[0-9]+-(.*)" "\\1" VERSION_SHA1 "${VERSION}")
+    set(GIT_INFO ${VERSION})
+    list(APPEND GIT_INFO ${_VERSION_MAJOR})
+    list(APPEND GIT_INFO ${_VERSION_MINOR})
+    list(APPEND GIT_INFO ${_VERSION_PATCH})
+    list(APPEND GIT_INFO ${VERSION_SHA1})
+    set(GIT_INFO ${GIT_INFO} PARENT_SCOPE)
+    message(STATUS "${MSG_PREFIX} Set git version info into GIT_INFO from the git tag ${VERSION}")
+endfunction()
diff --git a/cmake/cmake-modules/CodeCoverage.cmake b/cmake/cmake-modules/CodeCoverage.cmake
new file mode 100644
index 00000000..aef3d943
--- /dev/null
+++ b/cmake/cmake-modules/CodeCoverage.cmake
@@ -0,0 +1,719 @@
+# Copyright (c) 2012 - 2017, Lars Bilke
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without modification,
+# are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this
+#    list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+#    this list of conditions and the following disclaimer in the documentation
+#    and/or other materials provided with the distribution.
+#
+# 3. Neither the name of the copyright holder nor the names of its contributors
+#    may be used to endorse or promote products derived from this software without
+#    specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#
+# CHANGES:
+#
+# 2012-01-31, Lars Bilke
+# - Enable Code Coverage
+#
+# 2013-09-17, Joakim Söderberg
+# - Added support for Clang.
+# - Some additional usage instructions.
+#
+# 2016-02-03, Lars Bilke
+# - Refactored functions to use named parameters
+#
+# 2017-06-02, Lars Bilke
+# - Merged with modified version from github.com/ufz/ogs
+#
+# 2019-05-06, Anatolii Kurotych
+# - Remove unnecessary --coverage flag
+#
+# 2019-12-13, FeRD (Frank Dana)
+# - Deprecate COVERAGE_LCOVR_EXCLUDES and COVERAGE_GCOVR_EXCLUDES lists in favor
+#   of tool-agnostic COVERAGE_EXCLUDES variable, or EXCLUDE setup arguments.
+# - CMake 3.4+: All excludes can be specified relative to BASE_DIRECTORY
+# - All setup functions: accept BASE_DIRECTORY, EXCLUDE list
+# - Set lcov basedir with -b argument
+# - Add automatic --demangle-cpp in lcovr, if 'c++filt' is available (can be
+#   overridden with NO_DEMANGLE option in setup_target_for_coverage_lcovr().)
+# - Delete output dir, .info file on 'make clean'
+# - Remove Python detection, since version mismatches will break gcovr
+# - Minor cleanup (lowercase function names, update examples...)
+#
+# 2019-12-19, FeRD (Frank Dana)
+# - Rename Lcov outputs, make filtered file canonical, fix cleanup for targets
+#
+# 2020-01-19, Bob Apthorpe
+# - Added gfortran support
+#
+# 2020-02-17, FeRD (Frank Dana)
+# - Make all add_custom_target()s VERBATIM to auto-escape wildcard characters
+#   in EXCLUDEs, and remove manual escaping from gcovr targets
+#
+# 2021-01-19, Robin Mueller
+# - Add CODE_COVERAGE_VERBOSE option which will allow to print out commands which are run
+# - Added the option for users to set the GCOVR_ADDITIONAL_ARGS variable to supply additional
+#   flags to the gcovr command
+#
+# 2020-05-04, Mihchael Davis
+#     - Add -fprofile-abs-path to make gcno files contain absolute paths
+#     - Fix BASE_DIRECTORY not working when defined
+#     - Change BYPRODUCT from folder to index.html to stop ninja from complaining about double defines
+#
+# 2021-05-10, Martin Stump
+#     - Check if the generator is multi-config before warning about non-Debug builds
+#
+# 2022-02-22, Marko Wehle
+#     - Change gcovr output from -o <filename> for --xml <filename> and --html <filename> output respectively.
+#       This will allow for Multiple Output Formats at the same time by making use of GCOVR_ADDITIONAL_ARGS, e.g. GCOVR_ADDITIONAL_ARGS "--txt".
+#
+# USAGE:
+#
+# 1. Copy this file into your cmake modules path.
+#
+# 2. Add the following line to your CMakeLists.txt (best inside an if-condition
+#    using a CMake option() to enable it just optionally):
+#      include(CodeCoverage)
+#
+# 3. Append necessary compiler flags for all supported source files:
+#      append_coverage_compiler_flags()
+#    Or for specific target:
+#      append_coverage_compiler_flags_to_target(YOUR_TARGET_NAME)
+#
+# 3.a (OPTIONAL) Set appropriate optimization flags, e.g. -O0, -O1 or -Og
+#
+# 4. If you need to exclude additional directories from the report, specify them
+#    using full paths in the COVERAGE_EXCLUDES variable before calling
+#    setup_target_for_coverage_*().
+#    Example:
+#      set(COVERAGE_EXCLUDES
+#          '${PROJECT_SOURCE_DIR}/src/dir1/*'
+#          '/path/to/my/src/dir2/*')
+#    Or, use the EXCLUDE argument to setup_target_for_coverage_*().
+#    Example:
+#      setup_target_for_coverage_lcov(
+#          NAME coverage
+#          EXECUTABLE testrunner
+#          EXCLUDE "${PROJECT_SOURCE_DIR}/src/dir1/*" "/path/to/my/src/dir2/*")
+#
+# 4.a NOTE: With CMake 3.4+, COVERAGE_EXCLUDES or EXCLUDE can also be set
+#     relative to the BASE_DIRECTORY (default: PROJECT_SOURCE_DIR)
+#     Example:
+#       set(COVERAGE_EXCLUDES "dir1/*")
+#       setup_target_for_coverage_gcovr_html(
+#           NAME coverage
+#           EXECUTABLE testrunner
+#           BASE_DIRECTORY "${PROJECT_SOURCE_DIR}/src"
+#           EXCLUDE "dir2/*")
+#
+# 5. Use the functions described below to create a custom make target which
+#    runs your test executable and produces a code coverage report.
+#
+# 6. Build a Debug build:
+#      cmake -DCMAKE_BUILD_TYPE=Debug ..
+#      make
+#      make my_coverage_target
+#
+
+include(CMakeParseArguments)
+
+option(CODE_COVERAGE_VERBOSE "Verbose information" FALSE)
+
+# Check prereqs
+find_program( GCOV_PATH gcov )
+find_program( LCOV_PATH  NAMES lcov lcov.bat lcov.exe lcov.perl)
+find_program( FASTCOV_PATH NAMES fastcov fastcov.py )
+find_program( GENHTML_PATH NAMES genhtml genhtml.perl genhtml.bat )
+find_program( GCOVR_PATH gcovr PATHS ${CMAKE_SOURCE_DIR}/scripts/test)
+find_program( CPPFILT_PATH NAMES c++filt )
+
+if(NOT GCOV_PATH)
+    message(FATAL_ERROR "gcov not found! Aborting...")
+endif() # NOT GCOV_PATH
+
+get_property(LANGUAGES GLOBAL PROPERTY ENABLED_LANGUAGES)
+list(GET LANGUAGES 0 LANG)
+
+if("${CMAKE_${LANG}_COMPILER_ID}" MATCHES "(Apple)?[Cc]lang")
+    if("${CMAKE_${LANG}_COMPILER_VERSION}" VERSION_LESS 3)
+        message(FATAL_ERROR "Clang version must be 3.0.0 or greater! Aborting...")
+    endif()
+elseif(NOT CMAKE_COMPILER_IS_GNUCXX)
+    if("${CMAKE_Fortran_COMPILER_ID}" MATCHES "[Ff]lang")
+        # Do nothing; exit conditional without error if true
+    elseif("${CMAKE_Fortran_COMPILER_ID}" MATCHES "GNU")
+        # Do nothing; exit conditional without error if true
+    else()
+        message(FATAL_ERROR "Compiler is not GNU gcc! Aborting...")
+    endif()
+endif()
+
+set(COVERAGE_COMPILER_FLAGS "-g -fprofile-arcs -ftest-coverage"
+    CACHE INTERNAL "")
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+    include(CheckCXXCompilerFlag)
+    check_cxx_compiler_flag(-fprofile-abs-path HAVE_fprofile_abs_path)
+    if(HAVE_fprofile_abs_path)
+        set(COVERAGE_COMPILER_FLAGS "${COVERAGE_COMPILER_FLAGS} -fprofile-abs-path")
+    endif()
+endif()
+
+set(CMAKE_Fortran_FLAGS_COVERAGE
+    ${COVERAGE_COMPILER_FLAGS}
+    CACHE STRING "Flags used by the Fortran compiler during coverage builds."
+    FORCE )
+set(CMAKE_CXX_FLAGS_COVERAGE
+    ${COVERAGE_COMPILER_FLAGS}
+    CACHE STRING "Flags used by the C++ compiler during coverage builds."
+    FORCE )
+set(CMAKE_C_FLAGS_COVERAGE
+    ${COVERAGE_COMPILER_FLAGS}
+    CACHE STRING "Flags used by the C compiler during coverage builds."
+    FORCE )
+set(CMAKE_EXE_LINKER_FLAGS_COVERAGE
+    ""
+    CACHE STRING "Flags used for linking binaries during coverage builds."
+    FORCE )
+set(CMAKE_SHARED_LINKER_FLAGS_COVERAGE
+    ""
+    CACHE STRING "Flags used by the shared libraries linker during coverage builds."
+    FORCE )
+mark_as_advanced(
+    CMAKE_Fortran_FLAGS_COVERAGE
+    CMAKE_CXX_FLAGS_COVERAGE
+    CMAKE_C_FLAGS_COVERAGE
+    CMAKE_EXE_LINKER_FLAGS_COVERAGE
+    CMAKE_SHARED_LINKER_FLAGS_COVERAGE )
+
+get_property(GENERATOR_IS_MULTI_CONFIG GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG)
+if(NOT (CMAKE_BUILD_TYPE STREQUAL "Debug" OR GENERATOR_IS_MULTI_CONFIG))
+    message(WARNING "Code coverage results with an optimised (non-Debug) build may be misleading")
+endif() # NOT (CMAKE_BUILD_TYPE STREQUAL "Debug" OR GENERATOR_IS_MULTI_CONFIG)
+
+if(CMAKE_C_COMPILER_ID STREQUAL "GNU" OR CMAKE_Fortran_COMPILER_ID STREQUAL "GNU")
+    link_libraries(gcov)
+endif()
+
+# Defines a target for running and collection code coverage information
+# Builds dependencies, runs the given executable and outputs reports.
+# NOTE! The executable should always have a ZERO as exit code otherwise
+# the coverage generation will not complete.
+#
+# setup_target_for_coverage_lcov(
+#     NAME testrunner_coverage                    # New target name
+#     EXECUTABLE testrunner -j ${PROCESSOR_COUNT} # Executable in PROJECT_BINARY_DIR
+#     DEPENDENCIES testrunner                     # Dependencies to build first
+#     BASE_DIRECTORY "../"                        # Base directory for report
+#                                                 #  (defaults to PROJECT_SOURCE_DIR)
+#     EXCLUDE "src/dir1/*" "src/dir2/*"           # Patterns to exclude (can be relative
+#                                                 #  to BASE_DIRECTORY, with CMake 3.4+)
+#     NO_DEMANGLE                                 # Don't demangle C++ symbols
+#                                                 #  even if c++filt is found
+# )
+function(setup_target_for_coverage_lcov)
+
+    set(options NO_DEMANGLE)
+    set(oneValueArgs BASE_DIRECTORY NAME)
+    set(multiValueArgs EXCLUDE EXECUTABLE EXECUTABLE_ARGS DEPENDENCIES LCOV_ARGS GENHTML_ARGS)
+    cmake_parse_arguments(Coverage "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+    if(NOT LCOV_PATH)
+        message(FATAL_ERROR "lcov not found! Aborting...")
+    endif() # NOT LCOV_PATH
+
+    if(NOT GENHTML_PATH)
+        message(FATAL_ERROR "genhtml not found! Aborting...")
+    endif() # NOT GENHTML_PATH
+
+    # Set base directory (as absolute path), or default to PROJECT_SOURCE_DIR
+    if(DEFINED Coverage_BASE_DIRECTORY)
+        get_filename_component(BASEDIR ${Coverage_BASE_DIRECTORY} ABSOLUTE)
+    else()
+        set(BASEDIR ${PROJECT_SOURCE_DIR})
+    endif()
+
+    # Collect excludes (CMake 3.4+: Also compute absolute paths)
+    set(LCOV_EXCLUDES "")
+    foreach(EXCLUDE ${Coverage_EXCLUDE} ${COVERAGE_EXCLUDES} ${COVERAGE_LCOV_EXCLUDES})
+        if(CMAKE_VERSION VERSION_GREATER 3.4)
+            get_filename_component(EXCLUDE ${EXCLUDE} ABSOLUTE BASE_DIR ${BASEDIR})
+        endif()
+        list(APPEND LCOV_EXCLUDES "${EXCLUDE}")
+    endforeach()
+    list(REMOVE_DUPLICATES LCOV_EXCLUDES)
+
+    # Conditional arguments
+    if(CPPFILT_PATH AND NOT ${Coverage_NO_DEMANGLE})
+      set(GENHTML_EXTRA_ARGS "--demangle-cpp")
+    endif()
+     
+    # Setting up commands which will be run to generate coverage data.
+    # Cleanup lcov
+    set(LCOV_CLEAN_CMD 
+        ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} -directory . 
+        -b ${BASEDIR} --zerocounters
+    )
+    # Create baseline to make sure untouched files show up in the report
+    set(LCOV_BASELINE_CMD 
+        ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} -c -i -d . -b 
+        ${BASEDIR} -o ${Coverage_NAME}.base
+    )
+    # Run tests
+    set(LCOV_EXEC_TESTS_CMD 
+        ${Coverage_EXECUTABLE} ${Coverage_EXECUTABLE_ARGS}
+    )    
+    # Capturing lcov counters and generating report
+    set(LCOV_CAPTURE_CMD 
+        ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} --directory . -b 
+        ${BASEDIR} --capture --output-file ${Coverage_NAME}.capture
+    )
+    # add baseline counters
+    set(LCOV_BASELINE_COUNT_CMD
+        ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} -a ${Coverage_NAME}.base 
+        -a ${Coverage_NAME}.capture --output-file ${Coverage_NAME}.total
+    ) 
+    # filter collected data to final coverage report
+    set(LCOV_FILTER_CMD 
+        ${LCOV_PATH} ${Coverage_LCOV_ARGS} --gcov-tool ${GCOV_PATH} --remove 
+        ${Coverage_NAME}.total ${LCOV_EXCLUDES} --output-file ${Coverage_NAME}.info
+    )    
+    # Generate HTML output
+    set(LCOV_GEN_HTML_CMD
+        ${GENHTML_PATH} ${GENHTML_EXTRA_ARGS} ${Coverage_GENHTML_ARGS} -o 
+        ${Coverage_NAME} ${Coverage_NAME}.info
+    )
+    
+
+    if(CODE_COVERAGE_VERBOSE)
+        message(STATUS "Executed command report")
+        message(STATUS "Command to clean up lcov: ")
+        string(REPLACE ";" " " LCOV_CLEAN_CMD_SPACED "${LCOV_CLEAN_CMD}")
+        message(STATUS "${LCOV_CLEAN_CMD_SPACED}")
+
+        message(STATUS "Command to create baseline: ")
+        string(REPLACE ";" " " LCOV_BASELINE_CMD_SPACED "${LCOV_BASELINE_CMD}")
+        message(STATUS "${LCOV_BASELINE_CMD_SPACED}")
+
+        message(STATUS "Command to run the tests: ")
+        string(REPLACE ";" " " LCOV_EXEC_TESTS_CMD_SPACED "${LCOV_EXEC_TESTS_CMD}")
+        message(STATUS "${LCOV_EXEC_TESTS_CMD_SPACED}")
+
+        message(STATUS "Command to capture counters and generate report: ")
+        string(REPLACE ";" " " LCOV_CAPTURE_CMD_SPACED "${LCOV_CAPTURE_CMD}")
+        message(STATUS "${LCOV_CAPTURE_CMD_SPACED}")
+
+        message(STATUS "Command to add baseline counters: ")
+        string(REPLACE ";" " " LCOV_BASELINE_COUNT_CMD_SPACED "${LCOV_BASELINE_COUNT_CMD}")
+        message(STATUS "${LCOV_BASELINE_COUNT_CMD_SPACED}")
+
+        message(STATUS "Command to filter collected data: ")
+        string(REPLACE ";" " " LCOV_FILTER_CMD_SPACED "${LCOV_FILTER_CMD}")
+        message(STATUS "${LCOV_FILTER_CMD_SPACED}")
+
+        message(STATUS "Command to generate lcov HTML output: ")
+        string(REPLACE ";" " " LCOV_GEN_HTML_CMD_SPACED "${LCOV_GEN_HTML_CMD}")
+        message(STATUS "${LCOV_GEN_HTML_CMD_SPACED}")
+    endif()
+
+    # Setup target
+    add_custom_target(${Coverage_NAME}
+        COMMAND ${LCOV_CLEAN_CMD}
+        COMMAND ${LCOV_BASELINE_CMD} 
+        COMMAND ${LCOV_EXEC_TESTS_CMD}
+        COMMAND ${LCOV_CAPTURE_CMD}
+        COMMAND ${LCOV_BASELINE_COUNT_CMD}
+        COMMAND ${LCOV_FILTER_CMD} 
+        COMMAND ${LCOV_GEN_HTML_CMD}
+
+        # Set output files as GENERATED (will be removed on 'make clean')
+        BYPRODUCTS
+            ${Coverage_NAME}.base
+            ${Coverage_NAME}.capture
+            ${Coverage_NAME}.total
+            ${Coverage_NAME}.info
+            ${Coverage_NAME}/index.html
+        WORKING_DIRECTORY ${PROJECT_BINARY_DIR}
+        DEPENDS ${Coverage_DEPENDENCIES}
+        VERBATIM # Protect arguments to commands
+        COMMENT "Resetting code coverage counters to zero.\nProcessing code coverage counters and generating report."
+    )
+
+    # Show where to find the lcov info report
+    add_custom_command(TARGET ${Coverage_NAME} POST_BUILD
+        COMMAND ;
+        COMMENT "Lcov code coverage info report saved in ${Coverage_NAME}.info."
+    )
+
+    # Show info where to find the report
+    add_custom_command(TARGET ${Coverage_NAME} POST_BUILD
+        COMMAND ;
+        COMMENT "Open ./${Coverage_NAME}/index.html in your browser to view the coverage report."
+    )
+
+endfunction() # setup_target_for_coverage_lcov
+
+# Defines a target for running and collection code coverage information
+# Builds dependencies, runs the given executable and outputs reports.
+# NOTE! The executable should always have a ZERO as exit code otherwise
+# the coverage generation will not complete.
+#
+# setup_target_for_coverage_gcovr_xml(
+#     NAME ctest_coverage                    # New target name
+#     EXECUTABLE ctest -j ${PROCESSOR_COUNT} # Executable in PROJECT_BINARY_DIR
+#     DEPENDENCIES executable_target         # Dependencies to build first
+#     BASE_DIRECTORY "../"                   # Base directory for report
+#                                            #  (defaults to PROJECT_SOURCE_DIR)
+#     EXCLUDE "src/dir1/*" "src/dir2/*"      # Patterns to exclude (can be relative
+#                                            #  to BASE_DIRECTORY, with CMake 3.4+)
+# )
+# The user can set the variable GCOVR_ADDITIONAL_ARGS to supply additional flags to the
+# GCVOR command.
+function(setup_target_for_coverage_gcovr_xml)
+
+    set(options NONE)
+    set(oneValueArgs BASE_DIRECTORY NAME)
+    set(multiValueArgs EXCLUDE EXECUTABLE EXECUTABLE_ARGS DEPENDENCIES)
+    cmake_parse_arguments(Coverage "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+    if(NOT GCOVR_PATH)
+        message(FATAL_ERROR "gcovr not found! Aborting...")
+    endif() # NOT GCOVR_PATH
+
+    # Set base directory (as absolute path), or default to PROJECT_SOURCE_DIR
+    if(DEFINED Coverage_BASE_DIRECTORY)
+        get_filename_component(BASEDIR ${Coverage_BASE_DIRECTORY} ABSOLUTE)
+    else()
+        set(BASEDIR ${PROJECT_SOURCE_DIR})
+    endif()
+
+    # Collect excludes (CMake 3.4+: Also compute absolute paths)
+    set(GCOVR_EXCLUDES "")
+    foreach(EXCLUDE ${Coverage_EXCLUDE} ${COVERAGE_EXCLUDES} ${COVERAGE_GCOVR_EXCLUDES})
+        if(CMAKE_VERSION VERSION_GREATER 3.4)
+            get_filename_component(EXCLUDE ${EXCLUDE} ABSOLUTE BASE_DIR ${BASEDIR})
+        endif()
+        list(APPEND GCOVR_EXCLUDES "${EXCLUDE}")
+    endforeach()
+    list(REMOVE_DUPLICATES GCOVR_EXCLUDES)
+
+    # Combine excludes to several -e arguments
+    set(GCOVR_EXCLUDE_ARGS "")
+    foreach(EXCLUDE ${GCOVR_EXCLUDES})
+        list(APPEND GCOVR_EXCLUDE_ARGS "-e")
+        list(APPEND GCOVR_EXCLUDE_ARGS "${EXCLUDE}")
+    endforeach()
+    
+    # Set up commands which will be run to generate coverage data
+    # Run tests
+    set(GCOVR_XML_EXEC_TESTS_CMD
+        ${Coverage_EXECUTABLE} ${Coverage_EXECUTABLE_ARGS}
+    )
+    # Running gcovr
+    set(GCOVR_XML_CMD
+        ${GCOVR_PATH} --xml ${Coverage_NAME}.xml -r ${BASEDIR} ${GCOVR_ADDITIONAL_ARGS}
+        ${GCOVR_EXCLUDE_ARGS} --object-directory=${PROJECT_BINARY_DIR}
+    )
+    
+    if(CODE_COVERAGE_VERBOSE)
+        message(STATUS "Executed command report")
+
+        message(STATUS "Command to run tests: ")
+        string(REPLACE ";" " " GCOVR_XML_EXEC_TESTS_CMD_SPACED "${GCOVR_XML_EXEC_TESTS_CMD}")
+        message(STATUS "${GCOVR_XML_EXEC_TESTS_CMD_SPACED}")
+
+        message(STATUS "Command to generate gcovr XML coverage data: ")
+        string(REPLACE ";" " " GCOVR_XML_CMD_SPACED "${GCOVR_XML_CMD}")
+        message(STATUS "${GCOVR_XML_CMD_SPACED}")
+    endif()
+
+    add_custom_target(${Coverage_NAME}
+        COMMAND ${GCOVR_XML_EXEC_TESTS_CMD}
+        COMMAND ${GCOVR_XML_CMD}
+        
+        BYPRODUCTS ${Coverage_NAME}.xml
+        WORKING_DIRECTORY ${PROJECT_BINARY_DIR}
+        DEPENDS ${Coverage_DEPENDENCIES}
+        VERBATIM # Protect arguments to commands
+        COMMENT "Running gcovr to produce Cobertura code coverage report."
+    )
+
+    # Show info where to find the report
+    add_custom_command(TARGET ${Coverage_NAME} POST_BUILD
+        COMMAND ;
+        COMMENT "Cobertura code coverage report saved in ${Coverage_NAME}.xml."
+    )
+endfunction() # setup_target_for_coverage_gcovr_xml
+
+# Defines a target for running and collection code coverage information
+# Builds dependencies, runs the given executable and outputs reports.
+# NOTE! The executable should always have a ZERO as exit code otherwise
+# the coverage generation will not complete.
+#
+# setup_target_for_coverage_gcovr_html(
+#     NAME ctest_coverage                    # New target name
+#     EXECUTABLE ctest -j ${PROCESSOR_COUNT} # Executable in PROJECT_BINARY_DIR
+#     DEPENDENCIES executable_target         # Dependencies to build first
+#     BASE_DIRECTORY "../"                   # Base directory for report
+#                                            #  (defaults to PROJECT_SOURCE_DIR)
+#     EXCLUDE "src/dir1/*" "src/dir2/*"      # Patterns to exclude (can be relative
+#                                            #  to BASE_DIRECTORY, with CMake 3.4+)
+# )
+# The user can set the variable GCOVR_ADDITIONAL_ARGS to supply additional flags to the
+# GCVOR command.
+function(setup_target_for_coverage_gcovr_html)
+
+    set(options NONE)
+    set(oneValueArgs BASE_DIRECTORY NAME)
+    set(multiValueArgs EXCLUDE EXECUTABLE EXECUTABLE_ARGS DEPENDENCIES)
+    cmake_parse_arguments(Coverage "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+    if(NOT GCOVR_PATH)
+        message(FATAL_ERROR "gcovr not found! Aborting...")
+    endif() # NOT GCOVR_PATH
+
+    # Set base directory (as absolute path), or default to PROJECT_SOURCE_DIR
+    if(DEFINED Coverage_BASE_DIRECTORY)
+        get_filename_component(BASEDIR ${Coverage_BASE_DIRECTORY} ABSOLUTE)
+    else()
+        set(BASEDIR ${PROJECT_SOURCE_DIR})
+    endif()
+
+    # Collect excludes (CMake 3.4+: Also compute absolute paths)
+    set(GCOVR_EXCLUDES "")
+    foreach(EXCLUDE ${Coverage_EXCLUDE} ${COVERAGE_EXCLUDES} ${COVERAGE_GCOVR_EXCLUDES})
+        if(CMAKE_VERSION VERSION_GREATER 3.4)
+            get_filename_component(EXCLUDE ${EXCLUDE} ABSOLUTE BASE_DIR ${BASEDIR})
+        endif()
+        list(APPEND GCOVR_EXCLUDES "${EXCLUDE}")
+    endforeach()
+    list(REMOVE_DUPLICATES GCOVR_EXCLUDES)
+
+    # Combine excludes to several -e arguments
+    set(GCOVR_EXCLUDE_ARGS "")
+    foreach(EXCLUDE ${GCOVR_EXCLUDES})
+        list(APPEND GCOVR_EXCLUDE_ARGS "-e")
+        list(APPEND GCOVR_EXCLUDE_ARGS "${EXCLUDE}")
+    endforeach()
+
+    # Set up commands which will be run to generate coverage data
+    # Run tests
+    set(GCOVR_HTML_EXEC_TESTS_CMD
+        ${Coverage_EXECUTABLE} ${Coverage_EXECUTABLE_ARGS}
+    )
+    # Create folder
+    set(GCOVR_HTML_FOLDER_CMD
+        ${CMAKE_COMMAND} -E make_directory ${PROJECT_BINARY_DIR}/${Coverage_NAME}
+    )
+    # Running gcovr
+    set(GCOVR_HTML_CMD
+        ${GCOVR_PATH} --html ${Coverage_NAME}/index.html --html-details -r ${BASEDIR} ${GCOVR_ADDITIONAL_ARGS}
+        ${GCOVR_EXCLUDE_ARGS} --object-directory=${PROJECT_BINARY_DIR}
+    )
+
+    if(CODE_COVERAGE_VERBOSE)
+        message(STATUS "Executed command report")
+
+        message(STATUS "Command to run tests: ")
+        string(REPLACE ";" " " GCOVR_HTML_EXEC_TESTS_CMD_SPACED "${GCOVR_HTML_EXEC_TESTS_CMD}")
+        message(STATUS "${GCOVR_HTML_EXEC_TESTS_CMD_SPACED}")
+
+        message(STATUS "Command to create a folder: ")
+        string(REPLACE ";" " " GCOVR_HTML_FOLDER_CMD_SPACED "${GCOVR_HTML_FOLDER_CMD}")
+        message(STATUS "${GCOVR_HTML_FOLDER_CMD_SPACED}")
+
+        message(STATUS "Command to generate gcovr HTML coverage data: ")
+        string(REPLACE ";" " " GCOVR_HTML_CMD_SPACED "${GCOVR_HTML_CMD}")
+        message(STATUS "${GCOVR_HTML_CMD_SPACED}")
+    endif()
+
+    add_custom_target(${Coverage_NAME}
+        COMMAND ${GCOVR_HTML_EXEC_TESTS_CMD}
+        COMMAND ${GCOVR_HTML_FOLDER_CMD}
+        COMMAND ${GCOVR_HTML_CMD}
+
+        BYPRODUCTS ${PROJECT_BINARY_DIR}/${Coverage_NAME}/index.html  # report directory
+        WORKING_DIRECTORY ${PROJECT_BINARY_DIR}
+        DEPENDS ${Coverage_DEPENDENCIES}
+        VERBATIM # Protect arguments to commands
+        COMMENT "Running gcovr to produce HTML code coverage report."
+    )
+
+    # Show info where to find the report
+    add_custom_command(TARGET ${Coverage_NAME} POST_BUILD
+        COMMAND ;
+        COMMENT "Open ./${Coverage_NAME}/index.html in your browser to view the coverage report."
+    )
+
+endfunction() # setup_target_for_coverage_gcovr_html
+
+# Defines a target for running and collection code coverage information
+# Builds dependencies, runs the given executable and outputs reports.
+# NOTE! The executable should always have a ZERO as exit code otherwise
+# the coverage generation will not complete.
+#
+# setup_target_for_coverage_fastcov(
+#     NAME testrunner_coverage                    # New target name
+#     EXECUTABLE testrunner -j ${PROCESSOR_COUNT} # Executable in PROJECT_BINARY_DIR
+#     DEPENDENCIES testrunner                     # Dependencies to build first
+#     BASE_DIRECTORY "../"                        # Base directory for report
+#                                                 #  (defaults to PROJECT_SOURCE_DIR)
+#     EXCLUDE "src/dir1/" "src/dir2/"             # Patterns to exclude.
+#     NO_DEMANGLE                                 # Don't demangle C++ symbols
+#                                                 #  even if c++filt is found
+#     SKIP_HTML                                   # Don't create html report
+#     POST_CMD perl -i -pe s!${PROJECT_SOURCE_DIR}/!!g ctest_coverage.json  # E.g. for stripping source dir from file paths
+# )
+function(setup_target_for_coverage_fastcov)
+
+    set(options NO_DEMANGLE SKIP_HTML)
+    set(oneValueArgs BASE_DIRECTORY NAME)
+    set(multiValueArgs EXCLUDE EXECUTABLE EXECUTABLE_ARGS DEPENDENCIES FASTCOV_ARGS GENHTML_ARGS POST_CMD)
+    cmake_parse_arguments(Coverage "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+    if(NOT FASTCOV_PATH)
+        message(FATAL_ERROR "fastcov not found! Aborting...")
+    endif()
+
+    if(NOT Coverage_SKIP_HTML AND NOT GENHTML_PATH)
+        message(FATAL_ERROR "genhtml not found! Aborting...")
+    endif()
+
+    # Set base directory (as absolute path), or default to PROJECT_SOURCE_DIR
+    if(Coverage_BASE_DIRECTORY)
+        get_filename_component(BASEDIR ${Coverage_BASE_DIRECTORY} ABSOLUTE)
+    else()
+        set(BASEDIR ${PROJECT_SOURCE_DIR})
+    endif()
+
+    # Collect excludes (Patterns, not paths, for fastcov)
+    set(FASTCOV_EXCLUDES "")
+    foreach(EXCLUDE ${Coverage_EXCLUDE} ${COVERAGE_EXCLUDES} ${COVERAGE_FASTCOV_EXCLUDES})
+        list(APPEND FASTCOV_EXCLUDES "${EXCLUDE}")
+    endforeach()
+    list(REMOVE_DUPLICATES FASTCOV_EXCLUDES)
+
+    # Conditional arguments
+    if(CPPFILT_PATH AND NOT ${Coverage_NO_DEMANGLE})
+        set(GENHTML_EXTRA_ARGS "--demangle-cpp")
+    endif()
+
+    # Set up commands which will be run to generate coverage data
+    set(FASTCOV_EXEC_TESTS_CMD ${Coverage_EXECUTABLE} ${Coverage_EXECUTABLE_ARGS})
+
+    set(FASTCOV_CAPTURE_CMD ${FASTCOV_PATH} ${Coverage_FASTCOV_ARGS} --gcov ${GCOV_PATH}
+        --search-directory ${BASEDIR}
+        --process-gcno
+        --output ${Coverage_NAME}.json
+        --exclude ${FASTCOV_EXCLUDES}
+        --exclude ${FASTCOV_EXCLUDES}
+    )
+    
+    set(FASTCOV_CONVERT_CMD ${FASTCOV_PATH}
+        -C ${Coverage_NAME}.json --lcov --output ${Coverage_NAME}.info
+    )
+
+    if(Coverage_SKIP_HTML)
+        set(FASTCOV_HTML_CMD ";")
+    else()
+        set(FASTCOV_HTML_CMD ${GENHTML_PATH} ${GENHTML_EXTRA_ARGS} ${Coverage_GENHTML_ARGS}
+            -o ${Coverage_NAME} ${Coverage_NAME}.info
+        )
+    endif()
+
+    set(FASTCOV_POST_CMD ";")
+    if(Coverage_POST_CMD)
+        set(FASTCOV_POST_CMD ${Coverage_POST_CMD})
+    endif()
+
+    if(CODE_COVERAGE_VERBOSE)
+        message(STATUS "Code coverage commands for target ${Coverage_NAME} (fastcov):")
+
+        message("   Running tests:")
+        string(REPLACE ";" " " FASTCOV_EXEC_TESTS_CMD_SPACED "${FASTCOV_EXEC_TESTS_CMD}")
+        message("     ${FASTCOV_EXEC_TESTS_CMD_SPACED}")
+
+        message("   Capturing fastcov counters and generating report:")
+        string(REPLACE ";" " " FASTCOV_CAPTURE_CMD_SPACED "${FASTCOV_CAPTURE_CMD}")
+        message("     ${FASTCOV_CAPTURE_CMD_SPACED}")
+
+        message("   Converting fastcov .json to lcov .info:")
+        string(REPLACE ";" " " FASTCOV_CONVERT_CMD_SPACED "${FASTCOV_CONVERT_CMD}")
+        message("     ${FASTCOV_CONVERT_CMD_SPACED}")
+
+        if(NOT Coverage_SKIP_HTML)
+            message("   Generating HTML report: ")
+            string(REPLACE ";" " " FASTCOV_HTML_CMD_SPACED "${FASTCOV_HTML_CMD}")
+            message("     ${FASTCOV_HTML_CMD_SPACED}")
+        endif()
+        if(Coverage_POST_CMD)
+            message("   Running post command: ")
+            string(REPLACE ";" " " FASTCOV_POST_CMD_SPACED "${FASTCOV_POST_CMD}")
+            message("     ${FASTCOV_POST_CMD_SPACED}")
+        endif()
+    endif()
+
+    # Setup target
+    add_custom_target(${Coverage_NAME}
+
+        # Cleanup fastcov
+        COMMAND ${FASTCOV_PATH} ${Coverage_FASTCOV_ARGS} --gcov ${GCOV_PATH}
+            --search-directory ${BASEDIR}
+            --zerocounters
+
+        COMMAND ${FASTCOV_EXEC_TESTS_CMD}
+        COMMAND ${FASTCOV_CAPTURE_CMD}
+        COMMAND ${FASTCOV_CONVERT_CMD}
+        COMMAND ${FASTCOV_HTML_CMD}
+        COMMAND ${FASTCOV_POST_CMD}
+
+        # Set output files as GENERATED (will be removed on 'make clean')
+        BYPRODUCTS
+             ${Coverage_NAME}.info
+             ${Coverage_NAME}.json
+             ${Coverage_NAME}/index.html  # report directory
+
+        WORKING_DIRECTORY ${PROJECT_BINARY_DIR}
+        DEPENDS ${Coverage_DEPENDENCIES}
+        VERBATIM # Protect arguments to commands
+        COMMENT "Resetting code coverage counters to zero. Processing code coverage counters and generating report."
+    )
+
+    set(INFO_MSG "fastcov code coverage info report saved in ${Coverage_NAME}.info and ${Coverage_NAME}.json.")
+    if(NOT Coverage_SKIP_HTML)
+        string(APPEND INFO_MSG " Open ${PROJECT_BINARY_DIR}/${Coverage_NAME}/index.html in your browser to view the coverage report.")
+    endif()
+    # Show where to find the fastcov info report
+    add_custom_command(TARGET ${Coverage_NAME} POST_BUILD
+        COMMAND ${CMAKE_COMMAND} -E echo ${INFO_MSG}
+    )
+
+endfunction() # setup_target_for_coverage_fastcov
+
+function(append_coverage_compiler_flags)
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${COVERAGE_COMPILER_FLAGS}" PARENT_SCOPE)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${COVERAGE_COMPILER_FLAGS}" PARENT_SCOPE)
+    set(CMAKE_Fortran_FLAGS "${CMAKE_Fortran_FLAGS} ${COVERAGE_COMPILER_FLAGS}" PARENT_SCOPE)
+    message(STATUS "Appending code coverage compiler flags: ${COVERAGE_COMPILER_FLAGS}")
+endfunction() # append_coverage_compiler_flags
+
+# Setup coverage for specific library
+function(append_coverage_compiler_flags_to_target name)
+    target_compile_options(${name}
+        PRIVATE ${COVERAGE_COMPILER_FLAGS})
+endfunction()
diff --git a/cmake/cmake-modules/GetGitRevisionDescription.cmake b/cmake/cmake-modules/GetGitRevisionDescription.cmake
new file mode 100644
index 00000000..1175e673
--- /dev/null
+++ b/cmake/cmake-modules/GetGitRevisionDescription.cmake
@@ -0,0 +1,141 @@
+# - Returns a version string from Git
+#
+# These functions force a re-configure on each git commit so that you can
+# trust the values of the variables in your build system.
+#
+#  get_git_head_revision(<refspecvar> <hashvar> [<additional arguments to git describe> ...])
+#
+# Returns the refspec and sha hash of the current head revision
+#
+#  git_describe(<var> [<additional arguments to git describe> ...])
+#
+# Returns the results of git describe on the source tree, and adjusting
+# the output so that it tests false if an error occurs.
+#
+#  git_get_exact_tag(<var> [<additional arguments to git describe> ...])
+#
+# Returns the results of git describe --exact-match on the source tree,
+# and adjusting the output so that it tests false if there was no exact
+# matching tag.
+#
+# Requires CMake 2.6 or newer (uses the 'function' command)
+#
+# Original Author:
+# 2009-2010 Ryan Pavlik <rpavlik@iastate.edu> <abiryan@ryand.net>
+# http://academic.cleardefinition.com
+# Iowa State University HCI Graduate Program/VRAC
+#
+# Copyright Iowa State University 2009-2010.
+# Distributed under the Boost Software License, Version 1.0.
+# (See accompanying file LICENSE_1_0.txt or copy at
+# http://www.boost.org/LICENSE_1_0.txt)
+
+if(__get_git_revision_description)
+    return()
+endif()
+set(__get_git_revision_description YES)
+
+# We must run the following at "include" time, not at function call time,
+# to find the path to this module rather than the path to a calling list file
+get_filename_component(_gitdescmoddir ${CMAKE_CURRENT_LIST_FILE} PATH)
+
+function(get_git_head_revision _refspecvar _hashvar)
+    set(GIT_PARENT_DIR "${CMAKE_CURRENT_SOURCE_DIR}")
+    set(GIT_DIR "${GIT_PARENT_DIR}/.git")
+    while(NOT EXISTS "${GIT_DIR}")    # .git dir not found, search parent directories
+        set(GIT_PREVIOUS_PARENT "${GIT_PARENT_DIR}")
+        get_filename_component(GIT_PARENT_DIR ${GIT_PARENT_DIR} PATH)
+        if(GIT_PARENT_DIR STREQUAL GIT_PREVIOUS_PARENT)
+            # We have reached the root directory, we are not in git
+            set(${_refspecvar} "GITDIR-NOTFOUND" PARENT_SCOPE)
+            set(${_hashvar} "GITDIR-NOTFOUND" PARENT_SCOPE)
+            return()
+        endif()
+        set(GIT_DIR "${GIT_PARENT_DIR}/.git")
+    endwhile()
+    # check if this is a submodule
+    if(NOT IS_DIRECTORY ${GIT_DIR})
+        file(READ ${GIT_DIR} submodule)
+        string(REGEX REPLACE "gitdir: (.*)\n$" "\\1" GIT_DIR_RELATIVE ${submodule})
+        get_filename_component(SUBMODULE_DIR ${GIT_DIR} PATH)
+
+        if (IS_ABSOLUTE ${GIT_DIR_RELATIVE})
+            set(GIT_DIR ${GIT_DIR_RELATIVE})
+        else()
+            get_filename_component(GIT_DIR ${SUBMODULE_DIR}/${GIT_DIR_RELATIVE} ABSOLUTE)
+        endif()
+
+    endif()
+    set(GIT_DATA "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/git-data")
+    if(NOT EXISTS "${GIT_DATA}")
+        file(MAKE_DIRECTORY "${GIT_DATA}")
+    endif()
+
+    if(NOT EXISTS "${GIT_DIR}/HEAD")
+        return()
+    endif()
+    set(HEAD_FILE "${GIT_DATA}/HEAD")
+    configure_file("${GIT_DIR}/HEAD" "${HEAD_FILE}" COPYONLY)
+
+    configure_file("${_gitdescmoddir}/GetGitRevisionDescription.cmake.in"
+        "${GIT_DATA}/grabRef.cmake"
+        @ONLY)
+    include("${GIT_DATA}/grabRef.cmake")
+
+    set(${_refspecvar} "${HEAD_REF}" PARENT_SCOPE)
+    set(${_hashvar} "${HEAD_HASH}" PARENT_SCOPE)
+endfunction()
+
+function(git_describe _var)
+    if(NOT GIT_FOUND)
+        find_package(Git QUIET)
+    endif()
+    get_git_head_revision(refspec hash)
+    if(NOT GIT_FOUND)
+        set(${_var} "GIT-NOTFOUND" PARENT_SCOPE)
+        return()
+    endif()
+    if(NOT hash)
+        set(${_var} "HEAD-HASH-NOTFOUND" PARENT_SCOPE)
+        return()
+    endif()
+
+    # TODO sanitize
+    #if((${ARGN}" MATCHES "&&") OR
+    #    (ARGN MATCHES "||") OR
+    #    (ARGN MATCHES "\\;"))
+    #    message("Please report the following error to the project!")
+    #    message(FATAL_ERROR "Looks like someone's doing something nefarious with git_describe! Passed arguments ${ARGN}")
+    #endif()
+
+    #message(STATUS "Arguments to execute_process: ${ARGN}")
+
+    execute_process(COMMAND
+        ${GIT_EXECUTABLE}
+        describe
+        ${hash}
+        ${ARGN}
+        WORKING_DIRECTORY
+        "${CMAKE_SOURCE_DIR}"
+        RESULT_VARIABLE
+        res
+        OUTPUT_VARIABLE
+        out
+        ERROR_QUIET
+        OUTPUT_STRIP_TRAILING_WHITESPACE)
+    if(NOT res EQUAL 0)
+        set(out "${out}-${res}-NOTFOUND")
+    endif()
+
+    set(${_var} "${out}" PARENT_SCOPE)
+endfunction()
+
+function(git_get_exact_tag _var)
+    git_describe(out --exact-match ${ARGN})
+    set(${_var} "${out}" PARENT_SCOPE)
+endfunction()
+
+function(git_get_tag _var)
+    git_describe(out --tags ${ARGN})
+    set(${_var} "${out}" PARENT_SCOPE)
+endfunction()
diff --git a/cmake/cmake-modules/GetGitRevisionDescription.cmake.in b/cmake/cmake-modules/GetGitRevisionDescription.cmake.in
new file mode 100644
index 00000000..ae19652b
--- /dev/null
+++ b/cmake/cmake-modules/GetGitRevisionDescription.cmake.in
@@ -0,0 +1,38 @@
+# 
+# Internal file for GetGitRevisionDescription.cmake
+#
+# Requires CMake 2.6 or newer (uses the 'function' command)
+#
+# Original Author:
+# 2009-2010 Ryan Pavlik <rpavlik@iastate.edu> <abiryan@ryand.net>
+# http://academic.cleardefinition.com
+# Iowa State University HCI Graduate Program/VRAC
+#
+# Copyright Iowa State University 2009-2010.
+# Distributed under the Boost Software License, Version 1.0.
+# (See accompanying file LICENSE_1_0.txt or copy at
+# http://www.boost.org/LICENSE_1_0.txt)
+
+set(HEAD_HASH)
+
+file(READ "@HEAD_FILE@" HEAD_CONTENTS LIMIT 1024)
+
+string(STRIP "${HEAD_CONTENTS}" HEAD_CONTENTS)
+if(HEAD_CONTENTS MATCHES "ref")
+    # named branch
+    string(REPLACE "ref: " "" HEAD_REF "${HEAD_CONTENTS}")
+    if(EXISTS "@GIT_DIR@/${HEAD_REF}")
+    configure_file("@GIT_DIR@/${HEAD_REF}" "@GIT_DATA@/head-ref" COPYONLY)
+    elseif(EXISTS "@GIT_DIR@/logs/${HEAD_REF}")
+        configure_file("@GIT_DIR@/logs/${HEAD_REF}" "@GIT_DATA@/head-ref" COPYONLY)
+        set(HEAD_HASH "${HEAD_REF}")
+    endif()
+else()
+    # detached HEAD
+    configure_file("@GIT_DIR@/HEAD" "@GIT_DATA@/head-ref" COPYONLY)
+endif()
+
+if(NOT HEAD_HASH)
+file(READ "@GIT_DATA@/head-ref" HEAD_HASH LIMIT 1024)
+string(STRIP "${HEAD_HASH}" HEAD_HASH)
+endif()
diff --git a/cmake/cmake-modules/LICENSE_1_0.txt b/cmake/cmake-modules/LICENSE_1_0.txt
new file mode 100644
index 00000000..36b7cd93
--- /dev/null
+++ b/cmake/cmake-modules/LICENSE_1_0.txt
@@ -0,0 +1,23 @@
+Boost Software License - Version 1.0 - August 17th, 2003
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
diff --git a/cmake/cmake-modules/README.md b/cmake/cmake-modules/README.md
new file mode 100644
index 00000000..4692eddb
--- /dev/null
+++ b/cmake/cmake-modules/README.md
@@ -0,0 +1,5 @@
+The files in these folder were manually copy and pasted from the
+[cmake-modules repository](https://github.com/bilke/cmake-modules). It was decided to do
+this because only a small subset of its provided functions are needed.
+
+The license file in included here as well.
diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
index 644b488d..a13ae791 100644
--- 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;
diff --git a/hal/src/fsfw_hal/linux/gpio/Gpio.h b/hal/src/fsfw_hal/linux/gpio/Gpio.h
new file mode 100644
index 00000000..40cc1df5
--- /dev/null
+++ 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_ */
diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp
index 3fedc9d4..31dbc903 100644
--- 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),
diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.h b/hal/src/fsfw_hal/linux/uart/UartCookie.h
index 6840b352..cae33d58 100644
--- 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();
 
diff --git a/src/fsfw/FSFWVersion.h.in b/src/fsfw/FSFWVersion.h.in
index 19a56214..3b93bee5 100644
--- a/src/fsfw/FSFWVersion.h.in
+++ b/src/fsfw/FSFWVersion.h.in
@@ -1,9 +1,11 @@
 #ifndef FSFW_VERSION_H_
 #define FSFW_VERSION_H_
 
-// Versioning is kept in project CMakeLists.txt file
-#define FSFW_VERSION_MAJOR @FSFW_VERSION@
-#define FSFW_VERSION_MINOR @FSFW_SUBVERSION@
-#define FSFW_VERSION_REVISION @FSFW_REVISION@
+// Versioning is managed in project CMakeLists.txt file
+static constexpr int FSFW_VERSION_MAJOR = @FSFW_VERSION@;
+static constexpr int FSFW_VERSION_MINOR = @FSFW_SUBVERSION@;
+static constexpr int FSFW_VERSION_REVISION = @FSFW_REVISION@;
+// Also contains CST (Commits since tag) information
+static const char FSFW_VERSION_CST_GIT_SHA1[] = "@FSFW_VERSION_CST_GIT_SHA1@";
 
 #endif /* FSFW_VERSION_H_ */
diff --git a/src/fsfw/action/CommandActionHelper.h b/src/fsfw/action/CommandActionHelper.h
index dd8ad7f1..3b8acb04 100644
--- 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);
diff --git a/src/fsfw/cfdp/CFDPHandler.cpp b/src/fsfw/cfdp/CFDPHandler.cpp
index 96baa98c..09a24186 100644
--- 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;
 }
 
diff --git a/src/fsfw/container/HybridIterator.h b/src/fsfw/container/HybridIterator.h
index e8b24a3d..189f8410 100644
--- 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;
     }
   }
 
diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp
index 953dacb4..0e4ff970 100644
--- 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); }
diff --git a/src/fsfw/datapool/CMakeLists.txt b/src/fsfw/datapool/CMakeLists.txt
index be4606aa..535be2b0 100644
--- a/src/fsfw/datapool/CMakeLists.txt
+++ b/src/fsfw/datapool/CMakeLists.txt
@@ -1,4 +1,4 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE
-	PoolDataSetBase.cpp
-	PoolEntry.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    PoolDataSetBase.cpp
+    PoolEntry.cpp
 )
\ No newline at end of file
diff --git a/src/fsfw/datapool/PoolEntry.cpp b/src/fsfw/datapool/PoolEntry.cpp
index fd110e6c..9138a705 100644
--- 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) {
-    std::memset(this->address, 0, this->getByteSize());
-  } else {
-    std::copy(initValue.begin(), initValue.end(), this->address);
+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());
+}
+
+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());
   }
 }
 
diff --git a/src/fsfw/datapool/PoolEntry.h b/src/fsfw/datapool/PoolEntry.h
index d3d80f09..4010f78d 100644
--- 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;
diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h
index 17cf8be2..9166cf34 100644
--- 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);
diff --git a/src/fsfw/devicehandlers/AssemblyBase.cpp b/src/fsfw/devicehandlers/AssemblyBase.cpp
index c29022e5..414098ad 100644
--- 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);
+  }
+}
diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h
index 3e235928..a6e6e3db 100644
--- 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_ */
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index dd9bd5d7..b0a5d1f0 100644
--- 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;
+}
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h
index 5e974831..d8c3363e 100644
--- 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;
diff --git a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp
index 48783c20..88fba19f 100644
--- 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());
diff --git a/src/fsfw/devicehandlers/DeviceHandlerIF.h b/src/fsfw/devicehandlers/DeviceHandlerIF.h
index 1fc63d3b..1ea742f7 100644
--- 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;
 
diff --git a/src/fsfw/devicehandlers/HealthDevice.cpp b/src/fsfw/devicehandlers/HealthDevice.cpp
index a626fa6c..5514b65f 100644
--- 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); }
diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp
index aaa7d6c5..824682da 100644
--- 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 */
diff --git a/src/fsfw/events/EventManager.h b/src/fsfw/events/EventManager.h
index f2d642ff..e2a8dd95 100644
--- 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;
diff --git a/src/fsfw/fdir/FailureIsolationBase.cpp b/src/fsfw/fdir/FailureIsolationBase.cpp
index fedef869..c17cf624 100644
--- 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() {
@@ -51,11 +52,12 @@ ReturnValue_t FailureIsolationBase::initialize() {
         ObjectManager::instance()->get<ConfirmsFailuresIF>(faultTreeParent);
     if (parentIF == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "FailureIsolationBase::intialize: Parent object"
-                 << "invalid." << std::endl;
-#endif
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "Make sure it implements ConfirmsFailuresIF." << std::endl;
+      sif::error << "FailureIsolationBase::intialize: Parent object "
+                 << "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;
diff --git a/src/fsfw/fdir/FailureIsolationBase.h b/src/fsfw/fdir/FailureIsolationBase.h
index 85d18add..7d128083 100644
--- 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);
diff --git a/src/fsfw/globalfunctions/CMakeLists.txt b/src/fsfw/globalfunctions/CMakeLists.txt
index 5ccd3c4c..acd1edbe 100644
--- a/src/fsfw/globalfunctions/CMakeLists.txt
+++ b/src/fsfw/globalfunctions/CMakeLists.txt
@@ -1,13 +1,13 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		arrayprinter.cpp
-		AsciiConverter.cpp
-		CRC.cpp
-		DleEncoder.cpp
-		PeriodicOperationDivider.cpp
-		timevalOperations.cpp
-		Type.cpp
-		bitutility.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE
+    arrayprinter.cpp
+    AsciiConverter.cpp
+    CRC.cpp
+    DleEncoder.cpp
+    DleParser.cpp
+    PeriodicOperationDivider.cpp
+    timevalOperations.cpp
+    Type.cpp
+    bitutility.cpp
 )
 
 add_subdirectory(math)
diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp
new file mode 100644
index 00000000..71da7e6a
--- /dev/null
+++ 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 %zu", 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();
+}
diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h
new file mode 100644
index 00000000..32fe38cb
--- /dev/null
+++ 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_ */
diff --git a/src/fsfw/globalfunctions/matching/MatchTree.h b/src/fsfw/globalfunctions/matching/MatchTree.h
index f7775d45..68b8f270 100644
--- 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()) {
diff --git a/src/fsfw/health/HasHealthIF.h b/src/fsfw/health/HasHealthIF.h
index 41abeef3..bdd1ce96 100644
--- 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;
diff --git a/src/fsfw/internalerror/InternalErrorReporter.cpp b/src/fsfw/internalerror/InternalErrorReporter.cpp
index fa16ec3f..a3f9645b 100644
--- 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
     }
   }
diff --git a/src/fsfw/modes/HasModesIF.h b/src/fsfw/modes/HasModesIF.h
index 0ebe77d8..57c4fb66 100644
--- 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;
diff --git a/src/fsfw/objectmanager/ObjectManager.cpp b/src/fsfw/objectmanager/ObjectManager.cpp
index 2017938a..39fef5b5 100644
--- 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++;
     }
diff --git a/src/fsfw/parameters/HasParametersIF.h b/src/fsfw/parameters/HasParametersIF.h
index 48557b4a..e95b69ab 100644
--- 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,
diff --git a/src/fsfw/parameters/ParameterWrapper.cpp b/src/fsfw/parameters/ParameterWrapper.cpp
index 27552290..e772f1b6 100644
--- 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;
diff --git a/src/fsfw/power/CMakeLists.txt b/src/fsfw/power/CMakeLists.txt
index 1c625db1..e195b1c0 100644
--- a/src/fsfw/power/CMakeLists.txt
+++ b/src/fsfw/power/CMakeLists.txt
@@ -1,7 +1,8 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		Fuse.cpp
-		PowerComponent.cpp
-		PowerSensor.cpp
-		PowerSwitcher.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE
+    Fuse.cpp
+    PowerComponent.cpp
+    PowerSensor.cpp
+    PowerSwitcher.cpp
+    DummyPowerSwitcher.cpp
+    PowerSwitcherComponent.cpp
 )
\ No newline at end of file
diff --git a/src/fsfw/power/DummyPowerSwitcher.cpp b/src/fsfw/power/DummyPowerSwitcher.cpp
new file mode 100644
index 00000000..d0a87288
--- /dev/null
+++ 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; }
diff --git a/src/fsfw/power/DummyPowerSwitcher.h b/src/fsfw/power/DummyPowerSwitcher.h
new file mode 100644
index 00000000..24e9d648
--- /dev/null
+++ 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_ */
diff --git a/src/fsfw/power/Fuse.h b/src/fsfw/power/Fuse.h
index e6f9c149..0b57a6a9 100644
--- 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,
diff --git a/src/fsfw/power/PowerSensor.cpp b/src/fsfw/power/PowerSensor.cpp
index 08ff4724..1936e8ee 100644
--- 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); }
diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h
index a31d8971..bc883fbc 100644
--- 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
diff --git a/src/fsfw/power/PowerSwitcher.cpp b/src/fsfw/power/PowerSwitcher.cpp
index 0a3f8521..7608c6e7 100644
--- 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;
diff --git a/src/fsfw/power/PowerSwitcher.h b/src/fsfw/power/PowerSwitcher.h
index c72c241d..853bbb40 100644
--- 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);
diff --git a/src/fsfw/power/PowerSwitcherComponent.cpp b/src/fsfw/power/PowerSwitcherComponent.cpp
new file mode 100644
index 00000000..9c1ed4cf
--- /dev/null
+++ 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(); }
diff --git a/src/fsfw/power/PowerSwitcherComponent.h b/src/fsfw/power/PowerSwitcherComponent.h
new file mode 100644
index 00000000..a3ed640e
--- /dev/null
+++ 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_ */
diff --git a/src/fsfw/power/definitions.h b/src/fsfw/power/definitions.h
new file mode 100644
index 00000000..ed2a0037
--- /dev/null
+++ 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_ */
diff --git a/src/fsfw/pus/Service1TelecommandVerification.cpp b/src/fsfw/pus/Service1TelecommandVerification.cpp
index 13d6a1c4..772137dd 100644
--- 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() {
diff --git a/src/fsfw/pus/Service3Housekeeping.cpp b/src/fsfw/pus/Service3Housekeeping.cpp
index 07574783..cce8fc91 100644
--- 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;
diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp
index 4517bc26..a2407945 100644
--- 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
diff --git a/src/fsfw/pus/Service5EventReporting.h b/src/fsfw/pus/Service5EventReporting.h
index 74264130..c1032d18 100644
--- 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();
 
   /***
diff --git a/src/fsfw/pus/Service9TimeManagement.h b/src/fsfw/pus/Service9TimeManagement.h
index 9369e207..1bea2f51 100644
--- 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;
 
diff --git a/src/fsfw/subsystem/Subsystem.cpp b/src/fsfw/subsystem/Subsystem.cpp
index 8930d0a1..1e860efc 100644
--- 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];
diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h
index 5b2777e8..a3ee5744 100644
--- a/src/fsfw/subsystem/Subsystem.h
+++ b/src/fsfw/subsystem/Subsystem.h
@@ -1,14 +1,13 @@
 #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"
 
 struct TableSequenceBase {
@@ -34,8 +33,12 @@ struct SequenceEntry : public TableSequenceBase {
 };
 
 /**
- * @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:
diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index 104db3c3..aec96434 100644
--- 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;
diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h
index 8cfd5be0..0853ed6e 100644
--- 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();
 
diff --git a/src/fsfw/tcdistribution/TcDistributor.cpp b/src/fsfw/tcdistribution/TcDistributor.cpp
index a650546c..408e736e 100644
--- 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); }
diff --git a/src/fsfw/thermal/AbstractTemperatureSensor.cpp b/src/fsfw/thermal/AbstractTemperatureSensor.cpp
index 68cd3aca..f7b31386 100644
--- 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() {
diff --git a/src/fsfw/thermal/AbstractTemperatureSensor.h b/src/fsfw/thermal/AbstractTemperatureSensor.h
index 0c6493fe..b790b0ca 100644
--- 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;
 
diff --git a/src/fsfw/thermal/Heater.cpp b/src/fsfw/thermal/Heater.cpp
index 4f0f8060..46f5c822 100644
--- 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); }
diff --git a/src/fsfw/tmstorage/TmStoreBackendIF.h b/src/fsfw/tmstorage/TmStoreBackendIF.h
index d9f1a17b..1e08342a 100644
--- 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() {}
 
diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h
index 10140db1..dc45576e 100644
--- 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.
diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.cpp b/src/fsfw/tmtcservices/CommandingServiceBase.cpp
index bbdf8d2a..1bbd7fd1 100644
--- 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) {
diff --git a/src/fsfw/tmtcservices/PusServiceBase.cpp b/src/fsfw/tmtcservices/PusServiceBase.cpp
index 3af2b82c..565ac5cd 100644
--- 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); }
diff --git a/src/fsfw/tmtcservices/SourceSequenceCounter.h b/src/fsfw/tmtcservices/SourceSequenceCounter.h
index e981a945..6f2778a4 100644
--- 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_ */
diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp
index 8ea67119..cc6ec599 100644
--- 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,15 +174,18 @@ 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!"
-                 << std::endl;
+      sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number "
+                      "of stored packet IDs reached!"
+                   << std::endl;
 #else
-    sif::printWarning(
-        "TmTcBridge::storeDownlinkData: TM downlink max. number "
-        "of stored packet IDs reached!\n");
+      sif::printWarning(
+          "TmTcBridge::storeDownlinkData: TM downlink max. number "
+          "of stored packet IDs reached!\n");
 #endif
+      warningSwitch = true;
+    }
     if (overwriteOld) {
       tmFifo->retrieve(&storeId);
       tmStore->deleteData(storeId);
diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h
index 237f1f3e..81d8e5d8 100644
--- 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;
diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp
index e4a62002..0b2ee60c 100644
--- a/src/fsfw/version.cpp
+++ b/src/fsfw/version.cpp
@@ -12,12 +12,25 @@
 #undef minor
 #endif
 
-const fsfw::Version fsfw::FSFW_VERSION = {FSFW_VERSION_MAJOR, FSFW_VERSION_MINOR,
-                                          FSFW_VERSION_REVISION};
+const Version fsfw::FSFW_VERSION = {FSFW_VERSION_MAJOR, FSFW_VERSION_MINOR, FSFW_VERSION_REVISION,
+                                    FSFW_VERSION_CST_GIT_SHA1};
 
-fsfw::Version::Version(uint32_t major, uint32_t minor, uint32_t revision)
-    : major(major), minor(minor), revision(revision) {}
+Version::Version(int major, int minor, int revision, const char* addInfo)
+    : major(major), minor(minor), revision(revision), addInfo(addInfo) {}
 
-void fsfw::Version::getVersion(char* str, size_t maxLen) const {
-  snprintf(str, maxLen, "%d.%d.%d", major, minor, revision);
+void Version::getVersion(char* str, size_t maxLen) const {
+  size_t len = snprintf(str, maxLen, "%d.%d.%d", major, minor, revision);
+  if (addInfo != nullptr) {
+    snprintf(str + len, maxLen - len, "-%s", addInfo);
+  }
 }
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+std::ostream& operator<<(std::ostream& os, const Version& v) {
+  os << v.major << "." << v.minor << "." << v.revision;
+  if (v.addInfo != nullptr) {
+    os << "-" << v.addInfo;
+  }
+  return os;
+}
+#endif
diff --git a/src/fsfw/version.h b/src/fsfw/version.h
index bb4d0399..f7d56d91 100644
--- a/src/fsfw/version.h
+++ b/src/fsfw/version.h
@@ -8,14 +8,15 @@
 #endif
 #include <cstdint>
 
-namespace fsfw {
-
 class Version {
  public:
-  Version(uint32_t major, uint32_t minor, uint32_t revision);
-  uint32_t major = 0;
-  uint32_t minor = 0;
-  uint32_t revision = 0;
+  Version(int major, int minor, int revision, const char* addInfo = nullptr);
+  int major = -1;
+  int minor = -1;
+  int revision = -1;
+
+  // Additional information, e.g. a git SHA hash
+  const char* addInfo = nullptr;
 
   friend bool operator==(const Version& v1, const Version& v2) {
     return (v1.major == v2.major and v1.minor == v2.minor and v1.revision == v2.revision);
@@ -43,10 +44,7 @@ class Version {
    * @param v
    * @return
    */
-  friend std::ostream& operator<<(std::ostream& os, const Version& v) {
-    os << v.major << "." << v.minor << "." << v.revision;
-    return os;
-  }
+  friend std::ostream& operator<<(std::ostream& os, const Version& v);
 #endif
 
   /**
@@ -57,7 +55,9 @@ class Version {
   void getVersion(char* str, size_t maxLen) const;
 };
 
-extern const fsfw::Version FSFW_VERSION;
+namespace fsfw {
+
+extern const Version FSFW_VERSION;
 
 }  // namespace fsfw
 
diff --git a/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp b/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp
index 3ad26876..fa4e0908 100644
--- 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);
diff --git a/tests/src/fsfw_tests/unit/version.cpp b/tests/src/fsfw_tests/unit/version.cpp
index 2967dfa5..a95c1ea9 100644
--- a/tests/src/fsfw_tests/unit/version.cpp
+++ b/tests/src/fsfw_tests/unit/version.cpp
@@ -10,12 +10,12 @@ TEST_CASE("Version API Tests", "[TestVersionAPI]") {
   // Check that major version is non-zero
   REQUIRE(fsfw::FSFW_VERSION.major > 0);
   uint32_t fsfwMajor = fsfw::FSFW_VERSION.major;
-  REQUIRE(fsfw::Version(255, 0, 0) > fsfw::FSFW_VERSION);
-  REQUIRE(fsfw::Version(255, 0, 0) >= fsfw::FSFW_VERSION);
-  REQUIRE(fsfw::Version(0, 0, 0) < fsfw::FSFW_VERSION);
-  REQUIRE(fsfw::Version(0, 0, 0) <= fsfw::FSFW_VERSION);
-  fsfw::Version v1 = fsfw::Version(1, 1, 1);
-  fsfw::Version v2 = fsfw::Version(1, 1, 1);
+  REQUIRE(Version(255, 0, 0) > fsfw::FSFW_VERSION);
+  REQUIRE(Version(255, 0, 0) >= fsfw::FSFW_VERSION);
+  REQUIRE(Version(0, 0, 0) < fsfw::FSFW_VERSION);
+  REQUIRE(Version(0, 0, 0) <= fsfw::FSFW_VERSION);
+  Version v1 = Version(1, 1, 1);
+  Version v2 = Version(1, 1, 1);
   REQUIRE(v1 == v2);
   REQUIRE(not(v1 < v2));
   REQUIRE(not(v1 > v2));