possiible leak fixes

Reviewed-on: fsfw/fsfw#715

CHANGELOG.md

@@ -12,6 +12,26 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Fixes
 
+- `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated
+  on mode announcements, duplicate mode reply generated on announce commands, and the mode read
+  subservice not working properly.
+- `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval
+  seconds instead of uptime.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/726
+- HAL MGM3100 Handler: Use axis specific gain/scaling factors. Previously,
+  only the X scaling factor was used.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/724
+- DHB `setNormalDatapoolEntriesInvalid`: The default implementation did not set the validity
+  to false correctly because the `read` and `write` calls were missing.
+- PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead
+  of the correct unsegmented flags (0b11) as specified in the standard.
+- TC Scheduler Service 11: Add size and CRC check for contained TC.
+- Only delete health table entry in `HealthHelper` destructor if
+  health table was set.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/710/files
+- I2C Bugfixes: Do not keep iterator as member and fix some incorrect handling with the iterator.
+  Also properly reset the reply size for successfull transfers and erroneous transfers.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/700
 - Bugfix for Serial Buffer Stream: Setting `doActive` to false now
   actually fully disables printing.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/680
@@ -21,19 +41,31 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Added
 
-- DHB TM handler `handleDeviceTM` renamed to `handleDeviceTm` and now takes
-  `util::DataWrapper` as the data input argument. This allows more flexibility in the possible
-  types of telemetry.
-  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/669
-- Add `util::DataWrapper` class inside the `util` module. This is a tagged union which allows
-  to specify raw data either as a classic C-style raw pointer and size or as a `SerializeIF`
-  pointer.
-  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/668
+- `CServiceHealthCommanding`: Add announce all health info implementation
+  PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122
+- `Service9TimeManagement`: Add `DUMP_TIME` (129) subservice.
+- `TcpTmTcServer`: Allow setting the `SO_REUSEADDR` and `SO_REUSEPORT`
+  option on the TCP server. CTOR prototype has changed and expects an explicit
+  TCP configuration struct to be passed.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/722
+- `DleParser` helper class to parse DLE encoded packets from a byte stream.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/711
+- `UioMapper` is able to resolve symlinks now.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/709
 - Add new `UnsignedByteField` class
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/660
 
 ## Changes
 
+- `CService201HealthCommanding` renamed to `CServiceHealthCommanding`,
+  service ID customizable now. `CServiceHealthCommanding` expects configuration struct
+  `HealthServiceCfg` now
+  PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122
+- `AcceptsTelemetryIF`: `getReportReceptionQueue` is const now
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/712
+- Moved some container returnvalues to dedicated header and namespace
+  to they can be used without template specification.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/707
 - Remove default secondary header argument for
   `uint16_t getTcSpacePacketIdFromApid(uint16_t apid, bool secondaryHeaderFlag)` and
   `uint16_t getTmSpacePacketIdFromApid(uint16_t apid, bool secondaryHeaderFlag)`
@@ -62,6 +94,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 - `DeviceHandlerBase`: New signature of `handleDeviceTm` which expects
   a `const SerializeIF&` and additional helper variant which expects `const uint8_t*`
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/671
+- Move some generic `StorageManagerIF` implementations from `LocalPool` to
+  interface itself so it can be re-used more easily. Also add new
+  abstract function `bool hasDataAtId(store_address_t storeId) const`.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/685
 - Improvements for `AcceptsTelemetryIF` and `AcceptsTelecommandsIF`:
   - Make functions `const` where it makes sense
   - Add `const char* getName const` abstract function
@@ -89,6 +125,16 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
      implementation without an extra component
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/682
 
+## HAL
+
+- SPI: Cache the SPI device in the communication interface. Architecturally, this makes a
+  lot more sense because each ComIF should be responsible for one SPI bus.
+- SPI: Move the empty transfer to update the line polarity to separate function. This means
+  it is not automatically called when calling the setter function for SPI speed and mode.
+  The user should call this function after locking the CS mutex if multiple SPI devices with
+  differing speeds and modes are attached to one bus.
+- SPI: Getter functions for SPI speed and mode.
+
 # [v5.0.0] 25.07.2022
 
 ## Changes
@@ -222,6 +268,7 @@ https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593
     - https://gitlab.kitware.com/cmake/cmake/-/issues/21696
   Easiest solution for now: Keep this option OFF by default.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/616
+- Linux HAL: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1
 - Dedicated Version class and constant `fsfw::FSFW_VERSION` containing version information
   inside `fsfw/version.h`
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/559
@@ -236,17 +283,6 @@ https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/590
 - `Subsystem`: New API to add table and sequence entries
 
-## HAL
-
-- SPI: Cache the SPI device in the communication interface. Architecturally, this makes a
-  lot more sense because each ComIF should be responsible for one SPI bus.
-- SPI: Move the empty transfer to update the line polarity to separate function. This means
-  it is not automatically called when calling the setter function for SPI speed and mode.
-  The user should call this function after locking the CS mutex if multiple SPI devices with
-  differing speeds and modes are attached to one bus.
-- SPI: Getter functions for SPI speed and mode.
-- I2C: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1.
-
 ## Fixed
 
 - TCP TMTC Server: `MutexGuard` was not created properly in

CMakeLists.txt

@@ -122,6 +122,7 @@ if(UNIX)
   option(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS "Add Linux peripheral drivers"
          OFF)
   option(FSFW_HAL_LINUX_ADD_LIBGPIOD "Attempt to add Linux GPIOD drivers" OFF)
+  option(FSFW_HAL_LINUX_ADD_SERIAL_DRIVERS "Add serial drivers" ON)
 endif()
 
 # Optional sources
@@ -326,7 +327,8 @@ if(FSFW_BUILD_TESTS)
             "/usr/local/include/*"
             "*/fsfw_tests/*"
             "*/catch2-src/*"
-            "*/fsfw_hal/*")
+            "*/fsfw_hal/*"
+            "unittests/*")
       endif()
 
       target_link_options(${FSFW_TEST_TGT} PRIVATE -fprofile-arcs
@@ -344,8 +346,15 @@ if(FSFW_BUILD_TESTS)
           DEPENDENCIES ${FSFW_TEST_TGT})
       else()
         setup_target_for_coverage_lcov(
-          NAME ${FSFW_TEST_TGT}_coverage EXECUTABLE ${FSFW_TEST_TGT}
-          DEPENDENCIES ${FSFW_TEST_TGT})
+          NAME
+          ${FSFW_TEST_TGT}_coverage
+          EXECUTABLE
+          ${FSFW_TEST_TGT}
+          DEPENDENCIES
+          ${FSFW_TEST_TGT}
+          GENHTML_ARGS
+          --html-epilog
+          ${CMAKE_SOURCE_DIR}/unittests/lcov_epilog.html)
       endif()
     endif()
   endif()

automation/Dockerfile

@@ -5,7 +5,7 @@ RUN apt-get --yes upgrade
 
 #tzdata is a dependency, won't install otherwise
 ARG DEBIAN_FRONTEND=noninteractive
-RUN apt-get --yes install gcc g++ cmake make lcov git valgrind nano iputils-ping python3 pip doxygen graphviz
+RUN apt-get --yes install gcc g++ cmake make lcov git valgrind nano iputils-ping python3 pip doxygen graphviz rsync
 
 RUN python3 -m pip install sphinx breathe
 
@@ -23,3 +23,7 @@ RUN git clone https://github.com/ETLCPP/etl.git && \
 
 #ssh needs a valid user to work
 RUN adduser --uid 114 jenkins
+
+#add documentation server to known hosts
+RUN echo "|1|/LzCV4BuTmTb2wKnD146l9fTKgQ=|NJJtVjvWbtRt8OYqFgcYRnMQyVw= ecdsa-sha2-nistp256 AAAAE2VjZHNhLXNoYTItbmlzdHAyNTYAAAAIbmlzdHAyNTYAAABBBNL8ssTonYtgiR/6RRlSIK9WU1ywOcJmxFTLcEblAwH7oifZzmYq3XRfwXrgfMpylEfMFYfCU8JRqtmi19xc21A=" >> /etc/ssh/ssh_known_hosts
+RUN echo "|1|CcBvBc3EG03G+XM5rqRHs6gK/Gg=|oGeJQ+1I8NGI2THIkJsW92DpTzs= ecdsa-sha2-nistp256 AAAAE2VjZHNhLXNoYTItbmlzdHAyNTYAAAAIbmlzdHAyNTYAAABBBNL8ssTonYtgiR/6RRlSIK9WU1ywOcJmxFTLcEblAwH7oifZzmYq3XRfwXrgfMpylEfMFYfCU8JRqtmi19xc21A=" >> /etc/ssh/ssh_known_hosts

automation/Jenkinsfile

@@ -1,48 +1,90 @@
 pipeline {
     environment {
-        BUILDDIR = 'cmake-build-tests'
+        BUILDDIR_HOST = 'cmake-build-tests-host'
+        BUILDDIR_LINUX = 'cmake-build-tests-linux'
         DOCDDIR = 'cmake-build-documentation'
     }
     agent {
         docker { 
-            image 'fsfw-ci:d5'
-            args '--network host'
+            image 'fsfw-ci:d6'
+            args '--network host --sysctl fs.mqueue.msg_max=100'
         }
     }
+    stages {
+        stage('Host') {
             stages{
                 stage('Clean') {
                     steps {
-                sh 'rm -rf $BUILDDIR'
+                        sh 'rm -rf $BUILDDIR_HOST'
                     }
                 }
                 stage('Configure') {
                     steps {
-                dir(BUILDDIR) {
+                        dir(BUILDDIR_HOST) {
                             sh 'cmake -DFSFW_OSAL=host -DFSFW_BUILD_TESTS=ON -DFSFW_CICD_BUILD=ON ..'
                         }
                     }
                 }
                 stage('Build') {
                     steps {
-                dir(BUILDDIR) {
+                        dir(BUILDDIR_HOST) {
                             sh 'cmake --build . -j4'
                         }
                     }
                 }
                 stage('Unittests') {
                     steps {
-                dir(BUILDDIR) {
+                        dir(BUILDDIR_HOST) {
                             sh 'cmake --build . -- fsfw-tests_coverage -j4'
                         }
                     }
                 }
                 stage('Valgrind') {
                     steps {
-                dir(BUILDDIR) {
+                        dir(BUILDDIR_HOST) {
                             sh 'valgrind --leak-check=full --error-exitcode=1 ./fsfw-tests'
                         }
                     }
                 }
+            }
+        }
+        stage('Linux') {
+            stages{
+                stage('Clean') {
+                    steps {
+                        sh 'rm -rf $BUILDDIR_LINUX'
+                    }
+                }
+                stage('Configure') {
+                    steps {
+                        dir(BUILDDIR_LINUX) {
+                            sh 'cmake -DFSFW_OSAL=linux -DFSFW_BUILD_TESTS=ON -DFSFW_CICD_BUILD=ON ..'
+                        }
+                    }
+                }
+                stage('Build') {
+                    steps {
+                        dir(BUILDDIR_LINUX) {
+                            sh 'cmake --build . -j4'
+                        }
+                    }
+                }
+                stage('Unittests') {
+                    steps {
+                        dir(BUILDDIR_LINUX) {
+                            sh 'cmake --build . -- fsfw-tests_coverage -j4'
+                        }
+                    }
+                }
+                stage('Valgrind') {
+                    steps {
+                        dir(BUILDDIR_LINUX) {
+                            sh 'valgrind --leak-check=full --error-exitcode=1 ./fsfw-tests'
+                        }
+                    }
+                }
+            }
+        }
         stage('Documentation') {
             when {
                 branch 'development'
@@ -52,14 +94,12 @@ pipeline {
                     sh 'cmake -DFSFW_BUILD_DOCS=ON -DFSFW_OSAL=host ..'
                     sh 'make Sphinx'
                     sshagent(credentials: ['documentation-buildfix']) {
-                        sh 'ssh -o StrictHostKeyChecking=no buildfix@documentation.intra.irs.uni-stuttgart.de rm -rf /mnt/data/www/html/fsfw/development/*'
-                        sh 'scp -o StrictHostKeyChecking=no -r docs/sphinx/* buildfix@documentation.intra.irs.uni-stuttgart.de:/mnt/data/www/html/fsfw/development'
+                        sh 'rsync -r --delete docs/sphinx/* buildfix@documentation.irs.uni-stuttgart.de:/fsfw/development'
                     }
                 }
                 dir(BUILDDIR) {
                     sshagent(credentials: ['documentation-buildfix']) {
-                        sh 'ssh -o StrictHostKeyChecking=no buildfix@documentation.intra.irs.uni-stuttgart.de rm -rf /mnt/data/www/html/fsfw/coverage/development/*'
-                        sh 'scp -o StrictHostKeyChecking=no -r fsfw-tests_coverage/* buildfix@documentation.intra.irs.uni-stuttgart.de:/mnt/data/www/html/fsfw/coverage/development'
+                        sh 'rsync -r --delete fsfw-tests_coverage/* buildfix@documentation.irs.uni-stuttgart.de:/fsfw/coverage/development'
                     }
                 }
             }
@@ -73,14 +113,12 @@ pipeline {
                     sh 'cmake -DFSFW_BUILD_DOCS=ON -DFSFW_OSAL=host ..'
                     sh 'make Sphinx'
                     sshagent(credentials: ['documentation-buildfix']) {
-                        sh 'ssh -o StrictHostKeyChecking=no buildfix@documentation.intra.irs.uni-stuttgart.de rm -rf /mnt/data/www/html/fsfw/master/*'
-                        sh 'scp -o StrictHostKeyChecking=no -r docs/sphinx/* buildfix@documentation.intra.irs.uni-stuttgart.de:/mnt/data/www/html/fsfw/master'
+                        sh 'rsync -r --delete docs/sphinx/* buildfix@documentation.irs.uni-stuttgart.de:/fsfw/master'
                     }
                 }
                 dir(BUILDDIR) {
                     sshagent(credentials: ['documentation-buildfix']) {
-                        sh 'ssh -o StrictHostKeyChecking=no buildfix@documentation.intra.irs.uni-stuttgart.de rm -rf /mnt/data/www/html/fsfw/coverage/master/*'
-                        sh 'scp -o StrictHostKeyChecking=no -r fsfw-tests_coverage/* buildfix@documentation.intra.irs.uni-stuttgart.de:/mnt/data/www/html/fsfw/coverage/master'
+                        sh 'rsync -r --delete fsfw-tests_coverage/* buildfix@documentation.irs.uni-stuttgart.de:/fsfw/coverage/master'
                     }
                 }
             }

cmake/cmake-modules/bilke/CodeCoverage.cmake

@@ -140,7 +140,7 @@ 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( GCOVR_PATH gcovr )
 find_program( CPPFILT_PATH NAMES c++filt )
 
 if(NOT GCOV_PATH)

docs/README-config.md

@@ -1,40 +0,0 @@
-Configuring the FSFW
-======
-
-The FSFW can be configured via the `fsfwconfig` folder. A template folder has
-been provided to have a starting point for this. The folder should be added
-to the include path. The primary configuration file is the `FSFWConfig.h` folder. Some
-of the available options will be explained in more detail here.
-
-# Auto-Translation of Events
-
-The FSFW allows the automatic translation of events, which allows developers to track triggered
-events directly via console output. Using this feature requires:
-
-1. `FSFW_OBJ_EVENT_TRANSLATION` set to 1 in the configuration file.
-2. Special auto-generated translation files which translate event IDs and object IDs into
-   human readable strings. These files can be generated using the 
-   [modgen Python scripts](https://git.ksat-stuttgart.de/source/modgen.git).
-3. The generated translation files for the object IDs should be named `translatesObjects.cpp`
-   and `translateObjects.h` and should be copied to the `fsfwconfig/objects` folder
-4. The generated translation files for the event IDs should be named `translateEvents.cpp` and
-   `translateEvents.h` and should be copied to the `fsfwconfig/events` folder
-   
-An example implementations of these translation file generators can be found as part
-of the [SOURCE project here](https://git.ksat-stuttgart.de/source/sourceobsw/-/tree/development/generators)
-or the [FSFW example](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example_public/src/branch/master/generators)
-
-## Configuring the Event Manager
-
-The number of allowed subscriptions can be modified with the following
-parameters:
-
-``` c++
-namespace fsfwconfig {
-//! Configure the allocated pool sizes for the event manager.
-static constexpr size_t FSFW_EVENTMGMR_MATCHTREE_NODES = 240;
-static constexpr size_t FSFW_EVENTMGMT_EVENTIDMATCHERS = 120;
-static constexpr size_t FSFW_EVENTMGMR_RANGEMATCHERS   = 120;
-}
-```
-

docs/README-controllers.md

@@ -1 +0,0 @@
-## Controllers

docs/README-core.md

@@ -1,55 +0,0 @@
-## FSFW Core Modules
-
-These core modules provide the most important functionalities of the 
-Flight Software Framework
-
-### Clock
-
- * This is a class of static functions that can be used at anytime
- * Leap Seconds must be set if any time conversions from UTC to other times is used
-
-### ObjectManager
-
-* Must be created during program startup
-* The component which handles all references. All SystemObjects register at this component. 
-* Any SystemObject needs to have a unique ObjectId. Those can be managed like objects::framework_objects.
-* A reference to an object can be get by calling the following function. T must be the specific Interface you want to call.
-A nullptr check of the returning Pointer must be done. This function is based on Run-time type information. 
-
-```cpp
-template <typename T> T* ObjectManagerIF::get( object_id_t id )
-```
-* A typical way to create all objects on startup is a handing a static produce function to the
-  ObjectManager on creation. By calling objectManager->initialize() the produce function will be
-  called and all SystemObjects will be initialized afterwards.
-
-### Event Manager
-
-* Component which allows routing of events
-* Other objects can subscribe to specific events, ranges of events or all events of an object.
-* Subscriptions can be done during runtime but should be done during initialization
-* Amounts of allowed subscriptions can be configured in `FSFWConfig.h`
-
-### Health Table
-
-* A component which holds every health state 
-* Provides a thread safe way to access all health states without the need of message exchanges
-
-### Stores
-
-* The message based communication can only exchange a few bytes of information inside the message
-  itself. Therefore, additional information can  be exchanged with Stores. With this, only the
-  store address must be exchanged in the message.
-* Internally, the FSFW uses an IPC Store to exchange data between processes. For incoming TCs a TC
-  Store is used. For outgoing TM a TM store is used.
-* All of them should use the Thread Safe Class storagemanager/PoolManager
-
-### Tasks
-
-There are two different types of tasks:
- * The PeriodicTask just executes objects that are of type ExecutableObjectIF in the order of the
-   insertion to the Tasks.
- * FixedTimeslotTask executes a list of calls in the order of the given list. This is intended for
-   DeviceHandlers, where polling should be in a defined order. An example can be found in 
-   `defaultcfg/fsfwconfig/pollingSequence` folder
-

docs/README-devicehandlers.md

@@ -1 +0,0 @@
-## Device Handlers

docs/README-highlevel.md

@@ -1,135 +0,0 @@
-High-level overview
-======
-
-# Structure
-
-The general structure is driven by the usage of interfaces provided by objects. 
-The FSFW uses C++11 as baseline. The intention behind this is that this C++ Standard should be
-widely available, even with older compilers.
-The FSFW uses dynamic allocation during the initialization but provides static containers during runtime. 
-This simplifies the instantiation of objects and allows the usage of some standard containers. 
-Dynamic Allocation after initialization is discouraged and different solutions are provided in the
-FSFW to achieve that. The fsfw uses run-time type information but exceptions are not allowed.
-
-# Failure Handling
-
-Functions should return a defined `ReturnValue_t` to signal to the caller that something has
-gone wrong. Returnvalues must be unique. For this the function `returnvalue::makeCode`
-or the macro `MAKE_RETURN` can be used. The `CLASS_ID` is a unique id for that type of object.
-See `returnvalues/FwClassIds` folder. The user can add custom `CLASS_ID`s via the
-`fsfwconfig` folder.
-
-# OSAL
-
-The FSFW provides operation system abstraction layers for Linux, FreeRTOS and RTEMS. 
-The OSAL provides periodic tasks, message queues, clocks and semaphores as well as mutexes.
-The [OSAL README](doc/README-osal.md#top) provides more detailed information on provided components
-and how to use them.
-
-# Core Components 
-
-The FSFW has following core components. More detailed informations can be found in the
-[core component section](doc/README-core.md#top):
-
-1. Tasks: Abstraction for different (periodic) task types like periodic tasks or tasks 
-   with fixed timeslots
-2. ObjectManager: This module stores all `SystemObjects` by mapping a provided unique object ID
-   to the object handles.
-3. Static Stores: Different stores are provided to store data of variable size (like telecommands 
-   or small telemetry) in a pool structure without using dynamic memory allocation.
-   These pools are allocated up front.
-3. Clock: This module provided common time related functions
-4. EventManager: This module allows routing of events generated by `SystemObjects`
-5. HealthTable: A component which stores the health states of objects
-
-# Static IDs in the framework
-
-Some parts of the framework use a static routing address for communication. 
-An example setup of ids can be found in the example config in `defaultcft/fsfwconfig/objects`
- inside the function `Factory::setStaticFrameworkObjectIds()`.
-
-# Events
-
-Events are tied to objects. EventIds can be generated by calling the Macro MAKE_EVENT. 
-This works analog to the returnvalues. Every object that needs own EventIds has to get a
-unique SUBSYSTEM_ID. Every SystemObject can call triggerEvent from the parent class.
-Therefore, event messages contain the specific EventId and the objectId of the object that
-has triggered.
-
-# Internal Communication
-
-Components communicate mostly via Messages through Queues. 
-Those queues are created by calling the singleton `QueueFactory::instance()->create()` which
-will create `MessageQueue` instances for the used OSAL.
-
-# External Communication
-
-The external communication with the mission control system is mostly up to the user implementation.
-The FSFW provides PUS Services which can be used to but don't need to be used. 
-The services can be seen as a conversion from a TC to a message based communication and back.
-
-## TMTC Communication
-
-The FSFW provides some components to facilitate TMTC handling via the PUS commands.
-For example, a UDP or TCP PUS server socket can be opened on a specific port using the
-files located in `osal/common`. The FSFW example uses this functionality to allow sending telecommands
-and receiving telemetry using the [TMTC commander application](https://github.com/spacefisch/tmtccmd).
-Simple commands like the PUS Service 17 ping service can be tested by simply running the
-`tmtc_client_cli.py` or `tmtc_client_gui.py` utility in 
-the [example tmtc folder](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example_public/src/branch/master/tmtc)
-while the `fsfw_example` application is running.
-
-More generally, any class responsible for handling incoming telecommands and sending telemetry
-can implement the generic `TmTcBridge` class located in `tmtcservices`. Many applications
-also use a dedicated polling task for reading telecommands which passes telecommands
-to the `TmTcBridge` implementation.
-
-## CCSDS Frames, CCSDS Space Packets and PUS
-
-If the communication is based on CCSDS Frames and Space Packets, several classes can be used to
-distributed the packets to the corresponding services. Those can be found in `tcdistribution`. 
-If Space Packets are used, a timestamper has to be provided by the user. 
-An example can be found in the `timemanager` folder, which uses `CCSDSTime::CDS_short`.
-
-# Device Handlers
-
-DeviceHandlers are another important component of the FSFW. 
-The idea is, to have a software counterpart of every physical device to provide a simple mode,
-health and commanding interface. By separating the underlying Communication Interface with 
-`DeviceCommunicationIF`, a device handler (DH) can be tested on different hardware.
-The DH has mechanisms to monitor the communication with the physical device which allow
-for FDIR reaction.  Device Handlers can be created by implementing `DeviceHandlerBase`. 
-A standard FDIR component for the DH will be created automatically but can
-be overwritten by the user. More information on DeviceHandlers can be found in the
-related [documentation section](doc/README-devicehandlers.md#top).
-
-# Modes and Health
-
-The two interfaces `HasModesIF` and `HasHealthIF` provide access for commanding and monitoring
-of components. On-board Mode Management is implement in hierarchy system. 
-DeviceHandlers and Controllers are the lowest part of the hierarchy. 
-The next layer are Assemblies. Those assemblies act as a component which handle
-redundancies of handlers. Assemblies share a common core with the next level which
-are the Subsystems. 
-
-Those Assemblies are intended to act as auto-generated components from a database which describes
-the subsystem modes. The definitions contain transition and target tables which contain the DH,
-Assembly and Controller Modes to be commanded.
-Transition tables contain as many steps as needed to reach the mode from any other mode, e.g. a
-switch into any higher AOCS mode might first turn on the sensors, than the actuators and the
-controller as last component. 
-The target table is used to describe the state that is checked continuously by the subsystem. 
-All of this allows System Modes to be generated as Subsystem object as well from the same database. 
-This System contains list of subsystem modes in the transition and target tables. 
-Therefore, it allows a modular system to create system modes and easy commanding of those, because
-only the highest components must be commanded.
-
-The health state represents if the component is able to perform its tasks. 
-This can be used to signal the system to avoid using this component instead of a redundant one.
-The on-board FDIR uses the health state for isolation and recovery. 
-
-# Unit Tests
-
-Unit Tests are provided in the unittest folder. Those use the catch2 framework but do not include
-catch2 itself. More information on how to run these tests can be found in the separate
-[`fsfw_tests` reposoitory](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_tests)

docs/README-localpools.md

@@ -1,174 +0,0 @@
-## Local Data Pools Developer Information
-
-The following text is targeted towards mission software developers which would like
-to use the local data pools provided by the FSFW to store data like sensor values so they can be 
-used by other software objects like controllers as well. If a custom class should have a local 
-pool which  can be used by other software objects as well, following steps have to be performed:
-
-1. Create a `LocalDataPoolManager` member object in the custom class
-2. Implement the `HasLocalDataPoolIF` with specifies the interface between the local pool manager
-and the class owning the local pool.
-
-The local data pool manager is also able to process housekeeping service requests in form
-of messages, generate periodic housekeeping packet, generate notification and snapshots of changed
-variables and datasets and process notifications and snapshots coming from other objects.
-The two former tasks are related to the external interface using telemetry and telecommands (TMTC)
-while the later two are related to data consumers like controllers only acting on data change 
-detected by the data creator instead of checking the data manually each cycle. Two important 
-framework classes `DeviceHandlerBase` and `ExtendedControllerBase` already perform the two steps 
-shown above so the steps required are altered slightly.
-
-### Storing and Accessing pool data 
-
-The pool manager is responsible for thread-safe access of the pool data, but the actual
-access to the pool data from the point of view of a mission software developer happens via proxy
-classes like pool variable classes. These classes store a copy
-of the pool variable with the matching datatype and copy the actual data from the local pool
-on a `read` call. Changed variables can then be written to the local pool with a `commit` call.
-The `read` and `commit` calls are thread-safe and can be called concurrently from data creators
-and data consumers. Generally, a user will create a dataset class which in turn groups all 
-cohesive pool variables. These sets simply iterator over the list of variables and call the 
-`read` and `commit` functions of each variable. The following diagram shows the
-high-level architecture of the local data pools.
-
-.. image:: ../misc/logo/FSFW_Logo_V3_bw.png
-   :alt: FSFW Logo
-
-
-An example is shown for using the local data pools with a Gyroscope.
-For example, the following code shows an implementation to access data from a Gyroscope taken
-from the SOURCE CubeSat project:
-
-```cpp
-class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
-public:
-    /**
-     * Constructor for data users
-     * @param gyroId
-     */
-    GyroPrimaryDataset(object_id_t gyroId):
-            StaticLocalDataSet(sid_t(gyroId, gyrodefs::GYRO_DATA_SET_ID)) {
-        setAllVariablesReadOnly();
-    }
-
-    lp_var_t<float> angVelocityX = lp_var_t<float>(sid.objectId,
-            gyrodefs::ANGULAR_VELOCITY_X, this);
-    lp_var_t<float> angVelocityY = lp_var_t<float>(sid.objectId,
-            gyrodefs::ANGULAR_VELOCITY_Y, this);
-    lp_var_t<float> angVelocityZ = lp_var_t<float>(sid.objectId,
-            gyrodefs::ANGULAR_VELOCITY_Z, this);
-private:
-
-    friend class GyroHandler;
-    /**
-     * Constructor for data creator
-     * @param hkOwner
-     */
-    GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
-            StaticLocalDataSet(hkOwner, gyrodefs::GYRO_DATA_SET_ID) {}
-};
-```
-
-There is a public constructor for users which sets all variables to read-only and there is a
-constructor for the GyroHandler data creator by marking it private and declaring the `GyroHandler`
-as a friend class. Both the atittude controller and the `GyroHandler` can now
-use the same class definition to access the pool variables with `read` and `commit` semantics
-in a thread-safe way. Generally, each class requiring access will have the set class as a member
-class. The data creator will also be generally a `DeviceHandlerBase` subclass and some additional
-steps are necessary to expose the set for housekeeping purposes.
-
-### Using the local data pools in a `DeviceHandlerBase` subclass
-
-It is very common to store data generated by devices like a sensor into a pool which can 
-then be used by other objects. Therefore, the `DeviceHandlerBase` already has a
-local pool. Using the aforementioned example, our `GyroHandler` will now have the set class
-as a member:
-
-```cpp
-class GyroHandler: ... {
-
-public:
-	...
-private:
-	...
-	GyroPrimaryDataset gyroData;
-	...
-};
-```
-
-The constructor used for the creators expects the owner class as a parameter, so we initialize
-the object in the `GyroHandler` constructor like this:
-
-```cpp
-GyroHandler::GyroHandler(object_id_t objectId, object_id_t comIF,
-        CookieIF *comCookie, uint8_t switchId):
-        DeviceHandlerBase(objectId, comIF, comCookie), switchId(switchId),
-		gyroData(this) {}
-```
-
-We need to assign the set to a reply ID used in the `DeviceHandlerBase`. 
-The combination of the `GyroHandler` object ID and the reply ID will be the 64-bit structure ID
-`sid_t` and is used to globally identify the set, for example when requesting housekeeping data or
-generating update messages. We need to assign our custom set class in some way so that the local 
-pool manager can access the custom data sets as well.
-By default, the `getDataSetHandle` will take care of this tasks. The default implementation for a
-`DeviceHandlerBase` subclass will use the internal command map to retrieve 
-a handle to a dataset from a given reply ID. Therefore,
-we assign the set in the `fillCommandAndReplyMap` function:
-
-```cpp
-void GyroHandler::fillCommandAndReplyMap() {
-	...
-	this->insertInCommandAndReplyMap(gyrodefs::GYRO_DATA, 3, &gyroData);
-	...
-}
-```
-
-Now, we need to create the actual pool entries as well, using the `initializeLocalDataPool` 
-function. Here, we also immediately subscribe for periodic housekeeping packets 
-with an interval of 4 seconds. They are still disabled in this example and can be enabled 
-with a housekeeping service command.
-
-```cpp
-ReturnValue_t GyroHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
-		LocalDataPoolManager &poolManager) {
-	localDataPoolMap.emplace(gyrodefs::ANGULAR_VELOCITY_X,
-			new PoolEntry<float>({0.0}));
-	localDataPoolMap.emplace(gyrodefs::ANGULAR_VELOCITY_Y,
-			new PoolEntry<float>({0.0}));
-	localDataPoolMap.emplace(gyrodefs::ANGULAR_VELOCITY_Z,
-			new PoolEntry<float>({0.0}));
-	localDataPoolMap.emplace(gyrodefs::GENERAL_CONFIG_REG42,
-			new PoolEntry<uint8_t>({0}));
-	localDataPoolMap.emplace(gyrodefs::RANGE_CONFIG_REG43,
-			new PoolEntry<uint8_t>({0}));
-
-	poolManager.subscribeForPeriodicPacket(gyroData.getSid(), false, 4.0, false);
-	return returnvalue::OK;
-}
-```
-
-Now, if we receive some sensor data and converted them into the right format, 
-we can write it into the pool like this, using a guard class to ensure the set is commited back 
-in any case:
-
-```cpp
-PoolReadGuard readHelper(&gyroData);
-if(readHelper.getReadResult() == returnvalue::OK) {
-	if(not gyroData.isValid()) {
-		gyroData.setValidity(true, true);
-	}
-
-	gyroData.angVelocityX = angularVelocityX;
-	gyroData.angVelocityY = angularVelocityY;
-	gyroData.angVelocityZ = angularVelocityZ;
-}
-```
-
-The guard class will commit the changed data on destruction automatically.
-
-### Using the local data pools in a `ExtendedControllerBase` subclass
-
-Coming soon
-
-

docs/README-osal.md

@@ -1,32 +0,0 @@
-# Operating System Abstraction Layer (OSAL)
-
-Some specific information on the provided OSALs are provided.
-
-## Linux OSAL
-
-This OSAL can be used to compile for Linux host systems like Ubuntu 20.04 or for 
-embedded Linux targets like the Raspberry Pi. This OSAL generally requires threading support
-and real-time functionalities. For most UNIX systems, this is done by adding `-lrt` and `-lpthread` to the linked libraries in the compilation process. The CMake build support provided will do this automatically for the `fsfw` target. It should be noted that most UNIX systems need to be configured specifically to allow the real-time functionalities required by the FSFW.
-
-More information on how to set up a Linux system accordingly can be found in the 
-[Linux README of the FSFW example application](https://egit.irs.uni-stuttgart.de/fsfw/fsfw_example/src/branch/master/doc/README-linux.md#top)
-
-## Hosted OSAL
-
-This is the newest OSAL. Support for Semaphores has not been implemented yet and will propably be implemented as soon as C++20 with Semaphore support has matured. This OSAL can be used to run the FSFW on any host system, but currently has only been tested on Windows 10 and Ubuntu 20.04. Unlike the other OSALs, it uses dynamic memory allocation (e.g. for the message queue implementation). Cross-platform serial port (USB) support might be added soon.
-
-## FreeRTOS OSAL
-
-FreeRTOS is not included and the developer needs to take care of compiling the FreeRTOS sources and adding the `FreeRTOSConfig.h` file location to the include path. This OSAL has only been tested extensively with the pre-emptive scheduler configuration so far but it should in principle also be possible to use a cooperative scheduler. It is recommended to use the `heap_4` allocation scheme. When using newlib (nano), it is also recommended to add `#define configUSE_NEWLIB_REENTRANT` to the FreeRTOS configuration file to ensure thread-safety.
-
-When using this OSAL, developers also need to provide an implementation for the `vRequestContextSwitchFromISR` function. This has been done because the call to request a context switch from an ISR is generally located in the `portmacro.h` header and is different depending on the target architecture or device.
-
-## RTEMS OSAL
-
-The RTEMS OSAL was the first implemented OSAL which is also used on the active satellite Flying Laptop.
-
-## TCP/IP socket abstraction
-
-The Linux and Host OSAL provide abstraction layers for the socket API. Currently, only UDP sockets have been imlemented. This is very useful to test TMTC handling either on the host computer directly (targeting localhost with a TMTC application) or on embedded Linux devices, sending TMTC packets via Ethernet.
-
-

docs/README-pus.md

@@ -1 +0,0 @@
-## PUS Services

docs/conf.py

@@ -50,6 +50,14 @@ exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"]
 #
 html_theme = "alabaster"
 
+html_theme_options = {
+    "extra_nav_links": {
+        "Impressum": "https://www.uni-stuttgart.de/impressum",
+        "Datenschutz": "https://info.irs.uni-stuttgart.de/datenschutz/datenschutzWebmit.html",
+    }
+}
+
+
 # Add any paths that contain custom static files (such as style sheets) here,
 # relative to this directory. They are copied after the builtin static files,
 # so a file named "default.css" will overwrite the builtin "default.css".

docs/highlevel.rst

@@ -7,14 +7,13 @@ Structure
 ----------
 
 The general structure is driven by the usage of interfaces provided by objects. 
-The FSFW uses C++17 as baseline.
-It also uses dynamic allocation during the initialization but provides
-static containers during runtime.
-This simplifies the instantiation of objects and allows the usage of some
-standard containers.
-Dynamic Allocation after initialization is discouraged and different solutions
-are provided in the FSFW to achieve that. The fsfw uses run-time type
-information but will not throw exceptions.
+The FSFW uses C++17 as baseline. Most modern compilers like GCC should have support for this
+standard, even for micocontrollers.
+
+The FSFW might use dynamic allocation during program initialization but not during runtime.
+It offers pool objects, static containers and it also exposes the
+`Embedded Template Library <https://www.etlcpp.com/>`_ to allow writing code which does not perform
+allocation during runtime. The fsfw uses run-time type information but will not throw exceptions.
 
 Failure Handling
 -----------------

scripts/helper.py

@@ -207,7 +207,7 @@ def check_for_cmake_build_dir(build_dir_list: list) -> list:
 def perform_lcov_operation(directory: str, chdir: bool):
     if chdir:
         os.chdir(directory)
-    cmd_runner("cmake --build . -- fsfw-tests_coverage -j")
+    cmd_runner("cmake --build . -j -- fsfw-tests_coverage")
 
 
 def determine_build_dir(build_dir_list: List[str]):

src/fsfw/cfdp/handler/DestHandler.cpp

@@ -425,21 +425,31 @@ ReturnValue_t cfdp::DestHandler::sendFinishedPdu() {
   store_address_t storeId;
   uint8_t* dataPtr = nullptr;
   ReturnValue_t result =
-      fp.tcStore->getFreeElement(&storeId, finishedPdu.getSerializedSize(), &dataPtr);
+      fp.tmStore->getFreeElement(&storeId, finishedPdu.getSerializedSize(), &dataPtr);
   if (result != OK) {
-    // TODO: Error handling and event, this is a non CFDP specific error (most likely store is full)
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << "cfdp::DestHandler:sendFinishedPdu: Getting store slot failed" << std::endl;
+#endif
+    fp.eventReporter->forwardEvent(events::STORE_ERROR, result, 0);
     return result;
   }
   size_t serLen = 0;
   result = finishedPdu.serialize(dataPtr, serLen, finishedPdu.getSerializedSize());
   if (result != OK) {
-    // TODO: Error printout, this really should not happen
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << "cfdp::DestHandler::sendFinishedPdu: Serializing Finished PDU failed"
+                 << std::endl;
+#endif
+    fp.eventReporter->forwardEvent(events::SERIALIZATION_ERROR, result, 0);
     return result;
   }
   TmTcMessage msg(storeId);
   result = fp.msgQueue->sendMessage(fp.packetDest.getReportReceptionQueue(), &msg);
   if (result != OK) {
-    // TODO: Error handling and event, this is a non CFDP specific error (most likely store is full)
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << "cfdp::DestHandler::sendFinishedPdu: Sending PDU failed" << std::endl;
+#endif
+    fp.eventReporter->forwardEvent(events::MSG_QUEUE_ERROR, result, 0);
     return result;
   }
   fsmRes.packetsSent++;

src/fsfw/cfdp/handler/defs.h

@@ -5,5 +5,15 @@ namespace cfdp {
 
 enum class CfdpStates { IDLE, BUSY_CLASS_1_NACKED, BUSY_CLASS_2_ACKED, SUSPENDED };
 
-}
+static constexpr uint8_t SSID = SUBSYSTEM_ID::CFDP;
+
+namespace events {
+
+static constexpr Event STORE_ERROR = event::makeEvent(SSID, 0, severity::LOW);
+static constexpr Event MSG_QUEUE_ERROR = event::makeEvent(SSID, 1, severity::LOW);
+static constexpr Event SERIALIZATION_ERROR = event::makeEvent(SSID, 2, severity::LOW);
+
+}  // namespace events
+
+}  // namespace cfdp
 #endif  // FSFW_CFDP_HANDLER_DEFS_H

src/fsfw/cfdp/pdu/FinishedPduCreator.cpp

@@ -17,7 +17,7 @@ ReturnValue_t FinishPduCreator::serialize(uint8_t **buffer, size_t *size, size_t
   if (result != returnvalue::OK) {
     return result;
   }
-  if (*size + 1 >= maxSize) {
+  if (*size + 1 > maxSize) {
     return SerializeIF::BUFFER_TOO_SHORT;
   }
   **buffer = finishInfo.getConditionCode() << 4 | finishInfo.getDeliveryCode() << 2 |

src/fsfw/container/ArrayList.h

@@ -4,6 +4,7 @@
 #include "../returnvalues/returnvalue.h"
 #include "../serialize/SerializeAdapter.h"
 #include "../serialize/SerializeIF.h"
+#include "definitions.h"
 
 /**
  * @brief     A List that stores its values in an array.
@@ -19,9 +20,6 @@ class ArrayList {
   friend class SerialArrayListAdapter;
 
  public:
-  static const uint8_t INTERFACE_ID = CLASS_ID::ARRAY_LIST;
-  static const ReturnValue_t FULL = MAKE_RETURN_CODE(0x01);
-
   /**
    * This is the allocating constructor.
    * It allocates an array of the specified size.
@@ -187,7 +185,7 @@ class ArrayList {
    */
   ReturnValue_t insert(T entry) {
     if (size >= maxSize_) {
-      return FULL;
+      return containers::LIST_FULL;
     }
     entries[size] = entry;
     ++size;

src/fsfw/container/FixedArrayList.h

@@ -12,6 +12,7 @@ template <typename T, size_t MAX_SIZE, typename count_t = uint8_t>
 class FixedArrayList : public ArrayList<T, count_t> {
   static_assert(MAX_SIZE <= std::numeric_limits<count_t>::max(),
                 "count_t is not large enough to hold MAX_SIZE");
+  static_assert(MAX_SIZE > 0, "MAX_SIZE is 0");
 
  private:
   T data[MAX_SIZE];
@@ -20,15 +21,19 @@ class FixedArrayList : public ArrayList<T, count_t> {
   FixedArrayList() : ArrayList<T, count_t>(data, MAX_SIZE) {}
 
   FixedArrayList(const FixedArrayList& other) : ArrayList<T, count_t>(data, MAX_SIZE) {
-    memcpy(this->data, other.data, sizeof(this->data));
     this->entries = data;
     this->size = other.size;
+    for (size_t idx = 0; idx < this->size; idx++) {
+      data[idx] = other.data[idx];
+    }
   }
 
   FixedArrayList& operator=(FixedArrayList other) {
-    memcpy(this->data, other.data, sizeof(this->data));
     this->entries = data;
     this->size = other.size;
+    for (size_t idx = 0; idx < this->size; idx++) {
+      data[idx] = other.data[idx];
+    }
     return *this;
   }
 

src/fsfw/container/FixedMap.h

@@ -4,8 +4,8 @@
 #include <type_traits>
 #include <utility>
 
-#include "../returnvalues/returnvalue.h"
 #include "ArrayList.h"
+#include "definitions.h"
 
 /**
  * @brief    Map implementation for maps with a pre-defined size.
@@ -24,11 +24,6 @@ class FixedMap : public SerializeIF {
                 "derived class from SerializeIF to be serialize-able");
 
  public:
-  static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP;
-  static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01);
-  static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02);
-  static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03);
-
  private:
   static const key_t EMPTY_SLOT = -1;
   ArrayList<std::pair<key_t, T>, uint32_t> theMap;
@@ -76,10 +71,10 @@ class FixedMap : public SerializeIF {
 
   ReturnValue_t insert(key_t key, T value, Iterator* storedValue = nullptr) {
     if (exists(key) == returnvalue::OK) {
-      return KEY_ALREADY_EXISTS;
+      return containers::KEY_ALREADY_EXISTS;
     }
     if (_size == theMap.maxSize()) {
-      return MAP_FULL;
+      return containers::MAP_FULL;
     }
     theMap[_size].first = key;
     theMap[_size].second = value;
@@ -93,7 +88,7 @@ class FixedMap : public SerializeIF {
   ReturnValue_t insert(std::pair<key_t, T> pair) { return insert(pair.first, pair.second); }
 
   ReturnValue_t exists(key_t key) const {
-    ReturnValue_t result = KEY_DOES_NOT_EXIST;
+    ReturnValue_t result = containers::KEY_DOES_NOT_EXIST;
     if (findIndex(key) < _size) {
       result = returnvalue::OK;
     }
@@ -103,7 +98,7 @@ class FixedMap : public SerializeIF {
   ReturnValue_t erase(Iterator* iter) {
     uint32_t i;
     if ((i = findIndex((*iter).value->first)) >= _size) {
-      return KEY_DOES_NOT_EXIST;
+      return containers::KEY_DOES_NOT_EXIST;
     }
     theMap[i] = theMap[_size - 1];
     --_size;
@@ -114,7 +109,7 @@ class FixedMap : public SerializeIF {
   ReturnValue_t erase(key_t key) {
     uint32_t i;
     if ((i = findIndex(key)) >= _size) {
-      return KEY_DOES_NOT_EXIST;
+      return containers::KEY_DOES_NOT_EXIST;
     }
     theMap[i] = theMap[_size - 1];
     --_size;

src/fsfw/container/definitions.h

@@ -0,0 +1,14 @@
+#ifndef FSFW_CONTAINER_DEFINITIONS_H_
+#define FSFW_CONTAINER_DEFINITIONS_H_
+
+#include "fsfw/retval.h"
+
+namespace containers {
+static const ReturnValue_t KEY_ALREADY_EXISTS = returnvalue::makeCode(CLASS_ID::FIXED_MAP, 0x01);
+static const ReturnValue_t MAP_FULL = returnvalue::makeCode(CLASS_ID::FIXED_MAP, 0x02);
+static const ReturnValue_t KEY_DOES_NOT_EXIST = returnvalue::makeCode(CLASS_ID::FIXED_MAP, 0x03);
+
+static const ReturnValue_t LIST_FULL = returnvalue::makeCode(CLASS_ID::ARRAY_LIST, 0x01);
+}  // namespace containers
+
+#endif /* FSFW_CONTAINER_DEFINITIONS_H_ */

src/fsfw/controller/ControllerBase.cpp

@@ -4,11 +4,10 @@
 #include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/subsystem/SubsystemBase.h"
+#include "fsfw/subsystem/helper.h"
 
-ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
-                               size_t commandQueueDepth)
+ControllerBase::ControllerBase(object_id_t setObjectId, size_t commandQueueDepth)
     : SystemObject(setObjectId),
-      parentId(parentId),
       mode(MODE_OFF),
       submode(SUBMODE_NONE),
       modeHelper(this),
@@ -21,33 +20,15 @@ ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
 ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
 
 ReturnValue_t ControllerBase::initialize() {
-  ReturnValue_t result = SystemObject::initialize();
+  ReturnValue_t result = modeHelper.initialize();
   if (result != returnvalue::OK) {
     return result;
   }
-
-  MessageQueueId_t parentQueue = 0;
-  if (parentId != objects::NO_OBJECT) {
-    auto* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
-    if (parent == nullptr) {
-      return returnvalue::FAILED;
-    }
-    parentQueue = parent->getCommandQueue();
-
-    parent->registerChild(getObjectId());
-  }
-
-  result = healthHelper.initialize(parentQueue);
+  result = healthHelper.initialize();
   if (result != returnvalue::OK) {
     return result;
   }
-
-  result = modeHelper.initialize(parentQueue);
-  if (result != returnvalue::OK) {
-    return result;
-  }
-
-  return returnvalue::OK;
+  return SystemObject::initialize();
 }
 
 MessageQueueId_t ControllerBase::getCommandQueue() const { return commandQueue->getId(); }
@@ -120,3 +101,13 @@ void ControllerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
 void ControllerBase::changeHK(Mode_t mode_, Submode_t submode_, bool enable) {}
 
 ReturnValue_t ControllerBase::initializeAfterTaskCreation() { return returnvalue::OK; }
+
+const HasHealthIF* ControllerBase::getOptHealthIF() const { return this; }
+
+const HasModesIF& ControllerBase::getModeIF() const { return *this; }
+
+ModeTreeChildIF& ControllerBase::getModeTreeChildIF() { return *this; }
+
+ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
+}

src/fsfw/controller/ControllerBase.h

@@ -6,6 +6,9 @@
 #include "fsfw/modes/HasModesIF.h"
 #include "fsfw/modes/ModeHelper.h"
 #include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
+#include "fsfw/subsystem/ModeTreeChildIF.h"
+#include "fsfw/subsystem/ModeTreeConnectionIF.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw/tasks/PeriodicTaskIF.h"
 
@@ -18,13 +21,18 @@
 class ControllerBase : public HasModesIF,
                        public HasHealthIF,
                        public ExecutableObjectIF,
+                       public ModeTreeChildIF,
+                       public ModeTreeConnectionIF,
                        public SystemObject {
  public:
   static const Mode_t MODE_NORMAL = 2;
 
-  ControllerBase(object_id_t setObjectId, object_id_t parentId, size_t commandQueueDepth = 3);
+  ControllerBase(object_id_t setObjectId, size_t commandQueueDepth = 3);
   ~ControllerBase() override;
 
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
+  ModeTreeChildIF &getModeTreeChildIF() override;
+
   /** SystemObject override */
   ReturnValue_t initialize() override;
 
@@ -38,6 +46,8 @@ class ControllerBase : public HasModesIF,
   ReturnValue_t performOperation(uint8_t opCode) override;
   void setTaskIF(PeriodicTaskIF *task) override;
   ReturnValue_t initializeAfterTaskCreation() override;
+  const HasHealthIF *getOptHealthIF() const override;
+  const HasModesIF &getModeIF() const override;
 
  protected:
   /**
@@ -56,8 +66,6 @@ class ControllerBase : public HasModesIF,
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
                                  uint32_t *msToReachTheMode) override = 0;
 
-  const object_id_t parentId;
-
   Mode_t mode;
 
   Submode_t submode;

src/fsfw/controller/ExtendedControllerBase.cpp

@@ -1,8 +1,7 @@
 #include "fsfw/controller/ExtendedControllerBase.h"
 
-ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, object_id_t parentId,
-                                               size_t commandQueueDepth)
-    : ControllerBase(objectId, parentId, commandQueueDepth),
+ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, size_t commandQueueDepth)
+    : ControllerBase(objectId, commandQueueDepth),
       poolManager(this, commandQueue),
       actionHelper(this, commandQueue) {}
 

src/fsfw/controller/ExtendedControllerBase.h

@@ -17,7 +17,7 @@ class ExtendedControllerBase : public ControllerBase,
                                public HasActionsIF,
                                public HasLocalDataPoolIF {
  public:
-  ExtendedControllerBase(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth = 3);
+  ExtendedControllerBase(object_id_t objectId, size_t commandQueueDepth = 3);
   ~ExtendedControllerBase() override;
 
   /* SystemObjectIF overrides */

src/fsfw/devicehandlers/AssemblyBase.cpp

@@ -1,7 +1,7 @@
 #include "fsfw/devicehandlers/AssemblyBase.h"
 
-AssemblyBase::AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth)
-    : SubsystemBase(objectId, parentId, MODE_OFF, commandQueueDepth),
+AssemblyBase::AssemblyBase(object_id_t objectId, uint16_t commandQueueDepth)
+    : SubsystemBase(objectId, MODE_OFF, commandQueueDepth),
       internalState(STATE_NONE),
       recoveryState(RECOVERY_IDLE),
       recoveringDevice(childrenMap.end()),

src/fsfw/devicehandlers/AssemblyBase.h

@@ -41,7 +41,7 @@ class AssemblyBase : public SubsystemBase {
   static const ReturnValue_t NEED_TO_CHANGE_HEALTH = MAKE_RETURN_CODE(0x05);
   static const ReturnValue_t NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE = MAKE_RETURN_CODE(0xa1);
 
-  AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth = 8);
+  AssemblyBase(object_id_t objectId, uint16_t commandQueueDepth = 8);
   virtual ~AssemblyBase();
 
  protected:

src/fsfw/devicehandlers/ChildHandlerBase.cpp

@@ -3,17 +3,12 @@
 #include "fsfw/subsystem/SubsystemBase.h"
 
 ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication,
-                                   CookieIF* cookie, object_id_t hkDestination,
-                                   uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId,
-                                   object_id_t parent, FailureIsolationBase* customFdir,
-                                   size_t cmdQueueSize)
+                                   CookieIF* cookie, HasModeTreeChildrenIF& parent,
+                                   FailureIsolationBase* customFdir, size_t cmdQueueSize)
     : DeviceHandlerBase(setObjectId, deviceCommunication, cookie,
                         (customFdir == nullptr ? &childHandlerFdir : customFdir), cmdQueueSize),
-      parentId(parent),
-      childHandlerFdir(setObjectId) {
-  this->setHkDestination(hkDestination);
-  this->setThermalStateRequestPoolIds(thermalStatePoolId, thermalRequestPoolId);
-}
+      parent(parent),
+      childHandlerFdir(setObjectId) {}
 
 ChildHandlerBase::~ChildHandlerBase() {}
 
@@ -23,21 +18,5 @@ ReturnValue_t ChildHandlerBase::initialize() {
     return result;
   }
 
-  MessageQueueId_t parentQueue = 0;
-
-  if (parentId != objects::NO_OBJECT) {
-    SubsystemBase* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
-    if (parent == NULL) {
-      return returnvalue::FAILED;
-    }
-    parentQueue = parent->getCommandQueue();
-
-    parent->registerChild(getObjectId());
-  }
-
-  healthHelper.setParentQueue(parentQueue);
-
-  modeHelper.setParentQueue(parentQueue);
-
-  return returnvalue::OK;
+  return DeviceHandlerBase::connectModeTreeParent(parent);
 }

src/fsfw/devicehandlers/ChildHandlerBase.h

@@ -1,22 +1,23 @@
 #ifndef FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_
 #define FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_
 
+#include <fsfw/subsystem/HasModeTreeChildrenIF.h>
+
 #include "ChildHandlerFDIR.h"
 #include "DeviceHandlerBase.h"
 
 class ChildHandlerBase : public DeviceHandlerBase {
  public:
   ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF* cookie,
-                   object_id_t hkDestination, uint32_t thermalStatePoolId,
-                   uint32_t thermalRequestPoolId, object_id_t parent = objects::NO_OBJECT,
-                   FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20);
+                   HasModeTreeChildrenIF& parent, FailureIsolationBase* customFdir = nullptr,
+                   size_t cmdQueueSize = 20);
 
   virtual ~ChildHandlerBase();
 
   virtual ReturnValue_t initialize();
 
  protected:
-  const uint32_t parentId;
+  HasModeTreeChildrenIF& parent;
   ChildHandlerFDIR childHandlerFdir;
 };
 

src/fsfw/devicehandlers/DeviceHandlerBase.cpp

@@ -1,5 +1,6 @@
-#include "fsfw/devicehandlers/DeviceHandlerBase.h"
+#include "DeviceHandlerBase.h"
 
+#include "fsfw/datapool/PoolReadGuard.h"
 #include "fsfw/datapoollocal/LocalPoolVariable.h"
 #include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h"
 #include "fsfw/devicehandlers/DeviceTmReportingWrapper.h"
@@ -12,6 +13,7 @@
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/storagemanager/StorageManagerIF.h"
 #include "fsfw/subsystem/SubsystemBase.h"
+#include "fsfw/subsystem/helper.h"
 #include "fsfw/thermal/ThermalComponentIF.h"
 
 object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT;
@@ -22,8 +24,6 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device
                                      CookieIF* comCookie, FailureIsolationBase* fdirInstance,
                                      size_t cmdQueueSize)
     : SystemObject(setObjectId),
-      mode(MODE_OFF),
-      submode(SUBMODE_NONE),
       wiretappingMode(OFF),
       storedRawData(StorageManagerIF::INVALID_ADDRESS),
       deviceCommunicationId(deviceCommunication),
@@ -38,6 +38,8 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device
       defaultFDIRUsed(fdirInstance == nullptr),
       switchOffWasReported(false),
       childTransitionDelay(5000),
+      mode(MODE_OFF),
+      submode(SUBMODE_NONE),
       transitionSourceMode(_MODE_POWER_DOWN),
       transitionSourceSubMode(SUBMODE_NONE) {
   auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
@@ -132,14 +134,6 @@ ReturnValue_t DeviceHandlerBase::initialize() {
         new DeviceHandlerFailureIsolation(this->getObjectId(), defaultFdirParentId);
   }
 
-  if (this->parent != objects::NO_OBJECT) {
-    HasModesIF* modeIF = ObjectManager::instance()->get<HasModesIF>(this->parent);
-    HasHealthIF* healthIF = ObjectManager::instance()->get<HasHealthIF>(this->parent);
-    if (modeIF != nullptr and healthIF != nullptr) {
-      setParentQueue(modeIF->getCommandQueue());
-    }
-  }
-
   communicationInterface =
       ObjectManager::instance()->get<DeviceCommunicationIF>(deviceCommunicationId);
   if (communicationInterface == nullptr) {
@@ -362,13 +356,14 @@ void DeviceHandlerBase::doStateMachine() {
           currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) {
         triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0);
         setMode(_MODE_POWER_DOWN);
-        callChildStatemachine();
         break;
       }
       ReturnValue_t switchState = getStateOfSwitches();
       if ((switchState == PowerSwitchIF::SWITCH_ON) || (switchState == NO_SWITCH)) {
         // NOTE: TransitionSourceMode and -SubMode are set by handleCommandedModeTransition
         childTransitionFailure = CHILD_TIMEOUT;
+        transitionSourceMode = _MODE_SHUT_DOWN;
+        transitionSourceSubMode = SUBMODE_NONE;
         setMode(_MODE_START_UP);
         callChildStatemachine();
       }
@@ -467,7 +462,7 @@ ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceComm
   info.expectedReplies = 0;
   info.isExecuting = false;
   info.sendReplyTo = NO_COMMANDER;
-  info.useAlternativeReplyId = alternativeReplyId;
+  info.useAlternativeReplyId = useAlternativeReply;
   info.alternativeReplyId = alternativeReplyId;
   auto resultPair = deviceCommandMap.emplace(deviceCommand, info);
   if (resultPair.second) {
@@ -574,8 +569,15 @@ void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) {
 }
 
 void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) {
-  /* TODO: This will probably be done by the LocalDataPoolManager now */
-  // changeHK(mode, submode, false);
+  /**
+   * handle transition from OFF to NORMAL by continuing towards normal when ON is reached
+   */
+  if (newMode == MODE_ON and continueToNormal) {
+    continueToNormal = false;
+    mode = _MODE_TO_NORMAL;
+    return;
+  }
+
   submode = newSubmode;
   mode = newMode;
   modeChanged();
@@ -584,6 +586,8 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) {
     disableCommandsAndReplies();
   }
   if (!isTransitionalMode()) {
+    // clear this flag when a non-transitional Mode is reached to be safe
+    continueToNormal = false;
     modeHelper.modeChanged(newMode, newSubmode);
     announceMode(false);
   }
@@ -1068,8 +1072,7 @@ Mode_t DeviceHandlerBase::getBaseMode(Mode_t transitionMode) {
   return transitionMode & ~(TRANSITION_MODE_BASE_ACTION_MASK | TRANSITION_MODE_CHILD_ACTION_MASK);
 }
 
-// SHOULDDO: Allow transition from OFF to NORMAL to reduce complexity in assemblies. And, by the
-// way, throw away DHB and write a new one:
+// SHOULDDO: throw away DHB and write a new one:
 //  - Include power and thermal completely, but more modular :-)
 //  - Don't use modes for state transitions, reduce FSM (Finte State Machine) complexity.
 //  - Modularization?
@@ -1081,11 +1084,10 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, Submode_
   if ((mode == MODE_ERROR_ON) && (commandedMode != MODE_OFF)) {
     return TRANS_NOT_ALLOWED;
   }
-  if ((commandedMode == MODE_NORMAL) && (mode == MODE_OFF)) {
-    return TRANS_NOT_ALLOWED;
-  }
 
-  if ((commandedMode == MODE_ON) && (mode == MODE_OFF) and (thermalSet != nullptr)) {
+  // Do not check thermal state for MODE_RAW
+  if ((mode == MODE_OFF) and ((commandedMode == MODE_ON) or (commandedMode == MODE_NORMAL)) and
+      (thermalSet != nullptr)) {
     ReturnValue_t result = thermalSet->read();
     if (result == returnvalue::OK) {
       if ((thermalSet->heaterRequest.value != ThermalComponentIF::STATE_REQUEST_IGNORE) and
@@ -1100,6 +1102,7 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, Submode_
 }
 
 void DeviceHandlerBase::startTransition(Mode_t commandedMode, Submode_t commandedSubmode) {
+  continueToNormal = false;
   switch (commandedMode) {
     case MODE_ON:
       handleTransitionToOnMode(commandedMode, commandedSubmode);
@@ -1129,8 +1132,9 @@ void DeviceHandlerBase::startTransition(Mode_t commandedMode, Submode_t commande
     case MODE_NORMAL:
       if (mode != MODE_OFF) {
         setTransition(MODE_NORMAL, commandedSubmode);
-      } else {
-        replyReturnvalueToCommand(HasModesIF::TRANS_NOT_ALLOWED);
+      } else {  // mode is off
+        continueToNormal = true;
+        handleTransitionToOnMode(MODE_NORMAL, commandedSubmode);
       }
       break;
   }
@@ -1521,10 +1525,13 @@ DeviceCommandId_t DeviceHandlerBase::getPendingCommand() const {
 void DeviceHandlerBase::setNormalDatapoolEntriesInvalid() {
   for (const auto& reply : deviceReplyMap) {
     if (reply.second.dataSet != nullptr) {
+      PoolReadGuard pg(reply.second.dataSet);
+      if (pg.getReadResult() == returnvalue::OK) {
         reply.second.dataSet->setValidity(false, true);
       }
     }
   }
+}
 
 void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType, const char* functionName,
                                             ReturnValue_t errorCode, const char* errorPrint) {
@@ -1578,12 +1585,14 @@ MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyI
 
 void DeviceHandlerBase::setCustomFdir(FailureIsolationBase* fdir) { this->fdirInstance = fdir; }
 
-void DeviceHandlerBase::setParent(object_id_t parent) { this->parent = parent; }
-
 void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) {
   this->powerSwitcher = switcher;
 }
 
+Mode_t DeviceHandlerBase::getMode() { return mode; }
+
+Submode_t DeviceHandlerBase::getSubmode() { return submode; }
+
 void DeviceHandlerBase::disableCommandsAndReplies() {
   for (auto& command : deviceCommandMap) {
     if (command.second.isExecuting) {
@@ -1601,3 +1610,24 @@ void DeviceHandlerBase::disableCommandsAndReplies() {
     }
   }
 }
+
+ReturnValue_t DeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
+}
+
+const HasHealthIF* DeviceHandlerBase::getOptHealthIF() const { return this; }
+
+const HasModesIF& DeviceHandlerBase::getModeIF() const { return *this; }
+
+ModeTreeChildIF& DeviceHandlerBase::getModeTreeChildIF() { return *this; }
+
+ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action,
+                                              ReturnValue_t result) {
+  auto commandIter = deviceCommandMap.find(action);
+  if (commandIter == deviceCommandMap.end()) {
+    return MessageQueueIF::NO_QUEUE;
+  }
+  commandIter->second.isExecuting = false;
+  actionHelper.finish(success, commandIter->second.sendReplyTo, action, result);
+  return returnvalue::OK;
+}

src/fsfw/devicehandlers/DeviceHandlerBase.h

@@ -21,9 +21,9 @@
 #include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
+#include "fsfw/subsystem/ModeTreeConnectionIF.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw/tasks/PeriodicTaskIF.h"
-#include "fsfw/util/dataWrapper.h"
 
 namespace Factory {
 void setStaticFrameworkObjectIds();
@@ -84,6 +84,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
                           public HasModesIF,
                           public HasHealthIF,
                           public HasActionsIF,
+                          public ModeTreeChildIF,
+                          public ModeTreeConnectionIF,
                           public ReceivesParameterMessagesIF,
                           public HasLocalDataPoolIF {
   friend void(Factory::setStaticFrameworkObjectIds)();
@@ -104,8 +106,55 @@ class DeviceHandlerBase : public DeviceHandlerIF,
                     FailureIsolationBase *fdirInstance = nullptr, size_t cmdQueueSize = 20);
 
   void setCustomFdir(FailureIsolationBase *fdir);
-  void setParent(object_id_t parent);
   void setPowerSwitcher(PowerSwitchIF *switcher);
+
+  /**
+   * extending the modes of DeviceHandler IF for internal state machine
+   */
+  static constexpr uint8_t TRANSITION_MODE_CHILD_ACTION_MASK = 0x20;
+  static constexpr uint8_t TRANSITION_MODE_BASE_ACTION_MASK = 0x10;
+  //! This is a transitional state which can not be commanded. The device
+  //! handler performs all commands to get the device in a state ready to
+  //! perform commands. When this is completed, the mode changes to @c MODE_ON.
+  static const Mode_t _MODE_START_UP = TRANSITION_MODE_CHILD_ACTION_MASK | 5;
+  //! This is a transitional state which can not be commanded.
+  //! The device handler performs all actions and commands to get the device
+  //! shut down. When the device is off, the mode changes to @c MODE_OFF.
+  //! It is possible to set the mode to _MODE_SHUT_DOWN to use the to off
+  //! transition if available.
+  static const Mode_t _MODE_SHUT_DOWN = TRANSITION_MODE_CHILD_ACTION_MASK | 6;
+  //! It is possible to set the mode to _MODE_TO_ON to use the to on
+  //! transition if available.
+  static const Mode_t _MODE_TO_ON = TRANSITION_MODE_CHILD_ACTION_MASK | HasModesIF::MODE_ON;
+  //! It is possible to set the mode to _MODE_TO_RAW to use the to raw
+  //! transition if available.
+  static const Mode_t _MODE_TO_RAW = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_RAW;
+  //! It is possible to set the mode to _MODE_TO_NORMAL to use the to normal
+  //! transition if available.
+  static const Mode_t _MODE_TO_NORMAL = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_NORMAL;
+  //! This is a transitional state which can not be commanded.
+  //! The device is shut down and ready to be switched off.
+  //! After the command to set the switch off has been sent,
+  //! the mode changes to @c _MODE_WAIT_OFF
+  static const Mode_t _MODE_POWER_DOWN = TRANSITION_MODE_BASE_ACTION_MASK | 1;
+  //! This is a transitional state which can not be commanded. The device
+  //! will be switched on in this state. After the command to set the switch
+  //! on has been sent, the mode changes to @c _MODE_WAIT_ON.
+  static const Mode_t _MODE_POWER_ON = TRANSITION_MODE_BASE_ACTION_MASK | 2;
+  //! This is a transitional state which can not be commanded. The switch has
+  //! been commanded off and the handler waits for it to be off.
+  //! When the switch is off, the mode changes to @c MODE_OFF.
+  static const Mode_t _MODE_WAIT_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 3;
+  //! This is a transitional state which can not be commanded. The switch
+  //! has been commanded on and the handler waits for it to be on.
+  //! When the switch is on, the mode changes to @c _MODE_TO_ON.
+  static const Mode_t _MODE_WAIT_ON = TRANSITION_MODE_BASE_ACTION_MASK | 4;
+  //! This is a transitional state which can not be commanded. The switch has
+  //! been commanded off and is off now. This state is only to do an RMAP
+  //! cycle once more where the doSendRead() function will set the mode to
+  //! MODE_OFF. The reason to do this is to get rid of stuck packets in the IO Board.
+  static const Mode_t _MODE_SWITCH_IS_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 5;
+
   void setHkDestination(object_id_t hkDestination);
 
   /**
@@ -121,6 +170,10 @@ class DeviceHandlerBase : public DeviceHandlerIF,
       lp_id_t thermalStatePoolId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID,
       lp_id_t thermalRequestPoolId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID,
       uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID);
+
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
+  ModeTreeChildIF &getModeTreeChildIF() override;
+
   /**
    * @brief   Helper function to ease device handler development.
    * This will instruct the transition to MODE_ON immediately
@@ -166,7 +219,7 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * @param counter Specifies which Action to perform
    * @return returnvalue::OK for successful execution
    */
-  virtual ReturnValue_t performOperation(uint8_t counter) override;
+  ReturnValue_t performOperation(uint8_t counter) override;
 
   /**
    * @brief  Initializes the device handler
@@ -176,14 +229,14 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * Calls fillCommandAndReplyMap().
    * @return
    */
-  virtual ReturnValue_t initialize() override;
+  ReturnValue_t initialize() override;
 
   /**
    * @brief   Intialization steps performed after all tasks have been created.
    *          This function will be called by the executing task.
    * @return
    */
-  virtual ReturnValue_t initializeAfterTaskCreation() override;
+  ReturnValue_t initializeAfterTaskCreation() override;
 
   /** Destructor. */
   virtual ~DeviceHandlerBase();
@@ -200,6 +253,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   virtual object_id_t getObjectId() const override;
 
   /**
+   * This is a helper method for classes which are parent nodes in the mode tree.
+   * It registers the passed queue as the destination for mode and health messages.
    * @param parentQueueId
    */
   virtual void setParentQueue(MessageQueueId_t parentQueueId);
@@ -399,6 +454,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    */
   virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) = 0;
   MessageQueueId_t getCommanderQueueId(DeviceCommandId_t replyId) const;
+  ReturnValue_t finishAction(bool success, DeviceCommandId_t action, ReturnValue_t result);
+
   /**
    * Helper function to get pending command. This is useful for devices
    * like SPI sensors to identify the last sent command.
@@ -694,15 +751,18 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   size_t rawPacketLen = 0;
 
   /**
-   * The mode the device handler is currently in.
-   * This should never be changed directly but only with setMode()
+   * Get the current mode
+   *
+   * set via setMode()
    */
-  Mode_t mode;
+  Mode_t getMode();
+
   /**
-   * The submode the device handler is currently in.
-   * This should never be changed directly but only with setMode()
+   * Get the current Submode
+   *
+   * set via setMode()
    */
-  Submode_t submode;
+  Submode_t getSubmode();
 
   /** This is the counter value from performOperation(). */
   uint8_t pstStep = 0;
@@ -891,8 +951,8 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * Do the transition to the main modes (MODE_ON, MODE_NORMAL and MODE_RAW).
    *
    * If the transition is complete, the mode should be set to the target mode,
-   * which can be deduced from the current mode which is
-   * [_MODE_TO_ON, _MODE_TO_NORMAL, _MODE_TO_RAW]
+   * which can be deduced from the current mode (which is
+   * [_MODE_TO_ON, _MODE_TO_NORMAL, _MODE_TO_RAW]) using getBaseMode()
    *
    * The intended target submode is already set.
    * The origin submode can be read in subModeFrom.
@@ -959,6 +1019,9 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    */
   LocalDataPoolManager *getHkManagerHandle() override;
 
+  const HasHealthIF *getOptHealthIF() const override;
+  const HasModesIF &getModeIF() const override;
+
   /**
    * Returns the delay cycle count of a reply.
    * A count != 0 indicates that the command is already executed.
@@ -1138,6 +1201,22 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    */
   virtual ReturnValue_t doSendReadHook();
 
+  /**
+   * Send a RMAP getRead command.
+   *
+   * The size of the getRead command is #maxDeviceReplyLen.
+   * This is always executed, independently from the current mode.
+   */
+  virtual void doSendRead(void);
+  /**
+   * Check the getRead reply and the contained data.
+   *
+   * If data was received scanForReply() and, if successful, handleReply()
+   * are called. If the current mode is @c MODE_RAW, the received packet
+   * is sent to the commanding object via commandQueue.
+   */
+  virtual void doGetRead();
+
  private:
   /**
    * State a cookie is in.
@@ -1188,6 +1267,18 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    */
   uint32_t childTransitionDelay;
 
+  /**
+   * The mode the device handler is currently in.
+   * This should not be changed directly but only with setMode()
+   */
+  Mode_t mode;
+
+  /**
+   * The submode the device handler is currently in.
+   * This should not be changed directly but only with setMode()
+   */
+  Submode_t submode;
+
   /**
    * @brief   The mode the current transition originated from
    *
@@ -1205,6 +1296,15 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    */
   Submode_t transitionSourceSubMode;
 
+  /**
+   * used to make the state machine continue from ON to NOMAL when
+   * a Device is commanded to NORMAL in OFF mode
+   *
+   * set in startTransition()
+   * evaluated in setMode() to continue to NORMAL when ON is reached
+   */
+  bool continueToNormal;
+
   /**
    * read the command queue
    */
@@ -1273,21 +1373,6 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * - if the action was successful, the reply timout counter is initialized
    */
   void doGetWrite(void);
-  /**
-   * Send a RMAP getRead command.
-   *
-   * The size of the getRead command is #maxDeviceReplyLen.
-   * This is always executed, independently from the current mode.
-   */
-  void doSendRead(void);
-  /**
-   * Check the getRead reply and the contained data.
-   *
-   * If data was received scanForReply() and, if successful, handleReply()
-   * are called. If the current mode is @c MODE_RAW, the received packet
-   * is sent to the commanding object via commandQueue.
-   */
-  void doGetRead(void);
 
   /**
    * @brief Resets replies which use a timeout to detect missed replies.

src/fsfw/devicehandlers/DeviceHandlerIF.h

@@ -24,9 +24,6 @@ class DeviceHandlerIF {
   static const DeviceCommandId_t RAW_COMMAND_ID = -1;
   static const DeviceCommandId_t NO_COMMAND_ID = -2;
 
-  static constexpr uint8_t TRANSITION_MODE_CHILD_ACTION_MASK = 0x20;
-  static constexpr uint8_t TRANSITION_MODE_BASE_ACTION_MASK = 0x10;
-
   using dh_heater_request_t = uint8_t;
   using dh_thermal_state_t = int8_t;
 
@@ -54,47 +51,6 @@ class DeviceHandlerIF {
   //! device still is powered. In this mode, only a mode change to @c MODE_OFF
   //! can be commanded, which tries to switch off the device again.
   static const Mode_t MODE_ERROR_ON = 4;
-  //! This is a transitional state which can not be commanded. The device
-  //! handler performs all commands to get the device in a state ready to
-  //! perform commands. When this is completed, the mode changes to @c MODE_ON.
-  static const Mode_t _MODE_START_UP = TRANSITION_MODE_CHILD_ACTION_MASK | 5;
-  //! This is a transitional state which can not be commanded.
-  //! The device handler performs all actions and commands to get the device
-  //! shut down. When the device is off, the mode changes to @c MODE_OFF.
-  //! It is possible to set the mode to _MODE_SHUT_DOWN to use the to off
-  //! transition if available.
-  static const Mode_t _MODE_SHUT_DOWN = TRANSITION_MODE_CHILD_ACTION_MASK | 6;
-  //! It is possible to set the mode to _MODE_TO_ON to use the to on
-  //! transition if available.
-  static const Mode_t _MODE_TO_ON = TRANSITION_MODE_CHILD_ACTION_MASK | HasModesIF::MODE_ON;
-  //! It is possible to set the mode to _MODE_TO_RAW to use the to raw
-  //! transition if available.
-  static const Mode_t _MODE_TO_RAW = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_RAW;
-  //! It is possible to set the mode to _MODE_TO_NORMAL to use the to normal
-  //! transition if available.
-  static const Mode_t _MODE_TO_NORMAL = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_NORMAL;
-  //! This is a transitional state which can not be commanded.
-  //! The device is shut down and ready to be switched off.
-  //! After the command to set the switch off has been sent,
-  //! the mode changes to @c MODE_WAIT_OFF
-  static const Mode_t _MODE_POWER_DOWN = TRANSITION_MODE_BASE_ACTION_MASK | 1;
-  //! This is a transitional state which can not be commanded. The device
-  //! will be switched on in this state. After the command to set the switch
-  //! on has been sent, the mode changes to @c MODE_WAIT_ON.
-  static const Mode_t _MODE_POWER_ON = TRANSITION_MODE_BASE_ACTION_MASK | 2;
-  //! This is a transitional state which can not be commanded. The switch has
-  //! been commanded off and the handler waits for it to be off.
-  //! When the switch is off, the mode changes to @c MODE_OFF.
-  static const Mode_t _MODE_WAIT_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 3;
-  //! This is a transitional state which can not be commanded. The switch
-  //! has been commanded on and the handler waits for it to be on.
-  //! When the switch is on, the mode changes to @c MODE_TO_ON.
-  static const Mode_t _MODE_WAIT_ON = TRANSITION_MODE_BASE_ACTION_MASK | 4;
-  //! This is a transitional state which can not be commanded. The switch has
-  //! been commanded off and is off now. This state is only to do an RMAP
-  //! cycle once more where the doSendRead() function will set the mode to
-  //! MODE_OFF. The reason to do this is to get rid of stuck packets in the IO Board.
-  static const Mode_t _MODE_SWITCH_IS_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 5;
 
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CDH;
   static const Event DEVICE_BUILDING_COMMAND_FAILED = MAKE_EVENT(0, severity::LOW);

src/fsfw/devicehandlers/DeviceTmReportingWrapper.h

@@ -4,7 +4,6 @@
 #include "fsfw/action/HasActionsIF.h"
 #include "fsfw/objectmanager/SystemObjectIF.h"
 #include "fsfw/serialize/SerializeIF.h"
-#include "fsfw/util/dataWrapper.h"
 
 class DeviceTmReportingWrapper : public SerializeIF {
  public:

src/fsfw/events/EventManager.cpp

@@ -24,6 +24,7 @@ EventManager::EventManager(object_id_t setObjectId)
 }
 
 EventManager::~EventManager() {
+  listenerList.clear();
   QueueFactory::instance()->deleteMessageQueue(eventReportQueue);
   MutexFactory::instance()->deleteMutex(mutex);
 }
@@ -73,9 +74,14 @@ ReturnValue_t EventManager::registerListener(MessageQueueId_t listener,
   if (!result.second) {
     return returnvalue::FAILED;
   }
+
   return returnvalue::OK;
 }
 
+ReturnValue_t EventManager::unregisterListener(MessageQueueId_t listener) {
+  return listenerList.erase(listener) == 1 ? returnvalue::OK : returnvalue::FAILED;
+}
+
 ReturnValue_t EventManager::subscribeToEvent(MessageQueueId_t listener, EventId_t event) {
   return subscribeToEventRange(listener, event);
 }

src/fsfw/events/EventManager.h

@@ -31,6 +31,7 @@ class EventManager : public EventManagerIF, public ExecutableObjectIF, public Sy
   MessageQueueId_t getEventReportQueue();
 
   ReturnValue_t registerListener(MessageQueueId_t listener, bool forwardAllButSelected = false);
+  ReturnValue_t unregisterListener(MessageQueueId_t listener) override;
   ReturnValue_t subscribeToEvent(MessageQueueId_t listener, EventId_t event);
   ReturnValue_t subscribeToAllEventsFrom(MessageQueueId_t listener, object_id_t object);
   ReturnValue_t subscribeToEventRange(MessageQueueId_t listener, EventId_t idFrom = 0,

src/fsfw/events/EventManagerIF.h

@@ -18,6 +18,7 @@ class EventManagerIF {
 
   virtual ReturnValue_t registerListener(MessageQueueId_t listener,
                                          bool forwardAllButSelected = false) = 0;
+  virtual ReturnValue_t unregisterListener(MessageQueueId_t listener) = 0;
   virtual ReturnValue_t subscribeToEvent(MessageQueueId_t listener, EventId_t event) = 0;
   virtual ReturnValue_t subscribeToAllEventsFrom(MessageQueueId_t listener, object_id_t object) = 0;
   virtual ReturnValue_t unsubscribeFromAllEvents(MessageQueueId_t listener, object_id_t object) = 0;

src/fsfw/events/fwSubsystemIdRanges.h

@@ -33,6 +33,7 @@ enum : uint8_t {
   PUS_SERVICE_23 = 103,
   MGM_LIS3MDL = 106,
   MGM_RM3100 = 107,
+  CFDP = 108,
 
   FW_SUBSYSTEM_ID_RANGE
 };

src/fsfw/fdir/FailureIsolationBase.cpp

@@ -24,7 +24,7 @@ FailureIsolationBase::~FailureIsolationBase() {
 #endif
     return;
   }
-  manager->unsubscribeFromAllEvents(eventQueue->getId(), ownerId);
+  manager->unregisterListener(eventQueue->getId());
   QueueFactory::instance()->deleteMessageQueue(eventQueue);
 }
 

src/fsfw/filesystem/HasFileSystemIF.h

@@ -40,6 +40,7 @@ class HasFileSystemIF {
   //! [EXPORT] : P1: Can be file system specific error code
   static constexpr ReturnValue_t GENERIC_FILE_ERROR = MAKE_RETURN_CODE(0);
   static constexpr ReturnValue_t GENERIC_DIR_ERROR = MAKE_RETURN_CODE(1);
+  static constexpr ReturnValue_t FILESYSTEM_INACTIVE = MAKE_RETURN_CODE(2);
   static constexpr ReturnValue_t GENERIC_RENAME_ERROR = MAKE_RETURN_CODE(3);
 
   //! [EXPORT] : File system is currently busy

src/fsfw/globalfunctions/DleParser.cpp

@@ -3,159 +3,94 @@
 #include <fsfw/serviceinterface/ServiceInterface.h>
 
 #include <cstdio>
+#include <fstream>
+#include <iostream>
 
 DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
-                     BufPair decodedBuf, UserHandler handler, void* args)
+                     BufPair decodedBuf)
     : 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
-  }
-}
+      decodedBuf(decodedBuf) {}
 
-ReturnValue_t DleParser::passData(uint8_t* data, size_t len) {
-  if (data == nullptr or len == 0 or handler == nullptr) {
+ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) {
+  if (data == nullptr or len == 0) {
     return returnvalue::FAILED;
   }
-  size_t copyIntoRingBufFromHere = 0;
-  size_t copyAmount = len;
-  size_t startIdx = 0;
-  ReturnValue_t result = returnvalue::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;
+  return decodeRingBuf.writeData(data, len);
 }
-      startFound = true;
-      startFoundInThisPacket = true;
-    } else if (data[idx] == DleEncoder::ETX_CHAR) {
-      if (startFoundInThisPacket) {
-        size_t readLen = 0;
+
+ReturnValue_t DleParser::parseRingBuf(size_t& readSize) {
+  ctx.setType(DleParser::ContextType::NONE);
+  size_t availableData = decodeRingBuf.getAvailableReadData();
+  if (availableData == 0) {
+    return NO_PACKET_FOUND;
+  }
+  if (availableData > encodedBuf.second) {
+    ErrorInfo info;
+    info.len = decodeRingBuf.getAvailableReadData();
+    setErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
+    return returnvalue::FAILED;
+  }
+  ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData);
+  if (result != returnvalue::OK) {
+    ErrorInfo info;
+    info.res = result;
+    setErrorContext(ErrorTypes::RING_BUF_ERROR, info);
+    return result;
+  }
+  bool stxFound = false;
+  size_t stxIdx = 0;
+  for (size_t vectorIdx = 0; vectorIdx < availableData; vectorIdx++) {
+    // handle STX char
+    if (encodedBuf.first[vectorIdx] == DleEncoder::STX_CHAR) {
+      if (not stxFound) {
+        stxFound = true;
+        stxIdx = vectorIdx;
+      } else {
+        // might be lost packet, so we should advance the read pointer
+        // without skipping the STX
+        readSize = vectorIdx;
+        ErrorInfo info;
+        setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info);
+        return POSSIBLE_PACKET_LOSS;
+      }
+    }
+    // handle ETX char
+    if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) {
+      if (stxFound) {
+        // This is propably a packet, so we decode it.
         size_t decodedLen = 0;
-        result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first,
-                                decodedBuf.second, &decodedLen);
+        size_t dummy = 0;
+
+        ReturnValue_t result =
+            decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy,
+                           decodedBuf.first, decodedBuf.second, &decodedLen);
         if (result == returnvalue::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 != returnvalue::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 == returnvalue::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 != returnvalue::OK) {
-      ErrorInfo info;
-      info.res = result;
-      prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info);
-      handler(ctx);
-    }
-  }
+          readSize = ++vectorIdx;
           return returnvalue::OK;
+        } else {
+          // invalid packet, skip.
+          readSize = ++vectorIdx;
+          ErrorInfo info;
+          info.res = result;
+          setErrorContext(ErrorTypes::DECODE_ERROR, info);
+          return POSSIBLE_PACKET_LOSS;
+        }
+      } else {
+        // might be lost packet, so we should advance the read pointer
+        readSize = ++vectorIdx;
+        ErrorInfo info;
+        info.len = 0;
+        setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info);
+        return POSSIBLE_PACKET_LOSS;
+      }
+    }
+  }
+  return NO_PACKET_FOUND;
 }
 
 void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) {
@@ -169,8 +104,12 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg
 #endif
 }
 
-void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) {
-  switch (err) {
+void DleParser::defaultErrorHandler() {
+  if (ctx.getType() != DleParser::ContextType::ERROR) {
+    errorPrinter("No error");
+    return;
+  }
+  switch (ctx.error.first) {
     case (ErrorTypes::NONE): {
       errorPrinter("No error");
       break;
@@ -186,8 +125,9 @@ void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) {
     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) {
+      snprintf(opt, sizeof(opt), ": Too small for packet with length %zu",
+               ctx.decodedPacket.second);
+      if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
         errorPrinter("Encoded buf too small", opt);
       } else {
         errorPrinter("Decoding buf too small", opt);
@@ -218,13 +158,16 @@ void DleParser::errorPrinter(const char* str, const char* opt) {
 #endif
 }
 
-void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) {
+void DleParser::setErrorContext(ErrorTypes err, ErrorInfo info) {
   ctx.setType(ContextType::ERROR);
   ctx.error.first = err;
   ctx.error.second = info;
 }
 
-void DleParser::reset() {
-  startFound = false;
-  decodeRingBuf.clear();
+ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) {
+  return decodeRingBuf.deleteData(bytesRead);
 }
+
+const DleParser::Context& DleParser::getContext() { return ctx; }
+
+void DleParser::reset() { decodeRingBuf.clear(); }

src/fsfw/globalfunctions/DleParser.h

@@ -18,9 +18,11 @@
  */
 class DleParser {
  public:
+  static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 1);
+  static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2);
   using BufPair = std::pair<uint8_t*, size_t>;
 
-  enum class ContextType { PACKET_FOUND, ERROR };
+  enum class ContextType { NONE, PACKET_FOUND, ERROR };
 
   enum class ErrorTypes {
     NONE,
@@ -41,7 +43,7 @@ class DleParser {
 
   struct Context {
    public:
-    Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); }
+    Context() { setType(ContextType::PACKET_FOUND); }
 
     void setType(ContextType type) {
       this->type = type;
@@ -58,14 +60,11 @@ class DleParser {
 
     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
@@ -79,7 +78,7 @@ class DleParser {
    * @param args Arbitrary user argument
    */
   DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
-            BufPair decodedBuf, UserHandler handler, void* args);
+            BufPair decodedBuf);
 
   /**
    * This function allows to pass new data into the parser. It then scans for DLE packets
@@ -88,8 +87,13 @@ class DleParser {
    * @param len
    * @return
    */
-  ReturnValue_t passData(uint8_t* data, size_t len);
+  ReturnValue_t passData(const uint8_t* data, size_t len);
 
+  ReturnValue_t parseRingBuf(size_t& bytesRead);
+
+  ReturnValue_t confirmBytesRead(size_t bytesRead);
+
+  const Context& getContext();
   /**
    * Example found packet handler
    * function call
@@ -104,11 +108,11 @@ class DleParser {
    *  - 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);
+  void defaultErrorHandler();
 
   static void errorPrinter(const char* str, const char* opt = nullptr);
 
-  void prepareErrorContext(ErrorTypes err, ErrorInfo ctx);
+  void setErrorContext(ErrorTypes err, ErrorInfo ctx);
   /**
    * Resets the parser by resetting the internal states and clearing the decoding ring buffer
    */
@@ -119,7 +123,5 @@ class DleParser {
   DleEncoder& decoder;
   BufPair encodedBuf;
   BufPair decodedBuf;
-  UserHandler handler = nullptr;
   Context ctx;
-  bool startFound = false;
 };

src/fsfw/globalfunctions/matching/MatchTree.h

@@ -24,7 +24,7 @@ class MatchTree : public SerializeableMatcherIF<T>, public BinaryTree<Serializea
   MatchTree(iterator root, uint8_t maxDepth = -1)
       : BinaryTree<SerializeableMatcherIF<T>>(root.element), maxDepth(maxDepth) {}
   MatchTree() : BinaryTree<SerializeableMatcherIF<T>>(), maxDepth(-1) {}
-  virtual ~MatchTree() {}
+  virtual ~MatchTree() { clear(); }
   virtual bool match(T number) override { return matchesTree(number); }
   bool matchesTree(T number) {
     iterator iter = this->begin();
@@ -176,6 +176,45 @@ class MatchTree : public SerializeableMatcherIF<T>, public BinaryTree<Serializea
     return cleanUpElement(position);
   }
 
+  void clear() {
+    Node* localRoot = BinaryTree<SerializeableMatcherIF<T>>::rootNode;
+
+    if (localRoot == nullptr) {
+      return;
+    }
+
+    Node* node = localRoot->left;
+
+    while (true) {
+      if (node->left != nullptr) {
+        node = node->left;
+        continue;
+      }
+      if (node->right != nullptr) {
+        node = node->right;
+        continue;
+      }
+      if (node->parent == nullptr) {
+        // this is the root node with no children
+        if (node->value != nullptr) {
+          cleanUpElement(iterator(node));
+        }
+        return;
+      }
+      // leaf
+      {
+        Node* parent = node->parent;
+        if (parent->left == node) {
+          parent->left = nullptr;
+        } else {
+          parent->right = nullptr;
+        }
+        cleanUpElement(iterator(node));
+        node = parent;
+      }
+    }
+  }
+
   virtual ReturnValue_t cleanUpElement(iterator position) { return returnvalue::OK; }
 
   bool matchSubtree(iterator iter, T number) {

src/fsfw/globalfunctions/timevalOperations.cpp

@@ -1,8 +1,8 @@
 #include "fsfw/globalfunctions/timevalOperations.h"
 
 timeval& operator+=(timeval& lhs, const timeval& rhs) {
-  int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
-  sum += rhs.tv_sec * 1000000. + rhs.tv_usec;
+  int64_t sum = static_cast<int64_t>(lhs.tv_sec) * 1000000. + lhs.tv_usec;
+  sum += static_cast<int64_t>(rhs.tv_sec) * 1000000. + rhs.tv_usec;
   lhs.tv_sec = sum / 1000000;
   lhs.tv_usec = sum - lhs.tv_sec * 1000000;
   return lhs;

src/fsfw/health/HealthHelper.cpp

@@ -5,7 +5,11 @@
 HealthHelper::HealthHelper(HasHealthIF* owner, object_id_t objectId)
     : objectId(objectId), owner(owner) {}
 
-HealthHelper::~HealthHelper() { healthTable->removeObject(objectId); }
+HealthHelper::~HealthHelper() {
+  if (healthTable != nullptr) {
+    healthTable->removeObject(objectId);
+  }
+}
 
 ReturnValue_t HealthHelper::handleHealthCommand(CommandMessage* message) {
   switch (message->getCommand()) {

src/fsfw/health/HealthTable.cpp

@@ -6,8 +6,6 @@
 
 HealthTable::HealthTable(object_id_t objectid) : SystemObject(objectid) {
   mutex = MutexFactory::instance()->createMutex();
-  ;
-
   mapIterator = healthMap.begin();
 }
 

src/fsfw/health/HealthTable.h

@@ -8,6 +8,8 @@
 #include "HealthTableIF.h"
 
 class HealthTable : public HealthTableIF, public SystemObject {
+  friend class CServiceHealthCommanding;
+
  public:
   explicit HealthTable(object_id_t objectid);
   ~HealthTable() override;

src/fsfw/internalerror/InternalErrorReporter.cpp

@@ -10,13 +10,17 @@ InternalErrorReporter::InternalErrorReporter(object_id_t setObjectId, uint32_t m
       poolManager(this, commandQueue),
       internalErrorSid(setObjectId, InternalErrorDataset::ERROR_SET_ID),
       internalErrorDataset(this) {
+  commandQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
   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); }
+InternalErrorReporter::~InternalErrorReporter() {
+  MutexFactory::instance()->deleteMutex(mutex);
+  QueueFactory::instance()->deleteMessageQueue(commandQueue);
+}
 
 void InternalErrorReporter::setDiagnosticPrintout(bool enable) {
   this->diagnosticPrintout = enable;

src/fsfw/modes/ModeMessage.cpp

@@ -24,3 +24,19 @@ void ModeMessage::setCantReachMode(CommandMessage* message, ReturnValue_t reason
   message->setParameter(reason);
   message->setParameter2(0);
 }
+
+void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive) {
+  Command_t cmd;
+  if (recursive) {
+    cmd = CMD_MODE_ANNOUNCE_RECURSIVELY;
+  } else {
+    cmd = CMD_MODE_ANNOUNCE;
+  }
+  message.setCommand(cmd);
+}
+
+void ModeMessage::setCmdModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode) {
+  setModeMessage(&message, CMD_MODE_COMMAND, mode, submode);
+}
+
+void ModeMessage::setModeReadMessage(CommandMessage& message) { message.setCommand(CMD_MODE_READ); }

src/fsfw/modes/ModeMessage.h

@@ -1,43 +1,42 @@
 #ifndef FSFW_MODES_MODEMESSAGE_H_
 #define FSFW_MODES_MODEMESSAGE_H_
 
-#include "../ipc/CommandMessage.h"
+#include "fsfw/ipc/CommandMessage.h"
 
 typedef uint32_t Mode_t;
 typedef uint8_t Submode_t;
 
 class ModeMessage {
- private:
-  ModeMessage();
-
  public:
   static const uint8_t MESSAGE_ID = messagetypes::MODE_COMMAND;
-  static const Command_t CMD_MODE_COMMAND =
-      MAKE_COMMAND_ID(0x01);  //!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY,
+  //!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY,
   //! REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies,
   //! as this will break the subsystem mode machine!!
-  static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(
-      0xF1);  //!> Command to set the specified Mode, regardless of external control flag, replies
+  static const Command_t CMD_MODE_COMMAND = MAKE_COMMAND_ID(0x01);
+  //!> Command to set the specified Mode, regardless of external control flag, replies
   //! are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any
   //! replies, as this will break the subsystem mode machine!!
-  static const Command_t REPLY_MODE_REPLY =
-      MAKE_COMMAND_ID(0x02);  //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
-  static const Command_t REPLY_MODE_INFO =
-      MAKE_COMMAND_ID(0x03);  //!> Unrequested info about the current mode (used for composites to
+  static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(0xF1);
+  //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
+  static const Command_t REPLY_MODE_REPLY = MAKE_COMMAND_ID(0x02);
+  //!> Unrequested info about the current mode (used for composites to
   //! inform their container of a changed mode)
-  static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(
-      0x04);  //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
-  static const Command_t REPLY_WRONG_MODE_REPLY =
-      MAKE_COMMAND_ID(0x05);  //!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded
+  static const Command_t REPLY_MODE_INFO = MAKE_COMMAND_ID(0x03);
+  //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
+  static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(0x04);
+  //!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded
   //! and a transition started but was aborted; the parameters contain
   //! the mode that was reached
-  static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(
-      0x06);  //!> Command to read the current mode and reply with a REPLY_MODE_REPLY
-  static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(
-      0x07);  //!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
-  static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY =
-      MAKE_COMMAND_ID(0x08);  //!> Command to trigger an ModeInfo Event and to send this command to
+  static const Command_t REPLY_WRONG_MODE_REPLY = MAKE_COMMAND_ID(0x05);
+  //!> Command to read the current mode and reply with a REPLY_MODE_REPLY
+  static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(0x06);
+  //!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
+  static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(0x07);
+  //!> Command to trigger an ModeInfo Event and to send this command to
   //! every child. This command does NOT have a reply.
+  static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY = MAKE_COMMAND_ID(0x08);
+
+  ModeMessage() = delete;
 
   static Mode_t getMode(const CommandMessage* message);
   static Submode_t getSubmode(const CommandMessage* message);
@@ -45,6 +44,9 @@ class ModeMessage {
 
   static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
                              Submode_t submode);
+  static void setCmdModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode);
+  static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
+  static void setModeReadMessage(CommandMessage& message);
   static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
   static void clear(CommandMessage* message);
 };

src/fsfw/objectmanager/ObjectManager.cpp

@@ -23,9 +23,17 @@ void ObjectManager::setObjectFactoryFunction(produce_function_t objFactoryFunc,
 
 ObjectManager::ObjectManager() = default;
 
+void ObjectManager::clear() {
+  if (objManagerInstance != nullptr) {
+    delete objManagerInstance;
+    objManagerInstance = nullptr;
+  }
+}
+
 ObjectManager::~ObjectManager() {
-  for (auto const& iter : objectList) {
-    delete iter.second;
+  teardown = true;
+  for (auto iter = objectList.begin(); iter != objectList.end(); iter = objectList.erase(iter)) {
+    delete iter->second;
   }
 }
 
@@ -53,6 +61,12 @@ ReturnValue_t ObjectManager::insert(object_id_t id, SystemObjectIF* object) {
 }
 
 ReturnValue_t ObjectManager::remove(object_id_t id) {
+  // this function is called during destruction of System Objects
+  // disabeld for teardown to avoid iterator invalidation and
+  // double free
+  if (teardown) {
+    return returnvalue::OK;
+  }
   if (this->getSystemObject(id) != nullptr) {
     this->objectList.erase(id);
 #if FSFW_CPP_OSTREAM_ENABLED == 1

src/fsfw/objectmanager/ObjectManager.h

@@ -24,12 +24,17 @@ class ObjectManager : public ObjectManagerIF {
   using produce_function_t = void (*)(void* args);
 
   /**
-   * Returns the single instance of TaskFactory.
+   * Returns the single instance of ObjectManager.
    * The implementation of #instance is found in its subclasses.
    * Thus, we choose link-time variability of the  instance.
    */
   static ObjectManager* instance();
 
+  /**
+   * Deletes the single instance of ObjectManager
+   */
+  static void clear();
+
   void setObjectFactoryFunction(produce_function_t prodFunc, void* args);
 
   template <typename T>
@@ -66,6 +71,9 @@ class ObjectManager : public ObjectManagerIF {
    */
   std::map<object_id_t, SystemObjectIF*> objectList;
   static ObjectManager* objManagerInstance;
+  // used when the OM itself is deleted to modify behaviour of remove()
+  // to avoid iterator invalidation and double free
+  bool teardown = false;
 };
 
 // Documentation can be found in the class method declaration above

src/fsfw/osal/CMakeLists.txt

@@ -1,10 +1,13 @@
 # Check the OS_FSFW variable
 if(FSFW_OSAL MATCHES "freertos")
   add_subdirectory(freertos)
+  set(FSFW_OSAL_FREERTOS 1)
 elseif(FSFW_OSAL MATCHES "rtems")
   add_subdirectory(rtems)
+  set(FSFW_OSAL_RTEMS 1)
 elseif(FSFW_OSAL MATCHES "linux")
   add_subdirectory(linux)
+  set(FSFW_OSAL_LINUX 1)
 elseif(FSFW_OSAL MATCHES "host")
   add_subdirectory(host)
   if(WIN32)
@@ -13,18 +16,20 @@ elseif(FSFW_OSAL MATCHES "host")
     # We still need to pull in some Linux specific sources
     target_sources(${LIB_FSFW_NAME} PUBLIC linux/tcpipHelpers.cpp)
   endif()
-
+  set(FSFW_OSAL_HOST 1)
 else()
-
   message(
     WARNING
       "${MSG_PREFIX} The FSFW_OSAL variable was not set. Assuming host OS..")
+
   # Not set. Assumuing this is a host build, try to determine host OS
   if(WIN32)
     add_subdirectory(host)
     add_subdirectory(windows)
+    set(FSFW_OSAL_HOST 1)
   elseif(UNIX)
     add_subdirectory(linux)
+    set(FSFW_OSAL_LINUX 1)
   else()
     # MacOS or other OSes have not been tested yet / are not supported.
     message(FATAL_ERROR "The host OS could not be determined! Aborting.")
@@ -33,3 +38,5 @@ else()
 endif()
 
 add_subdirectory(common)
+
+configure_file(osal.h.in ${CMAKE_BINARY_DIR}/fsfw/osal/osal.h)

src/fsfw/osal/common/TcpTmTcServer.cpp

@@ -26,12 +26,12 @@
 const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT;
 
 TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
-                             size_t receptionBufferSize, size_t ringBufferSize,
-                             std::string customTcpServerPort, ReceptionModes receptionMode)
+                             TcpTmTcServer::TcpConfig cfg, size_t receptionBufferSize,
+                             size_t ringBufferSize, ReceptionModes receptionMode)
     : SystemObject(objectId),
       tmtcBridgeId(tmtcTcpBridge),
       receptionMode(receptionMode),
-      tcpConfig(std::move(customTcpServerPort)),
+      tcpConfig(cfg),
       receptionBuffer(receptionBufferSize),
       ringBuffer(ringBufferSize, true) {}
 
@@ -91,6 +91,15 @@ ReturnValue_t TcpTmTcServer::initialize() {
     return returnvalue::FAILED;
   }
 
+  if (tcpConfig.reuseAddr) {
+    unsigned int enable = 1;
+    setsockopt(listenerTcpSocket, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable));
+  }
+  if (tcpConfig.reusePort) {
+    unsigned int enable = 1;
+    setsockopt(listenerTcpSocket, SOL_SOCKET, SO_REUSEPORT, &enable, sizeof(enable));
+  }
+
   // Bind to the address found by getaddrinfo
   retval = bind(listenerTcpSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
   if (retval == SOCKET_ERROR) {
@@ -274,6 +283,8 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent)
     ConstStorageAccessor storeAccessor(storeId);
     ReturnValue_t result = tmStore->getData(storeId, storeAccessor);
     if (result != returnvalue::OK) {
+      // Invalid entry, pop FIFO
+      tmtcBridge->tmFifo->pop();
       return result;
     }
     if (wiretappingEnabled) {

src/fsfw/osal/common/TcpTmTcServer.h

@@ -41,11 +41,11 @@ class SpacePacketParser;
  */
 class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableObjectIF {
  public:
-  enum class ReceptionModes { SPACE_PACKETS };
-
   struct TcpConfig {
    public:
-    explicit TcpConfig(std::string tcpPort) : tcpPort(std::move(tcpPort)) {}
+    TcpConfig(bool reuseAddr, bool reusePort) : reuseAddr(reuseAddr), reusePort(reusePort) {}
+    TcpConfig(std::string tcpPort, bool reuseAddr, bool reusePort)
+        : tcpPort(std::move(tcpPort)), reuseAddr(reuseAddr), reusePort(reusePort) {}
 
     /**
      * Passed to the recv call
@@ -63,9 +63,25 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
      */
     int tcpTmFlags = 0;
 
-    const std::string tcpPort;
+    std::string tcpPort = DEFAULT_SERVER_PORT;
+
+    /**
+     * Sets the SO_REUSEADDR option on the socket. See
+     * https://man7.org/linux/man-pages/man7/socket.7.html for more details. This option is
+     * especially useful in a debugging and development environment where an OBSW image might be
+     * re-flashed oftentimes and where all incoming telecommands are received on a dedicated TCP
+     * port.
+     */
+    bool reuseAddr = false;
+    /**
+     * Sets the SO_REUSEPORT option on the socket. See
+     * https://man7.org/linux/man-pages/man7/socket.7.html for more details.
+     */
+    bool reusePort = false;
   };
 
+  enum class ReceptionModes { SPACE_PACKETS };
+
   static const std::string DEFAULT_SERVER_PORT;
 
   static constexpr size_t ETHERNET_MTU_SIZE = 1500;
@@ -80,10 +96,9 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
    *                              size will be the Ethernet MTU size
    * @param customTcpServerPort   The user can specify another port than the default (7301) here.
    */
-  TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
+  TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, TcpTmTcServer::TcpConfig cfg,
                 size_t receptionBufferSize = RING_BUFFER_SIZE,
                 size_t ringBufferSize = RING_BUFFER_SIZE,
-                std::string customTcpServerPort = DEFAULT_SERVER_PORT,
                 ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS);
   ~TcpTmTcServer() override;
 

src/fsfw/osal/linux/MessageQueue.cpp

@@ -21,7 +21,7 @@ MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs*
   attributes.mq_msgsize = maxMessageSize;
   attributes.mq_flags = 0;  // Flags are ignored on Linux during mq_open
   // Set the name of the queue. The slash is mandatory!
-  sprintf(name, "/FSFW_MQ%u\n", queueCounter++);
+  sprintf(name, "/FSFW_MQ%u", queueCounter++);
 
   // Create a nonblocking queue if the name is available (the queue is read
   // and writable for the owner as well as the group)

src/fsfw/osal/osal.h.in

@@ -0,0 +1,36 @@
+#pragma once
+
+namespace osal {
+    enum osalTarget{
+        HOST,
+        LINUX,
+        WINDOWS,
+        FREERTOS,
+        RTEMS,
+    };
+
+#cmakedefine FSFW_OSAL_HOST
+#cmakedefine FSFW_OSAL_LINUX
+#cmakedefine FSFW_OSAL_WINDOWS
+#cmakedefine FSFW_OSAL_FREERTOS
+#cmakedefine FSFW_OSAL_RTEMS
+
+
+    constexpr osalTarget getTarget() {
+#ifdef FSFW_OSAL_HOST
+  return HOST;
+#endif
+#ifdef FSFW_OSAL_LINUX
+  return LINUX;
+#endif
+#ifdef FSFW_OSAL_WINDOWS
+  return WINDOWS;
+#endif
+#ifdef FSFW_OSAL_FREERTOS
+  return FREERTOS;
+#endif
+#ifdef FSFW_OSAL_RTEMS
+  return RTEMS;
+#endif
+    }
+};

src/fsfw/power/PowerSwitcherComponent.cpp

@@ -105,3 +105,15 @@ void PowerSwitcherComponent::setMode(Mode_t newMode, Submode_t newSubmode) {
 }
 
 HasHealthIF::HealthState PowerSwitcherComponent::getHealth() { return healthHelper.getHealth(); }
+
+const HasHealthIF* PowerSwitcherComponent::getOptHealthIF() const { return this; }
+
+const HasModesIF& PowerSwitcherComponent::getModeIF() const { return *this; }
+
+ReturnValue_t PowerSwitcherComponent::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return parent.registerChild(*this);
+}
+
+object_id_t PowerSwitcherComponent::getObjectId() const { return SystemObject::getObjectId(); }
+
+ModeTreeChildIF& PowerSwitcherComponent::getModeTreeChildIF() { return *this; }

src/fsfw/power/PowerSwitcherComponent.h

@@ -8,6 +8,8 @@
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/power/PowerSwitcher.h>
 #include <fsfw/power/definitions.h>
+#include <fsfw/subsystem/ModeTreeChildIF.h>
+#include <fsfw/subsystem/ModeTreeConnectionIF.h>
 #include <fsfw/tasks/ExecutableObjectIF.h>
 
 class PowerSwitchIF;
@@ -24,12 +26,17 @@ class PowerSwitchIF;
  */
 class PowerSwitcherComponent : public SystemObject,
                                public ExecutableObjectIF,
+                               public ModeTreeChildIF,
+                               public ModeTreeConnectionIF,
                                public HasModesIF,
                                public HasHealthIF {
  public:
   PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF *pwrSwitcher,
                          power::Switch_t pwrSwitch);
 
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
+  ModeTreeChildIF &getModeTreeChildIF() override;
+
  private:
   MessageQueueIF *queue = nullptr;
   PowerSwitcher switcher;
@@ -56,6 +63,10 @@ class PowerSwitcherComponent : public SystemObject,
 
   ReturnValue_t setHealth(HealthState health) override;
   HasHealthIF::HealthState getHealth() override;
+
+  object_id_t getObjectId() const override;
+  const HasHealthIF *getOptHealthIF() const override;
+  const HasModesIF &getModeIF() const override;
 };
 
 #endif /* _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ */

src/fsfw/pus/CMakeLists.txt

@@ -9,4 +9,4 @@ target_sources(
           Service17Test.cpp
           Service20ParameterManagement.cpp
           CService200ModeCommanding.cpp
-          CService201HealthCommanding.cpp)
+          CServiceHealthCommanding.cpp)

src/fsfw/pus/CService200ModeCommanding.cpp

@@ -20,6 +20,7 @@ ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) {
     case (Subservice::COMMAND_MODE_COMMAND):
     case (Subservice::COMMAND_MODE_READ):
     case (Subservice::COMMAND_MODE_ANNCOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY):
       return returnvalue::OK;
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
@@ -53,6 +54,8 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
                                                         const uint8_t *tcData, size_t tcDataLen,
                                                         uint32_t *state, object_id_t objectId) {
+  switch (subservice) {
+    case (Subservice::COMMAND_MODE_COMMAND): {
       ModePacket modeCommandPacket;
       ReturnValue_t result =
           modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
@@ -60,9 +63,27 @@ ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message,
         return result;
       }
 
-  ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
-                              modeCommandPacket.getSubmode());
-  return result;
+      ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND,
+                                  modeCommandPacket.getMode(), modeCommandPacket.getSubmode());
+      return returnvalue::OK;
+    }
+    case (Subservice::COMMAND_MODE_ANNCOUNCE):
+    case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): {
+      bool recursive = true;
+      if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
+        recursive = false;
+      }
+      ModeMessage::setModeAnnounceMessage(*message, recursive);
+      return EXECUTION_COMPLETE;
+    }
+    case (Subservice::COMMAND_MODE_READ): {
+      ModeMessage::setModeReadMessage(*message);
+      return returnvalue::OK;
+    }
+    default: {
+      return CommandingServiceBase::INVALID_SUBSERVICE;
+    }
+  }
 }
 
 ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply,
@@ -73,8 +94,10 @@ ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply
   ReturnValue_t result = returnvalue::FAILED;
   switch (replyId) {
     case (ModeMessage::REPLY_MODE_REPLY): {
-      result = prepareModeReply(reply, objectId);
-      break;
+      if (previousCommand != ModeMessage::CMD_MODE_COMMAND) {
+        return prepareModeReply(reply, objectId);
+      }
+      return returnvalue::OK;
     }
     case (ModeMessage::REPLY_WRONG_MODE_REPLY): {
       result = prepareWrongModeReply(reply, objectId);

src/fsfw/pus/CService201HealthCommanding.cpp

@@ -1,106 +0,0 @@
-#include "fsfw/pus/CService201HealthCommanding.h"
-
-#include "fsfw/health/HasHealthIF.h"
-#include "fsfw/health/HealthMessage.h"
-#include "fsfw/objectmanager/ObjectManager.h"
-#include "fsfw/pus/servicepackets/Service201Packets.h"
-#include "fsfw/serviceinterface/ServiceInterface.h"
-
-CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, uint16_t apid,
-                                                         uint8_t serviceId,
-                                                         uint8_t numParallelCommands,
-                                                         uint16_t commandTimeoutSeconds)
-    : CommandingServiceBase(objectId, apid, "PUS 201 Health MGMT", serviceId, numParallelCommands,
-                            commandTimeoutSeconds) {}
-
-ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) {
-  switch (subservice) {
-    case (Subservice::COMMAND_SET_HEALTH):
-    case (Subservice::COMMAND_ANNOUNCE_HEALTH):
-    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL):
-      return returnvalue::OK;
-    default:
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "Invalid Subservice" << std::endl;
-#endif
-      return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
-  }
-}
-
-ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(uint8_t subservice,
-                                                                    const uint8_t *tcData,
-                                                                    size_t tcDataLen,
-                                                                    MessageQueueId_t *id,
-                                                                    object_id_t *objectId) {
-  if (tcDataLen < sizeof(object_id_t)) {
-    return CommandingServiceBase::INVALID_TC;
-  }
-  SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
-
-  return checkInterfaceAndAcquireMessageQueue(id, objectId);
-}
-
-ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue(
-    MessageQueueId_t *messageQueueToSet, const object_id_t *objectId) {
-  auto *destination = ObjectManager::instance()->get<HasHealthIF>(*objectId);
-  if (destination == nullptr) {
-    return CommandingServiceBase::INVALID_OBJECT;
-  }
-
-  *messageQueueToSet = destination->getCommandQueue();
-  return returnvalue::OK;
-}
-
-ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *message,
-                                                          uint8_t subservice, const uint8_t *tcData,
-                                                          size_t tcDataLen, uint32_t *state,
-                                                          object_id_t objectId) {
-  ReturnValue_t result = returnvalue::OK;
-  switch (subservice) {
-    case (Subservice::COMMAND_SET_HEALTH): {
-      HealthSetCommand healthCommand;
-      result = healthCommand.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
-      if (result != returnvalue::OK) {
-        break;
-      }
-      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_SET,
-                                      healthCommand.getHealth());
-      break;
-    }
-    case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
-      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE);
-      break;
-    }
-    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
-      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE_ALL);
-      break;
-    }
-    default: {
-      // Should never happen, subservice was already checked
-      result = returnvalue::FAILED;
-    }
-  }
-  return result;
-}
-
-ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *reply,
-                                                       Command_t previousCommand, uint32_t *state,
-                                                       CommandMessage *optionalNextCommand,
-                                                       object_id_t objectId, bool *isStep) {
-  Command_t replyId = reply->getCommand();
-  if (replyId == HealthMessage::REPLY_HEALTH_SET) {
-    return EXECUTION_COMPLETE;
-  } else if (replyId == CommandMessageIF::REPLY_REJECTED) {
-    return reply->getReplyRejectedReason();
-  }
-  return CommandingServiceBase::INVALID_REPLY;
-}
-
-// Not used for now, health state already reported by event
-[[maybe_unused]] ReturnValue_t CService201HealthCommanding::prepareHealthSetReply(
-    const CommandMessage *reply) {
-  auto health = static_cast<uint8_t>(HealthMessage::getHealth(reply));
-  auto oldHealth = static_cast<uint8_t>(HealthMessage::getOldHealth(reply));
-  HealthSetReply healthSetReply(health, oldHealth);
-  return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply);
-}

src/fsfw/pus/CServiceHealthCommanding.cpp

@@ -0,0 +1,152 @@
+#include <fsfw/events/EventManagerIF.h>
+#include <fsfw/pus/CServiceHealthCommanding.h>
+
+#include "fsfw/health/HasHealthIF.h"
+#include "fsfw/health/HealthMessage.h"
+#include "fsfw/objectmanager/ObjectManager.h"
+#include "fsfw/pus/servicepackets/Service201Packets.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+
+CServiceHealthCommanding::CServiceHealthCommanding(HealthServiceCfg args)
+    : CommandingServiceBase(args.objectId, args.apid, "PUS 201 Health MGMT", args.service,
+                            args.numParallelCommands, args.commandTimeoutSeconds),
+      healthTable(args.table),
+      maxNumHealthInfoPerCycle(args.maxNumHealthInfoPerCycle) {}
+
+ReturnValue_t CServiceHealthCommanding::isValidSubservice(uint8_t subservice) {
+  switch (subservice) {
+    case (Subservice::COMMAND_SET_HEALTH):
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH):
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL):
+      return returnvalue::OK;
+    default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::error << "Invalid Subservice" << std::endl;
+#endif
+      return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+  }
+}
+
+ReturnValue_t CServiceHealthCommanding::getMessageQueueAndObject(uint8_t subservice,
+                                                                 const uint8_t *tcData,
+                                                                 size_t tcDataLen,
+                                                                 MessageQueueId_t *id,
+                                                                 object_id_t *objectId) {
+  switch (subservice) {
+    case (Subservice::COMMAND_SET_HEALTH):
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
+      if (tcDataLen < sizeof(object_id_t)) {
+        return CommandingServiceBase::INVALID_TC;
+      }
+      SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+
+      return checkInterfaceAndAcquireMessageQueue(id, objectId);
+    }
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
+      return returnvalue::OK;
+    }
+    default: {
+      return returnvalue::FAILED;
+    }
+  }
+}
+
+ReturnValue_t CServiceHealthCommanding::checkInterfaceAndAcquireMessageQueue(
+    MessageQueueId_t *messageQueueToSet, const object_id_t *objectId) {
+  auto *destination = ObjectManager::instance()->get<HasHealthIF>(*objectId);
+  if (destination == nullptr) {
+    return CommandingServiceBase::INVALID_OBJECT;
+  }
+
+  *messageQueueToSet = destination->getCommandQueue();
+  return returnvalue::OK;
+}
+
+ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
+                                                       const uint8_t *tcData, size_t tcDataLen,
+                                                       uint32_t *state, object_id_t objectId) {
+  ReturnValue_t result = returnvalue::OK;
+  switch (subservice) {
+    case (Subservice::COMMAND_SET_HEALTH): {
+      HealthSetCommand healthCommand;
+      result = healthCommand.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+      if (result != returnvalue::OK) {
+        break;
+      }
+      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_SET,
+                                      healthCommand.getHealth());
+      break;
+    }
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH): {
+      HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE);
+      break;
+    }
+    case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): {
+      ReturnValue_t result = iterateHealthTable(true);
+      if (result == returnvalue::OK) {
+        reportAllHealth = true;
+        return EXECUTION_COMPLETE;
+      }
+      return result;
+      while (true) {
+        ReturnValue_t result = iterateHealthTable(false);
+        if (result != returnvalue::OK) {
+          break;
+        }
+      }
+
+      return returnvalue::OK;
+    }
+    default: {
+      // Should never happen, subservice was already checked
+      result = returnvalue::FAILED;
+    }
+  }
+  return result;
+}
+
+ReturnValue_t CServiceHealthCommanding::handleReply(const CommandMessage *reply,
+                                                    Command_t previousCommand, uint32_t *state,
+                                                    CommandMessage *optionalNextCommand,
+                                                    object_id_t objectId, bool *isStep) {
+  Command_t replyId = reply->getCommand();
+  if (replyId == HealthMessage::REPLY_HEALTH_SET) {
+    return EXECUTION_COMPLETE;
+  } else if (replyId == CommandMessageIF::REPLY_REJECTED) {
+    return reply->getReplyRejectedReason();
+  }
+  return CommandingServiceBase::INVALID_REPLY;
+}
+
+void CServiceHealthCommanding::doPeriodicOperation() {
+  if (reportAllHealth) {
+    for (uint8_t i = 0; i < maxNumHealthInfoPerCycle; i++) {
+      ReturnValue_t result = iterateHealthTable(false);
+      if (result != returnvalue::OK) {
+        reportAllHealth = false;
+        break;
+      }
+    }
+  }
+}
+
+// Not used for now, health state already reported by event
+[[maybe_unused]] ReturnValue_t CServiceHealthCommanding::prepareHealthSetReply(
+    const CommandMessage *reply) {
+  auto health = static_cast<uint8_t>(HealthMessage::getHealth(reply));
+  auto oldHealth = static_cast<uint8_t>(HealthMessage::getOldHealth(reply));
+  HealthSetReply healthSetReply(health, oldHealth);
+  return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply);
+}
+
+ReturnValue_t CServiceHealthCommanding::iterateHealthTable(bool reset) {
+  std::pair<object_id_t, HasHealthIF::HealthState> pair;
+
+  ReturnValue_t result = healthTable.iterate(&pair, reset);
+  if (result != returnvalue::OK) {
+    return result;
+  } else {
+    EventManagerIF::triggerEvent(pair.first, HasHealthIF::HEALTH_INFO, pair.second, pair.second);
+    return returnvalue::OK;
+  }
+}

src/fsfw/pus/CServiceHealthCommanding.h

@@ -1,8 +1,26 @@
 #ifndef FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_
 #define FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_
 
+#include <fsfw/health/HealthTable.h>
+
 #include "fsfw/tmtcservices/CommandingServiceBase.h"
 
+struct HealthServiceCfg {
+  HealthServiceCfg(object_id_t objectId, uint16_t apid, HealthTable &healthTable,
+                   uint16_t maxNumHealthInfoPerCycle)
+      : objectId(objectId),
+        apid(apid),
+        table(healthTable),
+        maxNumHealthInfoPerCycle(maxNumHealthInfoPerCycle) {}
+  object_id_t objectId;
+  uint16_t apid;
+  HealthTable &table;
+  uint16_t maxNumHealthInfoPerCycle;
+  uint8_t service = 201;
+  uint8_t numParallelCommands = 4;
+  uint16_t commandTimeoutSeconds = 60;
+};
+
 /**
  * @brief   Custom PUS service to set health of all objects
  *          implementing hasHealthIF.
@@ -17,11 +35,10 @@
  * child class like this service
  *
  */
-class CService201HealthCommanding : public CommandingServiceBase {
+class CServiceHealthCommanding : public CommandingServiceBase {
  public:
-  CService201HealthCommanding(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                              uint8_t numParallelCommands = 4, uint16_t commandTimeoutSeconds = 60);
-  ~CService201HealthCommanding() override = default;
+  CServiceHealthCommanding(HealthServiceCfg args);
+  ~CServiceHealthCommanding() override = default;
 
  protected:
   /* CSB abstract function implementations */
@@ -37,7 +54,13 @@ class CService201HealthCommanding : public CommandingServiceBase {
                             CommandMessage *optionalNextCommand, object_id_t objectId,
                             bool *isStep) override;
 
+  void doPeriodicOperation() override;
+
  private:
+  HealthTable &healthTable;
+  uint16_t maxNumHealthInfoPerCycle = 0;
+  bool reportAllHealth = false;
+  ReturnValue_t iterateHealthTable(bool reset);
   static ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet,
                                                             const object_id_t *objectId);
 

src/fsfw/pus/Service11TelecommandScheduling.h

@@ -41,6 +41,8 @@ class Service11TelecommandScheduling final : public PusServiceBase {
   static constexpr ReturnValue_t INVALID_TIME_WINDOW = returnvalue::makeCode(CLASS_ID, 2);
   static constexpr ReturnValue_t TIMESHIFTING_NOT_POSSIBLE = returnvalue::makeCode(CLASS_ID, 3);
   static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 4);
+  static constexpr ReturnValue_t CONTAINED_TC_TOO_SMALL = returnvalue::makeCode(CLASS_ID, 5);
+  static constexpr ReturnValue_t CONTAINED_TC_CRC_MISSMATCH = returnvalue::makeCode(CLASS_ID, 6);
 
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11;
 

src/fsfw/pus/Service11TelecommandScheduling.tpp

@@ -2,9 +2,11 @@
 
 #include <cstddef>
 
+#include "fsfw/globalfunctions/CRC.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/serialize/SerializeAdapter.h"
 #include "fsfw/serviceinterface.h"
+#include "fsfw/tmtcpacket/pus/tc/PusTcIF.h"
 #include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
 
 static constexpr auto DEF_END = SerializeIF::Endianness::BIG;
@@ -171,6 +173,14 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
     return returnvalue::FAILED;
   }
 
+  if (size < PusTcIF::MIN_SIZE) {
+    return CONTAINED_TC_TOO_SMALL;
+  }
+
+  if (CRC::crc16ccitt(data, size) != 0) {
+    return CONTAINED_TC_CRC_MISSMATCH;
+  }
+
   // store currentPacket and receive the store address
   store_address_t addr{};
   if (tcStore->addData(&addr, data, size) != returnvalue::OK ||

src/fsfw/pus/Service17Test.cpp

@@ -1,5 +1,7 @@
 #include "fsfw/pus/Service17Test.h"
 
+#include <fsfw/serialize/SerializeElement.h>
+
 #include "fsfw/FSFW.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/objectmanager/SystemObject.h"
@@ -31,6 +33,15 @@ ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
       }
       return tmHelper.storeAndSendTmPacket();
     }
+    case Subservice::PING_WITH_DATA: {
+      SerializeElement<uint32_t> receivedDataLen = currentPacket.getUserDataLen();
+      ReturnValue_t result =
+          tmHelper.prepareTmPacket(Subservice::PING_WITH_DATA_REPORT_WITH_SIZE, receivedDataLen);
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      return tmHelper.storeAndSendTmPacket();
+    }
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
   }

src/fsfw/pus/Service17Test.h

@@ -32,6 +32,9 @@ class Service17Test : public PusServiceBase {
     CONNECTION_TEST_REPORT = 2,
     //!  [EXPORT] : [COMMAND] Trigger test reply and test event
     EVENT_TRIGGER_TEST = 128,
+    PING_WITH_DATA = 129,
+    //! [EXPORT] : [COMMAND] Report which reports the sent user data size
+    PING_WITH_DATA_REPORT_WITH_SIZE = 130
   };
 
   explicit Service17Test(PsbParams params);

src/fsfw/pus/Service20ParameterManagement.cpp

@@ -69,14 +69,14 @@ ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "Service20ParameterManagement::checkInterfaceAndAcquire"
                << "MessageQueue: Can't access object" << std::endl;
-    sif::error << "Object ID: " << std::hex << objectId << std::dec << std::endl;
-    sif::error << "Make sure it implements ReceivesParameterMessagesIF!" << std::endl;
+    sif::error << "Object ID: 0x" << std::hex << *objectId << std::dec << std::endl;
+    sif::error << "Make sure it implements ReceivesParameterMessagesIF" << std::endl;
 #else
     sif::printError(
         "Service20ParameterManagement::checkInterfaceAndAcquire"
         "MessageQueue: Can't access object\n");
     sif::printError("Object ID: 0x%08x\n", *objectId);
-    sif::printError("Make sure it implements ReceivesParameterMessagesIF!\n");
+    sif::printError("Make sure it implements ReceivesParameterMessagesIF\n");
 #endif
 
     return CommandingServiceBase::INVALID_OBJECT;

src/fsfw/pus/Service5EventReporting.cpp

@@ -15,8 +15,8 @@ Service5EventReporting::Service5EventReporting(PsbParams params, size_t maxNumbe
       maxNumberReportsPerCycle(maxNumberReportsPerCycle) {
   auto mqArgs = MqArgs(getObjectId(), static_cast<void*>(this));
   psbParams.name = "PUS 5 Event Reporting";
-  eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth,
-    MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
+  eventQueue = QueueFactory::instance()->createMessageQueue(
+      messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 Service5EventReporting::~Service5EventReporting() {

src/fsfw/pus/Service5EventReporting.h

@@ -42,7 +42,7 @@
 class Service5EventReporting : public PusServiceBase {
  public:
   Service5EventReporting(PsbParams params, size_t maxNumberReportsPerCycle = 10,
-                         uint32_t messageQueueDepth = 10);
+                         uint32_t messageQueueDepth = 20);
   ~Service5EventReporting() override;
 
   /***

src/fsfw/pus/Service9TimeManagement.cpp

@@ -1,5 +1,7 @@
 #include "fsfw/pus/Service9TimeManagement.h"
 
+#include <cmath>
+
 #include "fsfw/events/EventManagerIF.h"
 #include "fsfw/pus/servicepackets/Service9Packets.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
@@ -15,9 +17,17 @@ ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK;
 
 ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
   switch (subservice) {
-    case SUBSERVICE::SET_TIME: {
+    case Subservice::SET_TIME: {
       return setTime();
     }
+    case Subservice::DUMP_TIME: {
+      timeval newTime;
+      Clock::getClock_timeval(&newTime);
+      uint32_t subsecondMs =
+          static_cast<uint32_t>(std::floor(static_cast<double>(newTime.tv_usec) / 1000.0));
+      triggerEvent(CLOCK_DUMP, newTime.tv_sec, subsecondMs);
+      return returnvalue::OK;
+    }
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
   }
@@ -33,14 +43,14 @@ ReturnValue_t Service9TimeManagement::setTime() {
     return result;
   }
 
-  uint32_t formerUptime;
-  Clock::getUptime(&formerUptime);
+  timeval time;
+  Clock::getClock_timeval(&time);
   result = Clock::setClock(&timeToSet);
 
   if (result == returnvalue::OK) {
-    uint32_t newUptime;
-    Clock::getUptime(&newUptime);
-    triggerEvent(CLOCK_SET, newUptime, formerUptime);
+    timeval newTime;
+    Clock::getClock_timeval(&newTime);
+    triggerEvent(CLOCK_SET, time.tv_sec, newTime.tv_sec);
     return returnvalue::OK;
   } else {
     triggerEvent(CLOCK_SET_FAILURE, result, 0);

src/fsfw/pus/Service9TimeManagement.h

@@ -6,10 +6,13 @@
 class Service9TimeManagement : public PusServiceBase {
  public:
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9;
-  //!< Clock has been set. P1: New Uptime. P2: Old Uptime
+
+  //!< Clock has been set. P1: old timeval seconds. P2: new timeval seconds.
   static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO);
+  //!< Clock dump event. P1: timeval seconds P2: timeval milliseconds.
+  static constexpr Event CLOCK_DUMP = MAKE_EVENT(1, severity::INFO);
   //!< Clock could not be set. P1: Returncode.
-  static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(1, severity::LOW);
+  static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(2, severity::LOW);
 
   static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9;
 
@@ -30,8 +33,9 @@ class Service9TimeManagement : public PusServiceBase {
   virtual ReturnValue_t setTime();
 
  private:
-  enum SUBSERVICE {
-    SET_TIME = 128  //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
+  enum Subservice {
+    SET_TIME = 128,  //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
+    DUMP_TIME = 129,
   };
 };
 

src/fsfw/pus/servicepackets/Service200Packets.h

@@ -1,9 +1,9 @@
 #ifndef FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_
 #define FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_
 
-#include "../../modes/ModeMessage.h"
-#include "../../serialize/SerialLinkedListAdapter.h"
-#include "../../serialize/SerializeIF.h"
+#include "fsfw/modes/ModeMessage.h"
+#include "fsfw/serialize/SerialLinkedListAdapter.h"
+#include "fsfw/serialize/SerializeIF.h"
 
 /**
  * @brief Subservice 1, 2, 3, 4, 5

src/fsfw/storagemanager/LocalPool.cpp

@@ -31,9 +31,8 @@ LocalPool::LocalPool(object_id_t setObjectId, const LocalPoolConfig& poolConfig,
 
 LocalPool::~LocalPool() = default;
 
-ReturnValue_t LocalPool::addData(store_address_t* storageId, const uint8_t* data, size_t size,
-                                 bool ignoreFault) {
-  ReturnValue_t status = reserveSpace(size, storageId, ignoreFault);
+ReturnValue_t LocalPool::addData(store_address_t* storageId, const uint8_t* data, size_t size) {
+  ReturnValue_t status = reserveSpace(size, storageId);
   if (status == returnvalue::OK) {
     write(*storageId, data, size);
   }
@@ -49,8 +48,8 @@ ReturnValue_t LocalPool::getData(store_address_t packetId, const uint8_t** packe
 }
 
 ReturnValue_t LocalPool::getFreeElement(store_address_t* storageId, const size_t size,
-                                        uint8_t** pData, bool ignoreFault) {
-  ReturnValue_t status = reserveSpace(size, storageId, ignoreFault);
+                                        uint8_t** pData) {
+  ReturnValue_t status = reserveSpace(size, storageId);
   if (status == returnvalue::OK) {
     *pData = &store[storageId->poolIndex][getRawPosition(*storageId)];
   } else {
@@ -167,7 +166,7 @@ void LocalPool::clearStore() {
   }
 }
 
-ReturnValue_t LocalPool::reserveSpace(size_t size, store_address_t* storeId, bool ignoreFault) {
+ReturnValue_t LocalPool::reserveSpace(size_t size, store_address_t* storeId) {
   ReturnValue_t status = getSubPoolIndex(size, &storeId->poolIndex);
   if (status != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -318,27 +317,3 @@ bool LocalPool::hasDataAtId(store_address_t storeId) const {
   }
   return false;
 }
-
-ReturnValue_t LocalPool::getFreeElement(store_address_t* storeId, size_t size, uint8_t** pData) {
-  return StorageManagerIF::getFreeElement(storeId, size, pData);
-}
-
-ConstAccessorPair LocalPool::getData(store_address_t storeId) {
-  return StorageManagerIF::getData(storeId);
-}
-
-ReturnValue_t LocalPool::addData(store_address_t* storeId, const uint8_t* data, size_t size) {
-  return StorageManagerIF::addData(storeId, data, size);
-}
-
-ReturnValue_t LocalPool::getData(store_address_t storeId, ConstStorageAccessor& accessor) {
-  return StorageManagerIF::getData(storeId, accessor);
-}
-
-ReturnValue_t LocalPool::modifyData(store_address_t storeId, StorageAccessor& accessor) {
-  return StorageManagerIF::modifyData(storeId, accessor);
-}
-
-AccessorPair LocalPool::modifyData(store_address_t storeId) {
-  return StorageManagerIF::modifyData(storeId);
-}

src/fsfw/storagemanager/LocalPool.h

@@ -86,21 +86,13 @@ class LocalPool : public SystemObject, public StorageManagerIF {
   /**
    * Documentation: See StorageManagerIF.h
    */
-  ReturnValue_t addData(store_address_t* storeId, const uint8_t* data, size_t size,
-                        bool ignoreFault) override;
   ReturnValue_t addData(store_address_t* storeId, const uint8_t* data, size_t size) override;
 
   ReturnValue_t getFreeElement(store_address_t* storeId, size_t size, uint8_t** pData) override;
-  ReturnValue_t getFreeElement(store_address_t* storeId, size_t size, uint8_t** pData,
-                               bool ignoreFault) override;
 
-  ConstAccessorPair getData(store_address_t storeId) override;
-  ReturnValue_t getData(store_address_t storeId, ConstStorageAccessor& accessor) override;
   ReturnValue_t getData(store_address_t storeId, const uint8_t** packet_ptr, size_t* size) override;
 
-  AccessorPair modifyData(store_address_t storeId) override;
   ReturnValue_t modifyData(store_address_t storeId, uint8_t** packet_ptr, size_t* size) override;
-  ReturnValue_t modifyData(store_address_t storeId, StorageAccessor& accessor) override;
 
   ReturnValue_t deleteData(store_address_t storeId) override;
   ReturnValue_t deleteData(uint8_t* ptr, size_t size, store_address_t* storeId) override;
@@ -136,6 +128,12 @@ class LocalPool : public SystemObject, public StorageManagerIF {
   [[nodiscard]] max_subpools_t getNumberOfSubPools() const override;
   [[nodiscard]] bool hasDataAtId(store_address_t storeId) const override;
 
+  // Using functions provided by StorageManagerIF requires either a fully qualified path
+  // like for example localPool.StorageManagerIF::getFreeElement(...) or re-exporting
+  // the fully qualified path with the using directive.
+  using StorageManagerIF::getData;
+  using StorageManagerIF::modifyData;
+
  protected:
   /**
    * With this helper method, a free element of @c size is reserved.
@@ -144,7 +142,7 @@ class LocalPool : public SystemObject, public StorageManagerIF {
    * @return	- returnvalue::OK on success,
    * 			- the return codes of #getPoolIndex or #findEmpty otherwise.
    */
-  virtual ReturnValue_t reserveSpace(size_t size, store_address_t* address, bool ignoreFault);
+  virtual ReturnValue_t reserveSpace(size_t size, store_address_t* address);
 
  private:
   /**
@@ -188,6 +186,8 @@ class LocalPool : public SystemObject, public StorageManagerIF {
   std::vector<std::vector<size_type>> sizeLists =
       std::vector<std::vector<size_type>>(NUMBER_OF_SUBPOOLS);
 
+  bool ignoreFault = false;
+
   //! A variable to determine whether higher n pools are used if
   //! the store is full.
   bool spillsToHigherPools = false;

src/fsfw/storagemanager/PoolManager.cpp

@@ -9,10 +9,9 @@ PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPo
 
 PoolManager::~PoolManager() { MutexFactory::instance()->deleteMutex(mutex); }
 
-ReturnValue_t PoolManager::reserveSpace(const size_t size, store_address_t* address,
-                                        bool ignoreFault) {
+ReturnValue_t PoolManager::reserveSpace(const size_t size, store_address_t* address) {
   MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs);
-  ReturnValue_t status = LocalPool::reserveSpace(size, address, ignoreFault);
+  ReturnValue_t status = LocalPool::reserveSpace(size, address);
   return status;
 }
 

src/fsfw/storagemanager/PoolManager.h

@@ -57,7 +57,7 @@ class PoolManager : public LocalPool {
   //! Default mutex timeout value to prevent permanent blocking.
   uint32_t mutexTimeoutMs = 20;
 
-  ReturnValue_t reserveSpace(size_t size, store_address_t* address, bool ignoreFault) override;
+  ReturnValue_t reserveSpace(size_t size, store_address_t* address) override;
 
   /**
    * @brief	The mutex is created in the constructor and makes

src/fsfw/storagemanager/StorageManagerIF.h

@@ -55,7 +55,7 @@ class StorageManagerIF {
   /**
    * @brief This is the empty virtual destructor as required for C++ interfaces.
    */
-  ~StorageManagerIF() = default;
+  virtual ~StorageManagerIF() = default;
   /**
    * @brief	With addData, a free storage position is allocated and data
    * 			stored there.
@@ -66,12 +66,7 @@ class StorageManagerIF {
    * @return	Returns @returnvalue::OK if data was added.
    * 		@returnvalue::FAILED if data could not be added, storageId is unchanged then.
    */
-  virtual ReturnValue_t addData(store_address_t* storageId, const uint8_t* data, size_t size,
-                                bool ignoreFault) = 0;
-
-  virtual ReturnValue_t addData(store_address_t* storageId, const uint8_t* data, size_t size) {
-    return addData(storageId, data, size, false);
-  }
+  virtual ReturnValue_t addData(store_address_t* storageId, const uint8_t* data, size_t size) = 0;
 
   /**
    * @brief	With deleteData, the storageManager frees the memory region
@@ -186,12 +181,8 @@ class StorageManagerIF {
    * @return	Returns @returnvalue::OK if data was added.
    * 		@returnvalue::FAILED if data could not be added, storageId is unchanged then.
    */
-  virtual ReturnValue_t getFreeElement(store_address_t* storageId, size_t size, uint8_t** dataPtr,
-                                       bool ignoreFault) = 0;
-
-  virtual ReturnValue_t getFreeElement(store_address_t* storageId, size_t size, uint8_t** dataPtr) {
-    return getFreeElement(storageId, size, dataPtr, false);
-  }
+  virtual ReturnValue_t getFreeElement(store_address_t* storageId, size_t size,
+                                       uint8_t** dataPtr) = 0;
 
   [[nodiscard]] virtual bool hasDataAtId(store_address_t storeId) const = 0;
 

src/fsfw/subsystem/CMakeLists.txt

@@ -1,3 +1,4 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp)
+target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp
+                                        helper.cpp)
 
 add_subdirectory(modes)

src/fsfw/subsystem/HasModeTreeChildrenIF.h

@@ -0,0 +1,13 @@
+#ifndef FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_
+#define FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_
+
+#include "ModeTreeChildIF.h"
+
+class HasModeTreeChildrenIF {
+ public:
+  virtual ~HasModeTreeChildrenIF() = default;
+  virtual ReturnValue_t registerChild(const ModeTreeChildIF& child) = 0;
+  virtual MessageQueueId_t getCommandQueue() const = 0;
+};
+
+#endif  // FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_

src/fsfw/subsystem/ModeTreeChildIF.h

@@ -0,0 +1,15 @@
+#ifndef FSFW_SUBSYSTEM_MODETREECHILDIF_H_
+#define FSFW_SUBSYSTEM_MODETREECHILDIF_H_
+
+#include <fsfw/health/HasHealthIF.h>
+#include <fsfw/modes/HasModesIF.h>
+
+class ModeTreeChildIF {
+ public:
+  virtual ~ModeTreeChildIF() = default;
+  virtual object_id_t getObjectId() const = 0;
+  virtual const HasHealthIF* getOptHealthIF() const = 0;
+  virtual const HasModesIF& getModeIF() const = 0;
+};
+
+#endif /* FSFW_SUBSYSTEM_MODETREECHILDIF_H_ */

src/fsfw/subsystem/ModeTreeConnectionIF.h

@@ -0,0 +1,13 @@
+#ifndef FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_
+#define FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_
+
+#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
+
+class ModeTreeConnectionIF {
+ public:
+  virtual ~ModeTreeConnectionIF() = default;
+  virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) = 0;
+  virtual ModeTreeChildIF& getModeTreeChildIF() = 0;
+};
+
+#endif /* FSFW_SRC_FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_ */

src/fsfw/subsystem/Subsystem.cpp

@@ -9,9 +9,9 @@
 #include "fsfw/serialize/SerialLinkedListAdapter.h"
 #include "fsfw/serialize/SerializeElement.h"
 
-Subsystem::Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
+Subsystem::Subsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
                      uint32_t maxNumberOfTables)
-    : SubsystemBase(setObjectId, parent, 0),
+    : SubsystemBase(setObjectId, 0),
       isInTransition(false),
       childrenChangedHealth(false),
       currentTargetTable(),
@@ -36,6 +36,13 @@ ReturnValue_t Subsystem::checkSequence(HybridIterator<ModeListEntry> iter,
 
   for (; iter.value != nullptr; ++iter) {
     if (!existsModeTable(iter->getTableId())) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      using namespace std;
+      sif::warning << "Subsystem::checkSequence: "
+                   << "Object " << setfill('0') << hex << "0x" << setw(8) << getObjectId()
+                   << setw(0) << ": Mode table for mode ID "
+                   << "0x" << setw(8) << iter->getTableId() << " does not exist" << dec << endl;
+#endif
       return TABLE_DOES_NOT_EXIST;
     } else {
       ReturnValue_t result = checkTable(getTable(iter->getTableId()));

src/fsfw/subsystem/Subsystem.h

@@ -66,8 +66,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF {
    * @param maxNumberOfSequences
    * @param maxNumberOfTables
    */
-  Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences,
-            uint32_t maxNumberOfTables);
+  Subsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
   virtual ~Subsystem();
 
   ReturnValue_t addSequence(SequenceEntry sequence);

src/fsfw/subsystem/SubsystemBase.cpp

@@ -1,16 +1,17 @@
 #include "fsfw/subsystem/SubsystemBase.h"
 
+#include "fsfw/FSFW.h"
 #include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/objectmanager/ObjectManager.h"
-#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/serviceinterface.h"
+#include "fsfw/subsystem/helper.h"
 
-SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode,
+SubsystemBase::SubsystemBase(object_id_t setObjectId, Mode_t initialMode,
                              uint16_t commandQueueDepth)
     : SystemObject(setObjectId),
       mode(initialMode),
       healthHelper(this, setObjectId),
-      modeHelper(this),
-      parentId(parent) {
+      modeHelper(this) {
   auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
   commandQueue = QueueFactory::instance()->createMessageQueue(
       commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE, &mqArgs);
@@ -18,36 +19,6 @@ SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t
 
 SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
 
-ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
-  ChildInfo info;
-
-  HasModesIF* child = ObjectManager::instance()->get<HasModesIF>(objectId);
-  // This is a rather ugly hack to have the changedHealth info for all
-  // children available.
-  HasHealthIF* healthChild = ObjectManager::instance()->get<HasHealthIF>(objectId);
-  if (child == nullptr) {
-    if (healthChild == nullptr) {
-      return CHILD_DOESNT_HAVE_MODES;
-    } else {
-      info.commandQueue = healthChild->getCommandQueue();
-      info.mode = MODE_OFF;
-    }
-  } else {
-    // intentional to force an initial command during system startup
-    info.commandQueue = child->getCommandQueue();
-    info.mode = HasModesIF::MODE_UNDEFINED;
-  }
-
-  info.submode = SUBMODE_NONE;
-  info.healthChanged = false;
-
-  auto resultPair = childrenMap.emplace(objectId, info);
-  if (not resultPair.second) {
-    return COULD_NOT_INSERT_CHILD;
-  }
-  return returnvalue::OK;
-}
-
 ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator<ModeListEntry> tableIter,
                                                     Submode_t targetSubmode) {
   std::map<object_id_t, ChildInfo>::iterator childIter;
@@ -87,7 +58,8 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
     if ((iter = childrenMap.find(object)) == childrenMap.end()) {
       // illegal table entry, should only happen due to misconfigured mode table
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::debug << std::hex << getObjectId() << ": invalid mode table entry" << std::endl;
+      sif::debug << std::hex << SystemObject::getObjectId() << ": invalid mode table entry"
+                 << std::endl;
 #endif
       continue;
     }
@@ -158,36 +130,15 @@ ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bo
 MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->getId(); }
 
 ReturnValue_t SubsystemBase::initialize() {
-  MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE;
-  ReturnValue_t result = SystemObject::initialize();
-
+  ReturnValue_t result = modeHelper.initialize();
   if (result != returnvalue::OK) {
     return result;
   }
-
-  if (parentId != objects::NO_OBJECT) {
-    SubsystemBase* parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
-    if (parent == nullptr) {
-      return returnvalue::FAILED;
-    }
-    parentQueue = parent->getCommandQueue();
-
-    parent->registerChild(getObjectId());
-  }
-
-  result = healthHelper.initialize(parentQueue);
-
+  result = healthHelper.initialize();
   if (result != returnvalue::OK) {
     return result;
   }
-
-  result = modeHelper.initialize(parentQueue);
-
-  if (result != returnvalue::OK) {
-    return result;
-  }
-
-  return returnvalue::OK;
+  return SystemObject::initialize();
 }
 
 ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) {
@@ -240,8 +191,14 @@ ReturnValue_t SubsystemBase::handleModeReply(CommandMessage* message) {
 }
 
 ReturnValue_t SubsystemBase::checkTable(HybridIterator<ModeListEntry> tableIter) {
-  for (; tableIter.value != NULL; ++tableIter) {
+  for (; tableIter.value != nullptr; ++tableIter) {
     if (childrenMap.find(tableIter.value->getObject()) == childrenMap.end()) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      using namespace std;
+      sif::warning << "SubsystemBase::checkTable: Could not find object " << setfill('0') << hex
+                   << "0x" << setw(8) << tableIter.value->getObject() << " in object " << setw(8)
+                   << setw(0) << "0x" << setw(8) << SystemObject::getObjectId() << dec << std::endl;
+#endif
       return TABLE_CONTAINS_INVALID_OBJECT_ID;
     }
   }
@@ -326,4 +283,33 @@ ReturnValue_t SubsystemBase::setHealth(HealthState health) {
 
 HasHealthIF::HealthState SubsystemBase::getHealth() { return healthHelper.getHealth(); }
 
+ReturnValue_t SubsystemBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
+}
+
+object_id_t SubsystemBase::getObjectId() const { return SystemObject::getObjectId(); }
+
 void SubsystemBase::modeChanged() {}
+
+ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) {
+  ChildInfo info;
+
+  const HasModesIF& modeChild = child.getModeIF();
+  // intentional to force an initial command during system startup
+  info.commandQueue = modeChild.getCommandQueue();
+  info.mode = HasModesIF::MODE_UNDEFINED;
+  info.submode = SUBMODE_NONE;
+  info.healthChanged = false;
+
+  auto resultPair = childrenMap.emplace(child.getObjectId(), info);
+  if (not resultPair.second) {
+    return COULD_NOT_INSERT_CHILD;
+  }
+  return returnvalue::OK;
+}
+
+const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; }
+
+const HasModesIF& SubsystemBase::getModeIF() const { return *this; }
+
+ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { return *this; }

src/fsfw/subsystem/SubsystemBase.h

@@ -3,14 +3,16 @@
 
 #include <map>
 
-#include "../container/HybridIterator.h"
-#include "../health/HasHealthIF.h"
-#include "../health/HealthHelper.h"
-#include "../ipc/MessageQueueIF.h"
-#include "../modes/HasModesIF.h"
-#include "../objectmanager/SystemObject.h"
-#include "../returnvalues/returnvalue.h"
-#include "../tasks/ExecutableObjectIF.h"
+#include "fsfw/container/HybridIterator.h"
+#include "fsfw/health/HasHealthIF.h"
+#include "fsfw/health/HealthHelper.h"
+#include "fsfw/ipc/MessageQueueIF.h"
+#include "fsfw/modes/HasModesIF.h"
+#include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/returnvalues/returnvalue.h"
+#include "fsfw/subsystem/HasModeTreeChildrenIF.h"
+#include "fsfw/subsystem/ModeTreeConnectionIF.h"
+#include "fsfw/tasks/ExecutableObjectIF.h"
 #include "modes/HasModeSequenceIF.h"
 
 /**
@@ -27,6 +29,9 @@
 class SubsystemBase : public SystemObject,
                       public HasModesIF,
                       public HasHealthIF,
+                      public HasModeTreeChildrenIF,
+                      public ModeTreeConnectionIF,
+                      public ModeTreeChildIF,
                       public ExecutableObjectIF {
  public:
   static const uint8_t INTERFACE_ID = CLASS_ID::SUBSYSTEM_BASE;
@@ -36,12 +41,14 @@ class SubsystemBase : public SystemObject,
   static const ReturnValue_t COULD_NOT_INSERT_CHILD = MAKE_RETURN_CODE(0x04);
   static const ReturnValue_t TABLE_CONTAINS_INVALID_OBJECT_ID = MAKE_RETURN_CODE(0x05);
 
-  SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode = 0,
-                uint16_t commandQueueDepth = 8);
+  SubsystemBase(object_id_t setObjectId, Mode_t initialMode = 0, uint16_t commandQueueDepth = 8);
   virtual ~SubsystemBase();
 
   virtual MessageQueueId_t getCommandQueue() const override;
 
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override;
+  ModeTreeChildIF &getModeTreeChildIF() override;
+
   /**
    * Function to register the child objects.
    * Performs a checks if the child does implement HasHealthIF and/or HasModesIF
@@ -53,15 +60,15 @@ class SubsystemBase : public SystemObject,
    *           CHILD_DOESNT_HAVE_MODES if Child is no HasHealthIF and no HasModesIF
    *           COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap
    */
-  ReturnValue_t registerChild(object_id_t objectId);
+  ReturnValue_t registerChild(const ModeTreeChildIF &child) override;
 
-  virtual ReturnValue_t initialize() override;
+  ReturnValue_t initialize() override;
 
-  virtual ReturnValue_t performOperation(uint8_t opCode) override;
+  ReturnValue_t performOperation(uint8_t opCode) override;
 
-  virtual ReturnValue_t setHealth(HealthState health) override;
+  ReturnValue_t setHealth(HealthState health) override;
 
-  virtual HasHealthIF::HealthState getHealth() override;
+  HasHealthIF::HealthState getHealth() override;
 
  protected:
   struct ChildInfo {
@@ -88,8 +95,6 @@ class SubsystemBase : public SystemObject,
 
   ModeHelper modeHelper;
 
-  const object_id_t parentId;
-
   typedef std::map<object_id_t, ChildInfo> ChildrenMap;
   ChildrenMap childrenMap;
 
@@ -136,6 +141,10 @@ class SubsystemBase : public SystemObject,
 
   virtual void getMode(Mode_t *mode, Submode_t *submode) override;
 
+  object_id_t getObjectId() const override;
+  const HasHealthIF *getOptHealthIF() const override;
+  const HasModesIF &getModeIF() const override;
+
   virtual void setToExternalControl() override;
 
   virtual void announceMode(bool recursive) override;

src/fsfw/subsystem/helper.cpp

@@ -0,0 +1,15 @@
+#include "helper.h"
+
+ReturnValue_t modetree::connectModeTreeParent(HasModeTreeChildrenIF& parent,
+                                              const ModeTreeChildIF& child,
+                                              HealthHelper* healthHelper, ModeHelper& modeHelper) {
+  ReturnValue_t result = parent.registerChild(child);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  if (healthHelper != nullptr) {
+    healthHelper->setParentQueue(parent.getCommandQueue());
+  }
+  modeHelper.setParentQueue(parent.getCommandQueue());
+  return returnvalue::OK;
+}

src/fsfw/subsystem/helper.h

@@ -0,0 +1,14 @@
+#ifndef FSFW_SUBSYSTEM_HELPER_H_
+#define FSFW_SUBSYSTEM_HELPER_H_
+
+#include "HasModeTreeChildrenIF.h"
+#include "fsfw/health/HealthHelper.h"
+
+namespace modetree {
+
+ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent, const ModeTreeChildIF& child,
+                                    HealthHelper* healthHelper, ModeHelper& modeHelper);
+
+}
+
+#endif /* FSFW_SRC_FSFW_SUBSYSTEM_HELPER_H_ */

src/fsfw/timemanager/TimeReaderIF.h

@@ -1,6 +1,7 @@
 #ifndef FSFW_TIMEMANAGER_TIMEREADERIF_H
 #define FSFW_TIMEMANAGER_TIMEREADERIF_H
 
+#include <cstdio>
 #include <cstdlib>
 
 #include "fsfw/platform.h"

src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp

@@ -14,6 +14,7 @@ SpacePacketCreator::SpacePacketCreator(ccsds::PacketType packetType, bool secHea
     : params(SpacePacketParams(PacketId(packetType, secHeaderFlag, apid),
                                PacketSeqCtrl(seqFlags, seqCount), dataLen)) {
   params.version = version;
+  checkFieldValidity();
 }
 
 uint16_t SpacePacketCreator::getPacketIdRaw() const { return params.packetId.raw(); }

src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp

@@ -21,7 +21,7 @@ SpacePacketReader::~SpacePacketReader() = default;
 
 inline uint16_t SpacePacketReader::getPacketIdRaw() const { return ccsds::getPacketId(*spHeader); }
 
-const uint8_t* SpacePacketReader::getPacketData() { return packetDataField; }
+const uint8_t* SpacePacketReader::getPacketData() const { return packetDataField; }
 
 ReturnValue_t SpacePacketReader::setData(uint8_t* data, size_t maxSize_, void* args) {
   return setInternalFields(data, maxSize_);

src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h

@@ -71,7 +71,7 @@ class SpacePacketReader : public SpacePacketIF,
   // Helper methods:
   [[nodiscard]] ReturnValue_t checkSize() const;
 
-  const uint8_t* getPacketData();
+  const uint8_t* getPacketData() const;
 
   ReturnValue_t setReadOnlyData(const uint8_t* data, size_t maxSize);
 

src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp

@@ -100,5 +100,6 @@ ReturnValue_t PusTcCreator::setSerializableUserData(const SerializeIF &serializa
 void PusTcCreator::setup() {
   spCreator.setPacketType(ccsds::PacketType::TC);
   spCreator.setSecHeaderFlag();
+  spCreator.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED);
   updateSpLengthField();
 }

src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp

@@ -119,6 +119,7 @@ void PusTmCreator::setup() {
   updateSpLengthField();
   spCreator.setPacketType(ccsds::PacketType::TM);
   spCreator.setSecHeaderFlag();
+  spCreator.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED);
 }
 
 void PusTmCreator::setMessageTypeCounter(uint16_t messageTypeCounter) {

src/fsfw/tmtcservices/AcceptsTelemetryIF.h

@@ -21,9 +21,11 @@ class AcceptsTelemetryIF {
    * 			receiving message queue.
    * @return	The telemetry reception message queue id.
    */
-  virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) = 0;
+  [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const = 0;
 
-  virtual MessageQueueId_t getReportReceptionQueue() { return getReportReceptionQueue(0); }
+  [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const {
+    return getReportReceptionQueue(0);
+  }
 };
 
 #endif /* FSFW_TMTCSERVICES_ACCEPTSTELEMETRYIF_H_ */

src/fsfw/tmtcservices/PusServiceBase.cpp

@@ -27,8 +27,8 @@ ReturnValue_t PusServiceBase::performOperation(uint8_t opCode) {
   ReturnValue_t result = performService();
   if (result != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "PusService " << psbParams.serviceId << ": performService returned with "
-               << static_cast<uint16_t>(result) << std::endl;
+    sif::error << "PusService " << static_cast<int>(psbParams.serviceId)
+               << ": performService returned with " << static_cast<uint16_t>(result) << std::endl;
 #endif
     return returnvalue::FAILED;
   }

src/fsfw/tmtcservices/SourceSequenceCounter.h

@@ -11,7 +11,7 @@ class SourceSequenceCounter {
   SourceSequenceCounter(uint16_t initialSequenceCount = 0) : sequenceCount(initialSequenceCount) {}
   void increment() { sequenceCount = (sequenceCount + 1) % (ccsds::LIMIT_SEQUENCE_COUNT); }
   void decrement() { sequenceCount = (sequenceCount - 1) % (ccsds::LIMIT_SEQUENCE_COUNT); }
-  uint16_t get() { return this->sequenceCount; }
+  uint16_t get() const { return this->sequenceCount; }
   void reset(uint16_t toValue = 0) { sequenceCount = toValue % (ccsds::LIMIT_SEQUENCE_COUNT); }
   SourceSequenceCounter& operator++(int) {
     this->increment();

src/fsfw/tmtcservices/TmTcBridge.cpp

@@ -145,13 +145,17 @@ ReturnValue_t TmTcBridge::handleTmQueue() {
 #endif /* FSFW_VERBOSE_LEVEL >= 3 */
 
     if (communicationLinkUp == false or packetSentCounter >= sentPacketsPerCycle) {
-      storeDownlinkData(&message);
+      ReturnValue_t result = storeDownlinkData(&message);
+      if (result != returnvalue::OK) {
+        tmStore->deleteData(message.getStorageId());
+      }
       continue;
     }
 
     result = tmStore->getData(message.getStorageId(), &data, &size);
     if (result != returnvalue::OK) {
       status = result;
+      tmStore->deleteData(message.getStorageId());
       continue;
     }
 
@@ -159,9 +163,9 @@ ReturnValue_t TmTcBridge::handleTmQueue() {
     if (result != returnvalue::OK) {
       status = result;
     } else {
-      tmStore->deleteData(message.getStorageId());
       packetSentCounter++;
     }
+    tmStore->deleteData(message.getStorageId());
   }
   return status;
 }
@@ -183,7 +187,7 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) {
           "TmTcBridge::storeDownlinkData: TM downlink max. number "
           "of stored packet IDs reached!\n");
 #endif
-      warningSwitch = true;
+      warningSwitch = false;
     }
     if (overwriteOld) {
       tmFifo->retrieve(&storeId);
@@ -226,6 +230,7 @@ ReturnValue_t TmTcBridge::handleStoredTm() {
     packetSentCounter++;
 
     if (tmFifo->empty()) {
+      warningSwitch = true;
       tmStored = false;
     }
     tmStore->deleteData(storeId);
@@ -245,7 +250,7 @@ void TmTcBridge::registerCommDisconnect() {
   }
 }
 
-MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) {
+MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) const {
   return tmTcReceptionQueue->getId();
 }