Merge pull request 'EIVE System' (#376) from eive_system into develop

Reviewed-on: #376
2023-02-13 09:38:53 +01:00 · 2023-02-13 09:38:53 +01:00 · f81984235b
commit f81984235b
parent b3202623f4 3759b5d34a
41 changed files with 501 additions and 113 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -17,6 +17,42 @@ change warranting a new major release:

 # [unreleased]

+# [v1.27.0] 2023-02-13
+
+Added EIVE system top mode component. Currently, only SAFE and IDLE mode are
+implemented, and the system does not do more than commanding TCS and ACS
+into the correct modes. It does not have a lot of mode tracking capabilities
+yet because the ACS controller might alternate between SAFE and DETUMBLE.
+It takes around 5-10 seconds for the EIVE system to reach the SAFE mode.
+
+The new system is used at software boot to command the satellite into safe mode
+on each reboot. This behaviour can be disabled with the
+`OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP` flag.
+
+## Added
+
+- New EIVE system component like explained above.
+
+## Changed
+
+- The satellite now commands itself into SAFE mode on each reboot, which
+  triggers a lot of events on each SW reboot. The TCS subsystem will commanded
+  to NORMAL mode immediately while the ACS subsystem will be commanded to
+  SAFE mode. The payload subsystem will be commanded OFF.
+- `RELEASE_BUILD` flag moved to `commonConfig.h`
+- The ACS subsystem transitions are now staggered: The SUS board assembly
+  is commanded as a separate transition. This reduces the risk of long bus lockups.
+- No INFO mode event translations for release builds to reduce number of
+  printouts.
+- More granular locking inside the MAX31865 low level read handler.
+
+## Fixed
+
+- More DHB thermal module fixes.
+- ACS PST frequency extended to 0.8 seconds in debug builds to avoid SPI
+  bus lockups.
+- Local datapool fixes for the `PlocSupervisorHandler`
+
 # [v1.26.4] 2023-02-10

 eive-tmtc: v2.12.3
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -307,6 +307,11 @@ endif()
 include(BuildType)
 set_build_type()

+set(FSFW_DEBUG_INFO 0)
+if(RELEASE_BUILD MATCHES 0)
+  set(FSFW_DEBUG_INFO 1)
+endif()
+
 # Configuration files
 configure_file(${COMMON_CONFIG_PATH}/commonConfig.h.in commonConfig.h)
 configure_file(${FSFW_CONFIG_PATH}/FSFWConfig.h.in FSFWConfig.h)
--- a/bsp_q7s/OBSWConfig.h.in
+++ b/bsp_q7s/OBSWConfig.h.in
@ -9,12 +9,16 @@
 #include "commonConfig.h"
 #include "q7sConfig.h"

-#cmakedefine RELEASE_BUILD
-
 /*******************************************************************/
 /** All of the following flags should be enabled for mission code  */
 /*******************************************************************/

+#define OBSW_ENABLE_PERIODIC_HK                     0
+#define OBSW_ENABLE_SYRLINKS_TRANSMIT_TIMEOUT       0
+// This switch will cause the SW to command the EIVE system object to safe mode. This will
+// trigger a lot of events, so it can make sense to disable this for debugging purposes
+#define OBSW_COMMAND_SAFE_MODE_AT_STARTUP           1
+
 #define OBSW_ADD_GOMSPACE_PCDU                      @OBSW_ADD_GOMSPACE_PCDU@
 #define OBSW_ADD_MGT                                @OBSW_ADD_MGT@
 #define OBSW_ADD_BPX_BATTERY_HANDLER                @OBSW_ADD_BPX_BATTERY_HANDLER@
@ -41,25 +45,23 @@
 #define OBSW_TM_TO_PTME                             @OBSW_TM_TO_PTME@
 // Set to 1 if telecommands are received via the PDEC IP Core
 #define OBSW_TC_FROM_PDEC                           @OBSW_TC_FROM_PDEC@
-#define OBSW_ENABLE_SYRLINKS_TRANSMIT_TIMEOUT       0

 // Configuration parameter which causes the core controller to try to keep at least one SD card
 // working
 #define OBSW_SD_CARD_MUST_BE_ON                     1
 #define OBSW_ENABLE_TIMERS                          1

-// This is a really tricky switch.. It initializes the PCDU switches to their default states
-// at powerup. I think it would be better
-// to leave it off for now. It makes testing a lot more difficult and it might mess with
-// something the operators might want to do by giving the software too much intelligence
-// at the wrong place. The system component might command all the Switches accordingly anyway
-#define OBSW_INITIALIZE_SWITCHES                    0
-#define OBSW_ENABLE_PERIODIC_HK                     0
-
 /*******************************************************************/
 /** All of the following flags should be disabled for mission code */
 /*******************************************************************/

+// Use TCP instead of UDP for the TMTC bridge. This allows using the TMTC client locally
+// because UDP packets are not allowed in the VPN
+// This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the
+// CCSDS IP Cores. 
+#define OBSW_ADD_TMTC_TCP_SERVER                    1
+#define OBSW_ADD_TMTC_UDP_SERVER                    1
+
 // Can be used to switch device to NORMAL mode immediately
 #define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP    0
 #define OBSW_PRINT_MISSED_DEADLINES                 1
@ -121,13 +123,6 @@
 /** CMake Defines                                                  */
 /*******************************************************************/

-// Use TCP instead of UDP for the TMTC bridge. This allows using the TMTC client locally
-// because UDP packets are not allowed in the VPN
-// This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the
-// CCSDS IP Cores. 
-#define OBSW_ADD_TMTC_TCP_SERVER            1
-#define OBSW_ADD_TMTC_UDP_SERVER            1
-
 #cmakedefine EIVE_BUILD_GPSD_GPS_HANDLER

 #cmakedefine LIBGPS_VERSION_MAJOR @LIBGPS_VERSION_MAJOR@
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@ -347,6 +347,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  SpiCookie* spiCookie =
      new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE,
                    spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
  auto mgmLis3Handler0 = new MgmLIS3MDLHandler(
      objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY);
  fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER);
@ -362,6 +363,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  spiCookie =
      new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, RM3100::MAX_BUFFER_SIZE,
                    spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
  auto mgmRm3100Handler1 =
      new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
                           spi::RM3100_TRANSITION_DELAY);
@ -378,6 +380,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  spiCookie =
      new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE,
                    spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
  auto* mgmLis3Handler2 = new MgmLIS3MDLHandler(
      objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY);
  fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER);
@ -393,6 +396,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  spiCookie =
      new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, RM3100::MAX_BUFFER_SIZE,
                    spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
  auto* mgmRm3100Handler3 =
      new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
                           spi::RM3100_TRANSITION_DELAY);
@ -411,6 +415,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  spiCookie =
      new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE,
                    spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
  auto adisHandler =
      new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
                               ADIS1650X::Type::ADIS16505);
@ -427,6 +432,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  // Gyro 1 Side A
  spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, L3GD20H::MAX_BUFFER_SIZE,
                            spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
  auto gyroL3gHandler1 = new GyroHandlerL3GD20H(
      objects::GYRO_1_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY);
  fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER);
@ -443,6 +449,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  spiCookie =
      new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE,
                    spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
  adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_MAIN_COM_IF,
                                         spiCookie, ADIS1650X::Type::ADIS16505);
  fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER);
@ -455,6 +462,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  // Gyro 3 Side B
  spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, L3GD20H::MAX_BUFFER_SIZE,
                            spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
  auto gyroL3gHandler3 = new GyroHandlerL3GD20H(
      objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY);
  fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER);
--- a/bsp_q7s/core/scheduling.cpp
+++ b/bsp_q7s/core/scheduling.cpp
@ -80,16 +80,12 @@ void scheduling::initTasks() {
  }
 #endif

-  PeriodicTaskIF* sysTask = factory->createPeriodicTask(
+  PeriodicTaskIF* coreCtrlTask = factory->createPeriodicTask(
      "CORE_CTRL", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, missedDeadlineFunc);
-  result = sysTask->addComponent(objects::CORE_CONTROLLER);
+  result = coreCtrlTask->addComponent(objects::CORE_CONTROLLER);
  if (result != returnvalue::OK) {
    scheduling::printAddObjectError("CORE_CTRL", objects::CORE_CONTROLLER);
  }
-  result = sysTask->addComponent(objects::PL_SUBSYSTEM);
-  if (result != returnvalue::OK) {
-    scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM);
-  }

  /* TMTC Distribution */
  PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask(
@ -144,15 +140,23 @@ void scheduling::initTasks() {
 #endif
 #endif

-  PeriodicTaskIF* comTask = factory->createPeriodicTask(
-      "COM_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc);
-  result = comTask->addComponent(objects::COM_SUBSYSTEM);
+  PeriodicTaskIF* genericSysTask = factory->createPeriodicTask(
+      "SYSTEM_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.5, missedDeadlineFunc);
+  result = genericSysTask->addComponent(objects::EIVE_SYSTEM);
  if (result != returnvalue::OK) {
-    scheduling::printAddObjectError("COM subsystem", objects::COM_SUBSYSTEM);
+    scheduling::printAddObjectError("EIVE_SYSTEM", objects::EIVE_SYSTEM);
+  }
+  result = genericSysTask->addComponent(objects::COM_SUBSYSTEM);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("COM_SUBSYSTEM", objects::COM_SUBSYSTEM);
+  }
+  result = genericSysTask->addComponent(objects::PL_SUBSYSTEM);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM);
  }

 #if OBSW_ADD_CCSDS_IP_CORES == 1
-  result = comTask->addComponent(objects::CCSDS_HANDLER);
+  result = genericSysTask->addComponent(objects::CCSDS_HANDLER);
  if (result != returnvalue::OK) {
    scheduling::printAddObjectError("CCSDS Handler", objects::CCSDS_HANDLER);
  }
@ -185,8 +189,12 @@ void scheduling::initTasks() {
 #endif /* OBSW_ADD_GPS_CTRL */

  PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
-      "SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+      "ACS_SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
  static_cast<void>(acsSysTask);
+  result = acsSysTask->addComponent(objects::ACS_SUBSYSTEM);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM);
+  }
 #if OBSW_ADD_ACS_BOARD == 1
  result = acsSysTask->addComponent(objects::ACS_BOARD_ASS);
  if (result != returnvalue::OK) {
@ -205,10 +213,6 @@ void scheduling::initTasks() {
    scheduling::printAddObjectError("SUS_BOARD_ASS", objects::SUS_BOARD_ASS);
  }
 #endif
-  result = acsSysTask->addComponent(objects::ACS_SUBSYSTEM);
-  if (result != returnvalue::OK) {
-    scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM);
-  }

 #if OBSW_ADD_RTD_DEVICES == 1
  PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask(
@ -327,12 +331,12 @@ void scheduling::initTasks() {
 #endif
 #endif

-  comTask->startTask();
+  genericSysTask->startTask();
 #if OBSW_ADD_CCSDS_IP_CORES == 1
  pdecHandlerTask->startTask();
 #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */

-  sysTask->startTask();
+  coreCtrlTask->startTask();
 #if OBSW_ADD_SA_DEPL == 1
  solarArrayDeplTask->startTask();
 #endif
@ -386,7 +390,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
 #ifdef RELEASE_BUILD
  static constexpr float acsPstPeriod = 0.4;
 #else
-  static constexpr float acsPstPeriod = 0.6;
+  static constexpr float acsPstPeriod = 0.8;
 #endif
  FixedTimeslotTaskIF* acsPst = factory.createFixedTimeslotTask(
      "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc);
--- a/bsp_q7s/obsw.cpp
+++ b/bsp_q7s/obsw.cpp
@ -13,6 +13,8 @@
 #include "core/scheduling.h"
 #include "fsfw/tasks/TaskFactory.h"
 #include "fsfw/version.h"
+#include "mission/acsDefs.h"
+#include "mission/system/tree/system.h"
 #include "q7sConfig.h"
 #include "watchdog/definitions.h"

@ -72,6 +74,18 @@ int obsw::obsw() {

  scheduling::initMission();

+#if OBSW_COMMAND_SAFE_MODE_AT_STARTUP == 1
+  // Command the EIVE system to safe mode
+  auto sysQueueId = satsystem::EIVE_SYSTEM.getCommandQueue();
+  CommandMessage msg;
+  ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
+  ReturnValue_t result =
+      MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
+  if (result != returnvalue::OK) {
+    sif::error << "Sending safe mode command to EIVE system failed" << std::endl;
+  }
+#endif
+
  for (;;) {
    /* Suspend main thread by sleeping it. */
    TaskFactory::delayTask(5000);
--- a/common/config/commonConfig.h.in
+++ b/common/config/commonConfig.h.in
@ -4,6 +4,8 @@
 #include <cstdint>
 #include "fsfw/version.h"

+#cmakedefine RELEASE_BUILD
+
 #cmakedefine RASPBERRY_PI
 #cmakedefine XIPHOS_Q7S
 #cmakedefine BEAGLEBONEBLACK
--- a/common/config/devConf.h
+++ b/common/config/devConf.h
@ -5,6 +5,7 @@

 #include <cstdint>

+#include "commonConfig.h"
 #include "fsfw/timemanager/clockDefinitions.h"
 #include "fsfw_hal/linux/serial/SerialCookie.h"
 #include "fsfw_hal/linux/spi/spiDefinitions.h"
@ -47,10 +48,19 @@ static constexpr spi::SpiModes DEFAULT_ADIS16507_MODE = spi::SpiModes::MODE_3;
 static constexpr uint32_t RW_SPEED = 300'000;
 static constexpr spi::SpiModes RW_MODE = spi::SpiModes::MODE_0;

-static constexpr dur_millis_t RTD_CS_TIMEOUT = 50;
+#ifdef RELEASE_BUILD
+static constexpr uint8_t CS_FACTOR = 1;
+#else
+static constexpr uint8_t CS_FACTOR = 3;
+#endif
+
+static constexpr dur_millis_t RTD_CS_TIMEOUT = 50 * CS_FACTOR;
 static constexpr uint32_t RTD_SPEED = 2'000'000;
 static constexpr spi::SpiModes RTD_MODE = spi::SpiModes::MODE_3;

+static constexpr dur_millis_t SUS_CS_TIMEOUT = 50 * CS_FACTOR;
+static constexpr dur_millis_t ACS_BOARD_CS_TIMEOUT = 50 * CS_FACTOR;
+
 }  // namespace spi

 namespace uart {
--- a/common/config/eive/definitions.h
+++ b/common/config/eive/definitions.h
@ -53,7 +53,7 @@ static constexpr uint8_t VC3_QUEUE_SIZE = 50;
 static constexpr uint32_t MAX_PUS_FUNNEL_QUEUE_DEPTH = 100;

 static constexpr uint32_t MAX_STORED_CMDS_UDP = 120;
-static constexpr uint32_t MAX_STORED_CMDS_TCP = 120;
+static constexpr uint32_t MAX_STORED_CMDS_TCP = 150;

 namespace acs {

--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit d302ba71858edfa15834ff8b28d6cce2c2cbbb84
+Subproject commit dac2d210b597adfaf45bd5ae6a4c027599927601
--- a/linux/ObjectFactory.cpp
+++ b/linux/ObjectFactory.cpp
@ -79,6 +79,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo
  std::array<SusHandler*, 12> susHandlers = {};
  SpiCookie* spiCookie = new SpiCookie(addresses::SUS_0, gpioIds::CS_SUS_0, SUS::MAX_CMD_SIZE,
                                       spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[0] =
      new SusHandler(objects::SUS_0_N_LOC_XFYFZM_PT_XF, 0, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
@ -86,6 +87,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_1, gpioIds::CS_SUS_1, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[1] =
      new SusHandler(objects::SUS_1_N_LOC_XBYFZM_PT_XB, 1, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
@ -93,6 +95,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_2, gpioIds::CS_SUS_2, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[2] =
      new SusHandler(objects::SUS_2_N_LOC_XFYBZB_PT_YB, 2, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
@ -100,6 +103,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_3, gpioIds::CS_SUS_3, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[3] =
      new SusHandler(objects::SUS_3_N_LOC_XFYBZF_PT_YF, 3, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
@ -107,6 +111,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_4, gpioIds::CS_SUS_4, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[4] =
      new SusHandler(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, 4, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
@ -114,6 +119,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_5, gpioIds::CS_SUS_5, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[5] =
      new SusHandler(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, 5, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
@ -121,6 +127,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[6] =
      new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
@ -128,6 +135,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_7, gpioIds::CS_SUS_7, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[7] =
      new SusHandler(objects::SUS_7_R_LOC_XBYBZM_PT_XB, 7, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
@ -135,6 +143,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_8, gpioIds::CS_SUS_8, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[8] =
      new SusHandler(objects::SUS_8_R_LOC_XBYBZB_PT_YB, 8, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
@ -142,6 +151,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_9, gpioIds::CS_SUS_9, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[9] =
      new SusHandler(objects::SUS_9_R_LOC_XBYBZB_PT_YF, 9, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
@ -149,6 +159,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_10, gpioIds::CS_SUS_10, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[10] =
      new SusHandler(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, 10, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
@ -156,6 +167,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo

  spiCookie = new SpiCookie(addresses::SUS_11, gpioIds::CS_SUS_11, SUS::MAX_CMD_SIZE,
                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
  susHandlers[11] =
      new SusHandler(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, 11, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
--- a/linux/devices/Max31865RtdLowlevelHandler.cpp
+++ b/linux/devices/Max31865RtdLowlevelHandler.cpp
@ -58,26 +58,27 @@ bool Max31865RtdReader::rtdIsActive(uint8_t idx) {

 bool Max31865RtdReader::periodicInitHandling() {
  using namespace MAX31865;
-  MutexGuard mg(readerMutex);
  ReturnValue_t result = returnvalue::OK;
-  if (mg.getLockResult() != returnvalue::OK) {
-    sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
-    return false;
-  }

  for (auto& rtd : rtds) {
    if (rtd == nullptr) {
      continue;
    }
+    MutexGuard mg(readerMutex);
+    if (mg.getLockResult() != returnvalue::OK) {
+      sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
+      return false;
+    }
    if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
      ManualCsLockWrapper mg(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
      if (mg.lockResult != returnvalue::OK or mg.gpioResult != returnvalue::OK) {
        sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
-        break;
+        continue;
      }
      result = writeCfgReg(rtd->spiCookie, BASE_CFG);
      if (result != returnvalue::OK) {
        handleSpiError(rtd, result, "writeCfgReg");
+        continue;
      }
      if (rtd->writeLowThreshold) {
        result = writeLowThreshold(rtd->spiCookie, rtd->lowThreshold);
@ -116,16 +117,16 @@ bool Max31865RtdReader::periodicInitHandling() {

 ReturnValue_t Max31865RtdReader::periodicReadReqHandling() {
  using namespace MAX31865;
-  MutexGuard mg(readerMutex);
-  if (mg.getLockResult() != returnvalue::OK) {
-    sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
-    return returnvalue::FAILED;
-  }
  // Now request one shot config for all active RTDs
  for (auto& rtd : rtds) {
    if (rtd == nullptr) {
      continue;
    }
+    MutexGuard mg(readerMutex);
+    if (mg.getLockResult() != returnvalue::OK) {
+      sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
+      return returnvalue::FAILED;
+    }
    if (rtdIsActive(rtd->idx)) {
      ReturnValue_t result = writeCfgReg(rtd->spiCookie, BASE_CFG | (1 << CfgBitPos::ONE_SHOT));
      if (result != returnvalue::OK) {
@ -141,27 +142,33 @@ ReturnValue_t Max31865RtdReader::periodicReadReqHandling() {
 ReturnValue_t Max31865RtdReader::periodicReadHandling() {
  using namespace MAX31865;
  auto result = returnvalue::OK;
-  MutexGuard mg(readerMutex);
-  if (mg.getLockResult() != returnvalue::OK) {
-    sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
-    return returnvalue::FAILED;
-  }
  // Now read the RTD values
  for (auto& rtd : rtds) {
    if (rtd == nullptr) {
      continue;
    }
+    MutexGuard mg(readerMutex);
+    if (mg.getLockResult() != returnvalue::OK) {
+      sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
+      return returnvalue::FAILED;
+    }
    if (rtdIsActive(rtd->idx)) {
+      ManualCsLockWrapper mg(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
+      if (mg.lockResult != returnvalue::OK or mg.gpioResult != returnvalue::OK) {
+        sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
+        continue;
+      }
      uint16_t rtdVal = 0;
      bool faultBitSet = false;
      result = writeCfgReg(rtd->spiCookie, BASE_CFG);
      if (result != returnvalue::OK) {
        handleSpiError(rtd, result, "writeCfgReg");
+        continue;
      }
      result = readRtdVal(rtd->spiCookie, rtdVal, faultBitSet);
      if (result != returnvalue::OK) {
        handleSpiError(rtd, result, "readRtdVal");
-        return returnvalue::FAILED;
+        continue;
      }
      if (faultBitSet) {
        rtd->db.faultBitSet = faultBitSet;
--- a/linux/devices/Max31865RtdLowlevelHandler.h
+++ b/linux/devices/Max31865RtdLowlevelHandler.h
@ -3,9 +3,11 @@

 #include <fsfw/ipc/MutexIF.h>
 #include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/timemanager/clockDefinitions.h>
 #include <fsfw_hal/linux/spi/SpiComIF.h>
 #include <fsfw_hal/linux/spi/SpiCookie.h>

+#include "devConf.h"
 #include "fsfw/devicehandlers/DeviceCommunicationIF.h"
 #include "mission/devices/devicedefinitions/Max31865Definitions.h"

@ -50,8 +52,8 @@ class Max31865RtdReader : public SystemObject,

  SpiComIF* comIF;
  GpioIF* gpioIF;
-  MutexIF::TimeoutType csTimeoutType = MutexIF::TimeoutType::BLOCKING;
-  uint32_t csTimeoutMs = 0;
+  MutexIF::TimeoutType csTimeoutType = MutexIF::TimeoutType::WAITING;
+  uint32_t csTimeoutMs = spi::RTD_CS_TIMEOUT;
  MutexIF* csLock = nullptr;

  bool periodicInitHandling();
--- a/linux/devices/ploc/PlocSupervisorHandler.cpp
+++ b/linux/devices/ploc/PlocSupervisorHandler.cpp
@ -704,8 +704,9 @@ ReturnValue_t PlocSupervisorHandler::initializeLocalDataPool(localpool::DataPool
  localDataPoolMap.emplace(supv::NVM0_1_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::NVM3_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::MISSION_IO_STATE, new PoolEntry<uint8_t>({0}));
-  localDataPoolMap.emplace(supv::FMC_STATE, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::FMC_STATE, &fmcStateEntry);
  localDataPoolMap.emplace(supv::NUM_TCS, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::TEMP_SUP, &tempSupEntry);
  localDataPoolMap.emplace(supv::UPTIME, new PoolEntry<uint64_t>({0}));
  localDataPoolMap.emplace(supv::CPULOAD, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(supv::AVAILABLEHEAP, new PoolEntry<uint32_t>({0}));
@ -718,6 +719,8 @@ ReturnValue_t PlocSupervisorHandler::initializeLocalDataPool(localpool::DataPool
  localDataPoolMap.emplace(supv::BP0_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::BP1_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::BP2_STATE, new PoolEntry<uint8_t>({0}));
+  localDataPoolMap.emplace(supv::BOOT_STATE, &bootStateEntry);
+  localDataPoolMap.emplace(supv::BOOT_CYCLES, &bootCyclesEntry);

  localDataPoolMap.emplace(supv::LATCHUP_ID, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(supv::CNT0, new PoolEntry<uint16_t>({0}));
--- a/linux/devices/ploc/PlocSupervisorHandler.h
+++ b/linux/devices/ploc/PlocSupervisorHandler.h
@ -156,6 +156,11 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
  Countdown bootTimeout = Countdown(BOOT_TIMEOUT);
  Countdown mramDumpTimeout = Countdown(MRAM_DUMP_TIMEOUT);

+  PoolEntry<uint8_t> fmcStateEntry = PoolEntry<uint8_t>(1);
+  PoolEntry<uint8_t> bootStateEntry = PoolEntry<uint8_t>(1);
+  PoolEntry<uint8_t> bootCyclesEntry = PoolEntry<uint8_t>(1);
+  PoolEntry<uint32_t> tempSupEntry = PoolEntry<uint32_t>(1);
+
  /**
   * @brief Adjusts the timeout of the execution report dependent on command
   */
--- a/linux/fsfwconfig/FSFWConfig.h.in
+++ b/linux/fsfwconfig/FSFWConfig.h.in
@ -34,7 +34,7 @@

 #if FSFW_OBJ_EVENT_TRANSLATION == 1
 //! Specify whether info events are printed too.
-#define FSFW_DEBUG_INFO				1
+#define FSFW_DEBUG_INFO				@FSFW_DEBUG_INFO@
 #include "objects/translateObjects.h"
 #include "events/translateEvents.h"
 #else
--- a/mission/CMakeLists.txt
+++ b/mission/CMakeLists.txt
@ -8,3 +8,5 @@ add_subdirectory(system)
 add_subdirectory(csp)
 add_subdirectory(cfdp)
 add_subdirectory(config)
+
+target_sources(${LIB_EIVE_MISSION} PRIVATE acsDefs.cpp)
--- a/mission/acsDefs.cpp
+++ b/mission/acsDefs.cpp
@ -0,0 +1,40 @@
+#include "acsDefs.h"
+
+const char* acs::getModeStr(AcsMode mode) {
+  static const char* modeStr = "UNKNOWN";
+  switch (mode) {
+    case (acs::AcsMode::OFF): {
+      modeStr = "OFF";
+      break;
+    }
+    case (acs::AcsMode::SAFE): {
+      modeStr = "SAFE";
+      break;
+    }
+    case (acs::AcsMode::DETUMBLE): {
+      modeStr = "DETUBMLE";
+      break;
+    }
+    case (acs::AcsMode::PTG_NADIR): {
+      modeStr = "POITNING NADIR";
+      break;
+    }
+    case (acs::AcsMode::PTG_IDLE): {
+      modeStr = "POINTING IDLE";
+      break;
+    }
+    case (acs::AcsMode::PTG_INERTIAL): {
+      modeStr = "POINTING INERTIAL";
+      break;
+    }
+    case (acs::AcsMode::PTG_TARGET): {
+      modeStr = "POINTING TARGET";
+      break;
+    }
+    case (acs::AcsMode::PTG_TARGET_GS): {
+      modeStr = "POINTING TARGET GS";
+      break;
+    }
+  }
+  return modeStr;
+}
--- a/mission/acsDefs.h
+++ b/mission/acsDefs.h
@ -7,7 +7,7 @@
 namespace acs {

 // These modes are the submodes of the ACS controller and the modes of the ACS subsystem.
-enum AcsMode {
+enum AcsMode : Mode_t {
  OFF = HasModesIF::MODE_OFF,
  SAFE = 10,
  DETUMBLE = 11,
@ -24,6 +24,8 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
 //!< The system has recovered from a safe rate rotation violation.
 static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);

+extern const char* getModeStr(AcsMode mode);
+
 }  // namespace acs

 #endif /* MISSION_ACSDEFS_H_ */
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@ -566,9 +566,15 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
  return INVALID_MODE;
 }

-void AcsController::modeChanged(Mode_t mode, Submode_t submode) {}
+void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
+  return ExtendedControllerBase::modeChanged(mode, submode);
+}

-void AcsController::announceMode(bool recursive) {}
+void AcsController::announceMode(bool recursive) {
+  const char *modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));
+  sif::info << "ACS controller is now in " << modeStr << " mode" << std::endl;
+  return ExtendedControllerBase::announceMode(recursive);
+}

 void AcsController::copyMgmData() {
  ACS::SensorValues sensorValues;
--- a/mission/core/GenericFactory.cpp
+++ b/mission/core/GenericFactory.cpp
@ -94,7 +94,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
  }

  {
-    PoolManager::LocalPoolConfig poolCfg = {{400, 32},  {350, 64},   {200, 128},
+    PoolManager::LocalPoolConfig poolCfg = {{400, 32},  {400, 64},   {250, 128},
                                            {150, 512}, {150, 1024}, {150, 2048}};
    tmStore = new PoolManager(objects::TM_STORE, poolCfg);
  }
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@ -707,7 +707,7 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
      subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(rawMtmMeasurementSet.getSid(), false, 10.0));
-  return returnvalue::OK;
+  return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
 }

 ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
--- a/mission/devices/PcduHandler.cpp
+++ b/mission/devices/PcduHandler.cpp
@ -457,22 +457,7 @@ void PCDUHandler::checkAndUpdatePduSwitch(GOMSPACE::Pdu pdu, pcdu::Switches swit
                                          uint8_t setValue) {
  using namespace pcdu;
  if (switchStates[switchIdx] != setValue) {
-#if OBSW_INITIALIZE_SWITCHES == 1
-    // This code initializes the switches to the default init switch states on every reboot.
-    // This is not done by the PCDU unless it is power-cycled.
-    if (((pdu == GOMSPACE::Pdu::PDU1) and firstSwitchInfoPdu1) or
-        ((pdu == GOMSPACE::Pdu::PDU2) and firstSwitchInfoPdu2)) {
-      ReturnValue_t state = PowerSwitchIF::SWITCH_OFF;
-      if (INIT_SWITCH_STATES[switchIdx] == ON) {
-        state = PowerSwitchIF::SWITCH_ON;
-      }
-      sendSwitchCommand(switchIdx, state);
-    } else {
-      triggerEvent(power::SWITCH_HAS_CHANGED, setValue, switchIdx);
-    }
-#else
    triggerEvent(power::SWITCH_HAS_CHANGED, setValue, switchIdx);
-#endif
  }
  switchStates[switchIdx] = setValue;
 }
--- a/mission/system/objects/AcsSubsystem.cpp
+++ b/mission/system/objects/AcsSubsystem.cpp
@ -81,3 +81,9 @@ void AcsSubsystem::handleEventMessages() {
    }
  }
 }
+
+void AcsSubsystem::announceMode(bool recursive) {
+  const char* modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));
+  sif::info << "ACS subsystem is now in " << modeStr << " mode" << std::endl;
+  return Subsystem::announceMode(recursive);
+}
--- a/mission/system/objects/AcsSubsystem.h
+++ b/mission/system/objects/AcsSubsystem.h
@ -10,6 +10,7 @@ class AcsSubsystem : public Subsystem {
 private:
  ReturnValue_t initialize() override;
  void performChildOperation() override;
+  void announceMode(bool recursive) override;

  void handleEventMessages();

--- a/mission/system/objects/CMakeLists.txt
+++ b/mission/system/objects/CMakeLists.txt
@ -4,6 +4,7 @@ target_sources(
          CamSwitcher.cpp
          AcsSubsystem.cpp
          ComSubsystem.cpp
+          TcsSubsystem.cpp
          PayloadSubsystem.cpp
          AcsBoardAssembly.cpp
          Stack5VHandler.cpp
--- a/mission/system/objects/EiveSystem.cpp
+++ b/mission/system/objects/EiveSystem.cpp
@ -1,5 +1,43 @@
 #include "EiveSystem.h"

+#include <mission/acsDefs.h>
+
 EiveSystem::EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
                       uint32_t maxNumberOfTables)
    : Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables) {}
+
+void EiveSystem::announceMode(bool recursive) {
+  const char* modeStr = "UNKNOWN";
+  switch (mode) {
+    case (acs::AcsMode::OFF): {
+      modeStr = "OFF";
+      break;
+    }
+    case (acs::AcsMode::SAFE): {
+      modeStr = "SAFE";
+      break;
+    }
+    case (acs::AcsMode::DETUMBLE): {
+      modeStr = "DETUBMLE";
+      break;
+    }
+    case (acs::AcsMode::PTG_IDLE): {
+      modeStr = "POINTING IDLE";
+      break;
+    }
+    case (acs::AcsMode::PTG_INERTIAL): {
+      modeStr = "POINTING INERTIAL";
+      break;
+    }
+    case (acs::AcsMode::PTG_TARGET): {
+      modeStr = "POINTING TARGET";
+      break;
+    }
+    case (acs::AcsMode::PTG_TARGET_GS): {
+      modeStr = "POINTING TARGET GS";
+      break;
+    }
+  }
+  sif::info << "EIVE system is now in " << modeStr << " mode" << std::endl;
+  return Subsystem::announceMode(recursive);
+}
--- a/mission/system/objects/EiveSystem.h
+++ b/mission/system/objects/EiveSystem.h
@ -8,6 +8,7 @@ class EiveSystem : public Subsystem {
  EiveSystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);

 private:
+  void announceMode(bool recursive) override;
 };

 #endif /* MISSION_SYSTEM_EIVESYSTEM_H_ */
--- a/mission/system/objects/TcsSubsystem.cpp
+++ b/mission/system/objects/TcsSubsystem.cpp
@ -0,0 +1,27 @@
+#include "TcsSubsystem.h"
+
+#include "fsfw/devicehandlers/DeviceHandlerIF.h"
+
+TcsSubsystem::TcsSubsystem(object_id_t objectId, uint32_t maxNumberOfSequences,
+                           uint32_t maxNumberOfTables)
+    : Subsystem(objectId, maxNumberOfSequences, maxNumberOfTables) {}
+
+void TcsSubsystem::announceMode(bool recursive) {
+  const char* modeStr = "UNKNOWN";
+  switch (mode) {
+    case (HasModesIF::MODE_OFF): {
+      modeStr = "OFF";
+      break;
+    }
+    case (HasModesIF::MODE_ON): {
+      modeStr = "ON";
+      break;
+    }
+    case (DeviceHandlerIF::MODE_NORMAL): {
+      modeStr = "NORMAL";
+      break;
+    }
+  }
+  sif::info << "TCS subsystem is now in " << modeStr << " mode" << std::endl;
+  return Subsystem::announceMode(recursive);
+}
--- a/mission/system/objects/TcsSubsystem.h
+++ b/mission/system/objects/TcsSubsystem.h
@ -0,0 +1,13 @@
+#ifndef MISSION_SYSTEM_OBJECTS_TCSSUBSYSTEM_H_
+#define MISSION_SYSTEM_OBJECTS_TCSSUBSYSTEM_H_
+#include <fsfw/subsystem/Subsystem.h>
+
+class TcsSubsystem : public Subsystem {
+ public:
+  TcsSubsystem(object_id_t objectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
+
+ private:
+  void announceMode(bool recursive) override;
+};
+
+#endif /* MISSION_SYSTEM_OBJECTS_TCSSUBSYSTEM_H_ */
--- a/mission/system/tree/acsModeTree.cpp
+++ b/mission/system/tree/acsModeTree.cpp
@ -30,6 +30,8 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper);
 static const auto OFF = HasModesIF::MODE_OFF;
 static const auto NML = DeviceHandlerIF::MODE_NORMAL;

+auto SUS_BOARD_NML_TRANS = std::make_pair(0x20, FixedArrayList<ModeListEntry, 1>());
+
 auto ACS_SEQUENCE_OFF = std::make_pair(acs::AcsMode::OFF, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_OFF_TGT =
    std::make_pair((acs::AcsMode::OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
@ -39,62 +41,62 @@ auto ACS_TABLE_OFF_TRANS_1 =
    std::make_pair((acs::AcsMode::OFF << 24) | 3, FixedArrayList<ModeListEntry, 6>());

 auto ACS_SEQUENCE_DETUMBLE =
-    std::make_pair(acs::AcsMode::DETUMBLE, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(acs::AcsMode::DETUMBLE, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_DETUMBLE_TGT =
    std::make_pair((acs::AcsMode::DETUMBLE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_DETUMBLE_TRANS_0 =
    std::make_pair((acs::AcsMode::DETUMBLE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 auto ACS_TABLE_DETUMBLE_TRANS_1 =
-    std::make_pair((acs::AcsMode::DETUMBLE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((acs::AcsMode::DETUMBLE << 24) | 3, FixedArrayList<ModeListEntry, 5>());

-auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsMode::SAFE, FixedArrayList<ModeListEntry, 3>());
+auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsMode::SAFE, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_SAFE_TGT =
    std::make_pair((acs::AcsMode::SAFE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_SAFE_TRANS_0 =
    std::make_pair((acs::AcsMode::SAFE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 auto ACS_TABLE_SAFE_TRANS_1 =
-    std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 5>());

-auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::PTG_IDLE, FixedArrayList<ModeListEntry, 3>());
+auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::PTG_IDLE, FixedArrayList<ModeListEntry, 5>());
 auto ACS_TABLE_IDLE_TGT =
    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 1, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_IDLE_TRANS_0 =
    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 2, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_IDLE_TRANS_1 =
-    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 3, FixedArrayList<ModeListEntry, 2>());
+    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 3, FixedArrayList<ModeListEntry, 3>());

 auto ACS_TABLE_PTG_TRANS_0 =
    std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 2, FixedArrayList<ModeListEntry, 5>());

 auto ACS_SEQUENCE_PTG_TARGET =
-    std::make_pair(acs::AcsMode::PTG_TARGET, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(acs::AcsMode::PTG_TARGET, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_PTG_TARGET_TGT =
    std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 1, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_PTG_TARGET_TRANS_1 =
    std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 3, FixedArrayList<ModeListEntry, 1>());

 auto ACS_SEQUENCE_PTG_TARGET_GS =
-    std::make_pair(acs::AcsMode::PTG_TARGET_GS, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(acs::AcsMode::PTG_TARGET_GS, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_PTG_TARGET_GS_TGT =
    std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 1, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 =
    std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 1>());

 auto ACS_SEQUENCE_PTG_TARGET_NADIR =
-    std::make_pair(acs::AcsMode::PTG_NADIR, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(acs::AcsMode::PTG_NADIR, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_PTG_TARGET_NADIR_TGT =
    std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 1, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 =
    std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>());

 auto ACS_SEQUENCE_PTG_TARGET_INERTIAL =
-    std::make_pair(acs::AcsMode::PTG_INERTIAL, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(acs::AcsMode::PTG_INERTIAL, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT =
    std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 =
    std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>());

-void satsystem::acs::init() {
+Subsystem& satsystem::acs::init() {
  ModeListEntry entry;
  const char* ctxc = "satsystem::acs::init: generic target";
  // Insert Helper Table
@ -114,6 +116,11 @@ void satsystem::acs::init() {
            TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)),
        ctxc);

+  // Build SUS board transition
+  iht(objects::SUS_BOARD_ASS, NML, 0, SUS_BOARD_NML_TRANS.second);
+  check(ACS_SUBSYSTEM.addTable(TableEntry(SUS_BOARD_NML_TRANS.first, &SUS_BOARD_NML_TRANS.second)),
+        ctxc);
+
  buildOffSequence(ACS_SUBSYSTEM, entry);
  buildSafeSequence(ACS_SUBSYSTEM, entry);
  buildDetumbleSequence(ACS_SUBSYSTEM, entry);
@ -123,6 +130,7 @@ void satsystem::acs::init() {
  buildTargetPtNadirSequence(ACS_SUBSYSTEM, entry);
  buildTargetPtInertialSequence(ACS_SUBSYSTEM, entry);
  ACS_SUBSYSTEM.setInitialMode(::acs::AcsMode::SAFE);
+  return ACS_SUBSYSTEM;
 }

 namespace {
@ -157,7 +165,6 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
  iht(objects::IMTQ_HANDLER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
  iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
  iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
-  iht(objects::SUS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
  iht(objects::RW_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
  check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc);

@ -198,13 +205,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {

  // Build SAFE transition 0
  iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
-  iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
  iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
  iht(objects::RW_ASS, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
  check(ss.addTable(&ACS_TABLE_SAFE_TRANS_0.second, ACS_TABLE_SAFE_TRANS_0.first, false, true),
        ctxc);

+  // SUS board transition table is defined above
+
  // Build SAFE transition 1
  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TRANS_1.second);
  check(ss.addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true),
@ -212,6 +220,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {

  // Build SAFE sequence
  ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TGT.first, 0, true);
+  ihs(ACS_SEQUENCE_SAFE.second, SUS_BOARD_NML_TRANS.first, 0, false);
  ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TRANS_0.first, 0, false);
  ihs(ACS_SEQUENCE_SAFE.second, ACS_TABLE_SAFE_TRANS_1.first, 0, false);
  check(ss.addSequence(&ACS_SEQUENCE_SAFE.second, ACS_SEQUENCE_SAFE.first, ACS_SEQUENCE_SAFE.first,
@ -246,6 +255,8 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {
  check(ss.addTable(&ACS_TABLE_DETUMBLE_TGT.second, ACS_TABLE_DETUMBLE_TGT.first, false, true),
        ctxc);

+  // SUS board transition table is defined above
+
  // Build DETUMBLE transition 0
  iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
@ -264,6 +275,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {

  // Build DETUMBLE sequence
  ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TGT.first, 0, true);
+  ihs(ACS_SEQUENCE_DETUMBLE.second, SUS_BOARD_NML_TRANS.first, 0, false);
  ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TRANS_0.first, 0, false);
  ihs(ACS_SEQUENCE_DETUMBLE.second, ACS_TABLE_DETUMBLE_TRANS_1.first, 0, false);
  check(ss.addSequence(&ACS_SEQUENCE_DETUMBLE.second, ACS_SEQUENCE_DETUMBLE.first,
@ -298,6 +310,8 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
  ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true);

+  // SUS board transition table is built above
+
  // Build IDLE transition 0
  iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
@ -312,6 +326,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {

  // Build IDLE sequence
  ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TGT.first, 0, true);
+  ihs(ACS_SEQUENCE_IDLE.second, SUS_BOARD_NML_TRANS.first, 0, true);
  ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_0.first, 0, true);
  ihs(ACS_SEQUENCE_IDLE.second, ACS_TABLE_IDLE_TRANS_1.first, 0, true);
  ss.addSequence(&ACS_SEQUENCE_IDLE.second, ACS_SEQUENCE_IDLE.first, ACS_SEQUENCE_SAFE.first, false,
@ -348,6 +363,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
  check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true),
        ctxc);

+  // SUS board transition table is built above
  // Transition 0 already built
  // Build TARGET PT transition 1
  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second);
@ -357,6 +373,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {

  // Build IDLE sequence
  ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TGT.first, 0, true);
+  ihs(ACS_SEQUENCE_PTG_TARGET.second, SUS_BOARD_NML_TRANS.first, 0, true);
  ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TRANS_0.first, 0, true);
  ihs(ACS_SEQUENCE_PTG_TARGET.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, 0, true);
  check(ss.addSequence(&ACS_SEQUENCE_PTG_TARGET.second, ACS_SEQUENCE_PTG_TARGET.first,
@ -406,6 +423,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {

  // Build IDLE sequence
  ihs(ACS_SEQUENCE_PTG_TARGET_NADIR.second, ACS_TABLE_PTG_TARGET_NADIR_TGT.first, 0, true);
+  ihs(ACS_SEQUENCE_PTG_TARGET_NADIR.second, SUS_BOARD_NML_TRANS.first, 0, true);
  ihs(ACS_SEQUENCE_PTG_TARGET_NADIR.second, ACS_TABLE_PTG_TRANS_0.first, 0, true);
  ihs(ACS_SEQUENCE_PTG_TARGET_NADIR.second, ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first, 0, true);
  check(
@ -456,6 +474,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {

  // Build IDLE sequence
  ihs(ACS_SEQUENCE_PTG_TARGET_GS.second, ACS_TABLE_PTG_TARGET_GS_TGT.first, 0, true);
+  ihs(ACS_SEQUENCE_PTG_TARGET_GS.second, SUS_BOARD_NML_TRANS.first, 0, true);
  ihs(ACS_SEQUENCE_PTG_TARGET_GS.second, ACS_TABLE_PTG_TRANS_0.first, 0, true);
  ihs(ACS_SEQUENCE_PTG_TARGET_GS.second, ACS_TABLE_PTG_TARGET_GS_TRANS_1.first, 0, true);
  check(ss.addSequence(SequenceEntry(ACS_SEQUENCE_PTG_TARGET_GS.first,
@ -505,6 +524,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {

  // Build IDLE sequence
  ihs(ACS_SEQUENCE_PTG_TARGET_INERTIAL.second, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first, 0, true);
+  ihs(ACS_SEQUENCE_PTG_TARGET_INERTIAL.second, SUS_BOARD_NML_TRANS.first, 0, true);
  ihs(ACS_SEQUENCE_PTG_TARGET_INERTIAL.second, ACS_TABLE_PTG_TRANS_0.first, 0, true);
  ihs(ACS_SEQUENCE_PTG_TARGET_INERTIAL.second, ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first, 0,
      true);
--- a/mission/system/tree/acsModeTree.h
+++ b/mission/system/tree/acsModeTree.h
@ -4,7 +4,7 @@ namespace satsystem {
 namespace acs {

 extern AcsSubsystem ACS_SUBSYSTEM;
-void init();
+Subsystem& init();

 }  // namespace acs
 }  // namespace satsystem
--- a/mission/system/tree/comModeTree.cpp
+++ b/mission/system/tree/comModeTree.cpp
@ -62,13 +62,14 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh);

 }  // namespace

-void satsystem::com::init() {
+Subsystem& satsystem::com::init() {
  ModeListEntry entry;
  buildRxOnlySequence(SUBSYSTEM, entry);
  buildTxAndRxLowRateSequence(SUBSYSTEM, entry);
  buildTxAndRxHighRateSequence(SUBSYSTEM, entry);
  buildTxAndRxDefaultRateSequence(SUBSYSTEM, entry);
  SUBSYSTEM.setInitialMode(NML, ::com::Submode::RX_ONLY);
+  return SUBSYSTEM;
 }

 namespace {
--- a/mission/system/tree/comModeTree.h
+++ b/mission/system/tree/comModeTree.h
@ -8,7 +8,7 @@ namespace satsystem {
 namespace com {
 extern ComSubsystem SUBSYSTEM;

-void init();
+Subsystem& init();
 }  // namespace com

 }  // namespace satsystem
--- a/mission/system/tree/payloadModeTree.cpp
+++ b/mission/system/tree/payloadModeTree.cpp
@ -20,7 +20,7 @@ void initEarthObsvSequence(Subsystem& ss, ModeListEntry& eh);
 void initScexSequence(Subsystem& ss, ModeListEntry& eh);
 }  // namespace

-Subsystem satsystem::pl::SUBSYSTEM = Subsystem(objects::PL_SUBSYSTEM, 12, 24);
+PayloadSubsystem satsystem::pl::SUBSYSTEM = PayloadSubsystem(objects::PL_SUBSYSTEM, 12, 24);

 const auto check = subsystem::checkInsert;
 static const auto OFF = HasModesIF::MODE_OFF;
@ -77,7 +77,7 @@ auto PL_TABLE_SCEX_TGT =
 auto PL_TABLE_SCEX_TRANS_0 =
    std::make_pair((payload::Mode::SCEX << 24) | 2, FixedArrayList<ModeListEntry, 1>());

-void satsystem::pl::init() {
+Subsystem& satsystem::pl::init() {
  ModeListEntry entry;
  initOffSequence(SUBSYSTEM, entry);
  initPlMpsocStreamSequence(SUBSYSTEM, entry);
@ -86,6 +86,7 @@ void satsystem::pl::init() {
  initEarthObsvSequence(SUBSYSTEM, entry);
  initScexSequence(SUBSYSTEM, entry);
  SUBSYSTEM.setInitialMode(OFF);
+  return SUBSYSTEM;
 }

 namespace {
--- a/mission/system/tree/payloadModeTree.h
+++ b/mission/system/tree/payloadModeTree.h
@ -1,15 +1,15 @@
 #ifndef MISSION_SYSTEM_TREE_PAYLOADMODETREE_H_
 #define MISSION_SYSTEM_TREE_PAYLOADMODETREE_H_

-#include <fsfw/subsystem/Subsystem.h>
+#include <mission/system/objects/PayloadSubsystem.h>

 namespace satsystem {

 namespace pl {

-extern Subsystem SUBSYSTEM;
+extern PayloadSubsystem SUBSYSTEM;

-void init();
+Subsystem& init();
 }  // namespace pl

 }  // namespace satsystem
--- a/mission/system/tree/system.cpp
+++ b/mission/system/tree/system.cpp
@ -1,13 +1,148 @@
 #include "system.h"

+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/subsystem/Subsystem.h>
+#include <mission/acsDefs.h>
+
 #include "acsModeTree.h"
 #include "comModeTree.h"
+#include "eive/objects.h"
 #include "payloadModeTree.h"
 #include "tcsModeTree.h"
+#include "util.h"
+
+namespace {
+// Alias for checker function
+const auto check = subsystem::checkInsert;
+
+void buildSafeSequence(Subsystem& ss, ModeListEntry& eh);
+void buildIdleSequence(Subsystem& ss, ModeListEntry& eh);
+}  // namespace
+
+static const auto OFF = HasModesIF::MODE_OFF;
+static const auto NML = DeviceHandlerIF::MODE_NORMAL;

 void satsystem::init() {
-  acs::init();
-  pl::init();
-  tcs::init();
-  com::init();
+  auto& acsSubsystem = acs::init();
+  acsSubsystem.connectModeTreeParent(EIVE_SYSTEM);
+  auto& payloadSubsystem = pl::init();
+  payloadSubsystem.connectModeTreeParent(EIVE_SYSTEM);
+  auto& tcsSubsystem = tcs::init();
+  tcsSubsystem.connectModeTreeParent(EIVE_SYSTEM);
+  auto& comSubsystem = com::init();
+  comSubsystem.connectModeTreeParent(EIVE_SYSTEM);
+  ModeListEntry entry;
+  buildSafeSequence(EIVE_SYSTEM, entry);
+  buildIdleSequence(EIVE_SYSTEM, entry);
 }
+
+EiveSystem satsystem::EIVE_SYSTEM = EiveSystem(objects::EIVE_SYSTEM, 12, 24);
+
+auto EIVE_SEQUENCE_SAFE = std::make_pair(acs::AcsMode::SAFE, FixedArrayList<ModeListEntry, 5>());
+auto EIVE_TABLE_SAFE_TGT =
+    std::make_pair((acs::AcsMode::SAFE << 24) | 1, FixedArrayList<ModeListEntry, 5>());
+auto EIVE_TABLE_SAFE_TRANS_0 =
+    std::make_pair((acs::AcsMode::SAFE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
+auto EIVE_TABLE_SAFE_TRANS_1 =
+    std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 5>());
+
+auto EIVE_SEQUENCE_IDLE =
+    std::make_pair(acs::AcsMode::PTG_IDLE, FixedArrayList<ModeListEntry, 5>());
+auto EIVE_TABLE_IDLE_TGT =
+    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 1, FixedArrayList<ModeListEntry, 5>());
+auto EIVE_TABLE_IDLE_TRANS_0 =
+    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
+auto EIVE_TABLE_IDLE_TRANS_1 =
+    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 3, FixedArrayList<ModeListEntry, 5>());
+
+namespace {
+
+void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
+  std::string context = "satsystem::buildSafeSequence";
+  auto ctxc = context.c_str();
+  // Insert Helper Table
+  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
+    eh.setObject(obj);
+    eh.setMode(mode);
+    eh.setSubmode(submode);
+    check(table.insert(eh), ctxc);
+  };
+  // Insert Helper Sequence
+  auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
+                 bool checkSuccess) {
+    eh.setTableId(tableId);
+    eh.setWaitSeconds(waitSeconds);
+    eh.setCheckSuccess(checkSuccess);
+    check(sequence.insert(eh), ctxc);
+  };
+
+  // Do no track ACS for now because it might jump to detumble mode and back to safe as part of
+  // normal operations.
+  // iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TGT.second);
+  iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TGT.second);
+  check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc);
+
+  // Build SAFE transition 0. Two transitions to reduce number of consecutive events and because
+  // consecutive commanding of TCS and ACS can lead to SPI issues.
+  iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
+  check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
+        ctxc);
+
+  // Build SAFE transition 1
+  iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_1.second);
+  iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_1.second);
+  check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_1.first, &EIVE_TABLE_SAFE_TRANS_1.second)),
+        ctxc);
+
+  // Build Safe sequence
+  ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TGT.first, 0, false);
+  ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_0.first, 0, false);
+  ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_1.first, 0, false);
+  check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second,
+                                     EIVE_SEQUENCE_SAFE.first)),
+        ctxc);
+}
+
+void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
+  std::string context = "satsystem::buildIdleSequence";
+  auto ctxc = context.c_str();
+  // Insert Helper Table
+  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
+    eh.setObject(obj);
+    eh.setMode(mode);
+    eh.setSubmode(submode);
+    check(table.insert(eh), ctxc);
+  };
+  // Insert Helper Sequence
+  auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
+                 bool checkSuccess) {
+    eh.setTableId(tableId);
+    eh.setWaitSeconds(waitSeconds);
+    eh.setCheckSuccess(checkSuccess);
+    check(sequence.insert(eh), ctxc);
+  };
+
+  iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TGT.second);
+  check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TGT.first, &EIVE_TABLE_IDLE_TGT.second)), ctxc);
+
+  // Build SAFE transition 0
+  iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
+  check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)),
+        ctxc);
+
+  // Build SAFE transition 1
+  iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_IDLE_TRANS_1.second);
+  iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_1.second);
+  check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_1.first, &EIVE_TABLE_IDLE_TRANS_1.second)),
+        ctxc);
+
+  // Build Safe sequence
+  ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TGT.first, 0, false);
+  ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_0.first, 0, false);
+  ihs(EIVE_SEQUENCE_IDLE.second, EIVE_TABLE_IDLE_TRANS_1.first, 0, false);
+  check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_IDLE.first, &EIVE_SEQUENCE_IDLE.second,
+                                     EIVE_SEQUENCE_SAFE.first)),
+        ctxc);
+}
+
+}  // namespace
--- a/mission/system/tree/system.h
+++ b/mission/system/tree/system.h
@ -1,10 +1,14 @@
 #ifndef MISSION_SYSTEM_TREE_SYSTEM_H_
 #define MISSION_SYSTEM_TREE_SYSTEM_H_

+#include <mission/system/objects/EiveSystem.h>
+
 namespace satsystem {

 void init();

-}
+extern EiveSystem EIVE_SYSTEM;
+
+}  // namespace satsystem

 #endif /* MISSION_SYSTEM_TREE_SYSTEM_H_ */
--- a/mission/system/tree/tcsModeTree.cpp
+++ b/mission/system/tree/tcsModeTree.cpp
@ -5,7 +5,7 @@
 #include "fsfw/subsystem/Subsystem.h"
 #include "mission/system/tree/util.h"

-Subsystem satsystem::tcs::SUBSYSTEM(objects::TCS_SUBSYSTEM, 12, 24);
+TcsSubsystem satsystem::tcs::SUBSYSTEM(objects::TCS_SUBSYSTEM, 12, 24);

 namespace {
 // Alias for checker function
@ -27,11 +27,12 @@ auto TCS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList<ModeL
 auto TCS_TABLE_NORMAL_TRANS_0 = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 7>());
 auto TCS_TABLE_NORMAL_TRANS_1 = std::make_pair((NML << 24) | 3, FixedArrayList<ModeListEntry, 2>());

-void satsystem::tcs::init() {
+Subsystem& satsystem::tcs::init() {
  ModeListEntry entry;
  buildOffSequence(SUBSYSTEM, entry);
  buildNormalSequence(SUBSYSTEM, entry);
  SUBSYSTEM.setInitialMode(OFF);
+  return SUBSYSTEM;
 }

 namespace {
--- a/mission/system/tree/tcsModeTree.h
+++ b/mission/system/tree/tcsModeTree.h
@ -1,12 +1,13 @@
 #ifndef MISSION_SYSTEM_TREE_TCSMODETREE_H_
 #define MISSION_SYSTEM_TREE_TCSMODETREE_H_
-#include <fsfw/subsystem/Subsystem.h>
+
+#include <mission/system/objects/TcsSubsystem.h>

 namespace satsystem {
 namespace tcs {

-extern Subsystem SUBSYSTEM;
-void init();
+extern TcsSubsystem SUBSYSTEM;
+Subsystem& init();

 }  // namespace tcs
 }  // namespace satsystem
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 8d23f29f947e2a4f79e1fc0910cd9ad59a4fc346
+Subproject commit d47da4c314837e63b054d5dfe22db7c0f3794b90