From 24b939c50b3ddeae101e585e28323e9b1c1185ae Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Mar 2023 18:06:36 +0100
Subject: [PATCH 01/40] add syrlinks assembly

---
 bsp_q7s/core/CoreController.cpp               |  2 +-
 bsp_q7s/core/ObjectFactory.cpp                |  5 +++-
 bsp_q7s/core/scheduling.cpp                   |  4 +++
 bsp_q7s/memory/LocalParameterHandler.h        |  4 +--
 common/config/eive/objects.h                  |  1 +
 common/config/eive/resultClassIds.h           |  4 +--
 linux/fsfwconfig/returnvalues/classIds.h      |  4 +--
 linux/ipcore/PdecHandler.cpp                  | 22 +++++++-------
 linux/ipcore/PdecHandler.h                    |  3 +-
 mission/system/objects/CMakeLists.txt         |  1 +
 mission/system/objects/ComSubsystem.cpp       |  3 +-
 mission/system/objects/ComSubsystem.h         | 18 ++++++------
 .../system/objects/DualLaneAssemblyBase.cpp   |  5 ++++
 mission/system/objects/DualLaneAssemblyBase.h |  1 +
 mission/system/objects/SyrlinksAssembly.cpp   | 29 +++++++++++++++++++
 mission/system/objects/SyrlinksAssembly.h     | 17 +++++++++++
 mission/system/tree/comModeTree.cpp           | 16 +++++-----
 17 files changed, 100 insertions(+), 39 deletions(-)
 create mode 100644 mission/system/objects/SyrlinksAssembly.cpp
 create mode 100644 mission/system/objects/SyrlinksAssembly.h

diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp
index 8977bab7..01757f28 100644
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@@ -1224,7 +1224,7 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
       }
     }
     wordIdx++;
-    if(wordIdx >= 10) {
+    if (wordIdx >= 10) {
       break;
     }
   }
diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp
index c8eb4cc9..e4d16781 100644
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@@ -8,6 +8,7 @@
 #include <mission/devices/MgmLis3CustomHandler.h>
 #include <mission/devices/MgmRm3100CustomHandler.h>
 #include <mission/system/objects/CamSwitcher.h>
+#include <mission/system/objects/SyrlinksAssembly.h>
 
 #include "OBSWConfig.h"
 #include "bsp_q7s/boardtest/Q7STestTask.h"
@@ -579,12 +580,14 @@ void ObjectFactory::createSyrlinksComponents(PowerSwitchIF* pwrSwitcher) {
                        syrlinks::MAX_REPLY_SIZE, UartModes::NON_CANONICAL);
   syrlinksUartCookie->setParityEven();
 
+  auto* syrlinksAssy = new SyrlinksAssembly(objects::SYRLINKS_ASSY);
+  syrlinksAssy->connectModeTreeParent(satsystem::com::SUBSYSTEM);
   auto syrlinksFdir = new SyrlinksFdir(objects::SYRLINKS_HANDLER);
   auto syrlinksHandler =
       new SyrlinksHandler(objects::SYRLINKS_HANDLER, objects::UART_COM_IF, syrlinksUartCookie,
                           pcdu::PDU1_CH1_SYRLINKS_12V, syrlinksFdir);
   syrlinksHandler->setPowerSwitcher(pwrSwitcher);
-  syrlinksHandler->connectModeTreeParent(satsystem::com::SUBSYSTEM);
+  syrlinksHandler->connectModeTreeParent(*syrlinksAssy);
 #if OBSW_DEBUG_SYRLINKS == 1
   syrlinksHandler->setDebugMode(true);
 #endif
diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp
index 7bb2ceec..753fa69c 100644
--- a/bsp_q7s/core/scheduling.cpp
+++ b/bsp_q7s/core/scheduling.cpp
@@ -150,6 +150,10 @@ void scheduling::initTasks() {
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("COM_SUBSYSTEM", objects::COM_SUBSYSTEM);
   }
+  result = genericSysTask->addComponent(objects::SYRLINKS_ASSY);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("SYRLINKS_ASSY", objects::SYRLINKS_ASSY);
+  }
   result = genericSysTask->addComponent(objects::PL_SUBSYSTEM);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PL_SUBSYSTEM", objects::PL_SUBSYSTEM);
diff --git a/bsp_q7s/memory/LocalParameterHandler.h b/bsp_q7s/memory/LocalParameterHandler.h
index a8b96950..61b2f723 100644
--- a/bsp_q7s/memory/LocalParameterHandler.h
+++ b/bsp_q7s/memory/LocalParameterHandler.h
@@ -19,8 +19,8 @@ class LocalParameterHandler : public NVMParameterBase {
    * @brief	Constructor
    *
    * @param sdRelativeName	Absolute name of json file relative to mount
-   * 												directory	of SD card.
-   * E.g. conf/example.json
+   * 												directory	of
+   * SD card. E.g. conf/example.json
    * @param sdcMan	Pointer to SD card manager
    */
   LocalParameterHandler(std::string sdRelativeName, SdCardMountedIF* sdcMan);
diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h
index c7c7cce9..b129c339 100644
--- a/common/config/eive/objects.h
+++ b/common/config/eive/objects.h
@@ -143,6 +143,7 @@ enum commonObjects : uint32_t {
   TCS_BOARD_ASS = 0x73000003,
   RW_ASS = 0x73000004,
   CAM_SWITCHER = 0x73000006,
+  SYRLINKS_ASSY = 0x73000007,
   EIVE_SYSTEM = 0x73010000,
   ACS_SUBSYSTEM = 0x73010001,
   PL_SUBSYSTEM = 0x73010002,
diff --git a/common/config/eive/resultClassIds.h b/common/config/eive/resultClassIds.h
index f7bf26c1..df47d935 100644
--- a/common/config/eive/resultClassIds.h
+++ b/common/config/eive/resultClassIds.h
@@ -40,8 +40,8 @@ enum commonClassIds : uint8_t {
   ACS_SAFE,                 // ACSSAF
   ACS_PTG,                  // ACSPTG
   ACS_DETUMBLE,             // ACSDTB
-	SD_CARD_MANAGER,          // SDMA
-	LOCAL_PARAM_HANDLER,      // LPH
+  SD_CARD_MANAGER,          // SDMA
+  LOCAL_PARAM_HANDLER,      // LPH
   COMMON_CLASS_ID_END       // [EXPORT] : [END]
 };
 }
diff --git a/linux/fsfwconfig/returnvalues/classIds.h b/linux/fsfwconfig/returnvalues/classIds.h
index 0a71a8b6..f4db7ffa 100644
--- a/linux/fsfwconfig/returnvalues/classIds.h
+++ b/linux/fsfwconfig/returnvalues/classIds.h
@@ -13,8 +13,8 @@
 namespace CLASS_ID {
 enum {
   CLASS_ID_START = COMMON_CLASS_ID_END,
-  SCRATCH_BUFFER,       // SCBU
-  CLASS_ID_END          // [EXPORT] : [END]
+  SCRATCH_BUFFER,  // SCBU
+  CLASS_ID_END     // [EXPORT] : [END]
 };
 }
 
diff --git a/linux/ipcore/PdecHandler.cpp b/linux/ipcore/PdecHandler.cpp
index 80327c94..d5e5c0c8 100644
--- a/linux/ipcore/PdecHandler.cpp
+++ b/linux/ipcore/PdecHandler.cpp
@@ -164,8 +164,8 @@ ReturnValue_t PdecHandler::polledOperation() {
 
 // See https://yurovsky.github.io/2014/10/10/linux-uio-gpio-interrupt.html for more information.
 ReturnValue_t PdecHandler::irqOperation() {
-	ReturnValue_t result = returnvalue::OK;
-	int fd = -1;
+  ReturnValue_t result = returnvalue::OK;
+  int fd = -1;
   // Used to unmask IRQ
   uint32_t info = 1;
 
@@ -183,7 +183,7 @@ ReturnValue_t PdecHandler::irqOperation() {
       case State::INIT: {
         result = handleInitState();
         if (result == returnvalue::OK) {
-        	openIrqFile(&fd);
+          openIrqFile(&fd);
         }
         break;
       }
@@ -196,7 +196,7 @@ ReturnValue_t PdecHandler::irqOperation() {
         break;
       }
       case State::RUNNING: {
-      	checkLocks();
+        checkLocks();
         checkAndHandleIrqs(fd, info);
         break;
       }
@@ -238,13 +238,13 @@ ReturnValue_t PdecHandler::handleInitState() {
 }
 
 void PdecHandler::openIrqFile(int* fd) {
-	*fd = open(uioNames.irq, O_RDWR);
-	if (*fd < 0) {
-		sif::error << "PdecHandler::irqOperation: Opening UIO IRQ file" << uioNames.irq << " failed"
-							 << std::endl;
-		triggerEvent(OPEN_IRQ_FILE_FAILED);
-		state = State::WAIT_FOR_RECOVERY;
-	}
+  *fd = open(uioNames.irq, O_RDWR);
+  if (*fd < 0) {
+    sif::error << "PdecHandler::irqOperation: Opening UIO IRQ file" << uioNames.irq << " failed"
+               << std::endl;
+    triggerEvent(OPEN_IRQ_FILE_FAILED);
+    state = State::WAIT_FOR_RECOVERY;
+  }
 }
 
 ReturnValue_t PdecHandler::checkAndHandleIrqs(int fd, uint32_t& info) {
diff --git a/linux/ipcore/PdecHandler.h b/linux/ipcore/PdecHandler.h
index e98e939c..2f0bcca2 100644
--- a/linux/ipcore/PdecHandler.h
+++ b/linux/ipcore/PdecHandler.h
@@ -105,8 +105,7 @@ class PdecHandler : public SystemObject,
   //! [EXPORT] : [COMMENT] Failed to pull PDEC reset to low
   static constexpr Event PDEC_RESET_FAILED = event::makeEvent(SUBSYSTEM_ID, 10, severity::HIGH);
   //! [EXPORT] : [COMMENT] Failed to open the IRQ uio file
-  static constexpr Event OPEN_IRQ_FILE_FAILED =
-      event::makeEvent(SUBSYSTEM_ID, 11, severity::HIGH);
+  static constexpr Event OPEN_IRQ_FILE_FAILED = event::makeEvent(SUBSYSTEM_ID, 11, severity::HIGH);
 
  private:
   static const uint8_t INTERFACE_ID = CLASS_ID::PDEC_HANDLER;
diff --git a/mission/system/objects/CMakeLists.txt b/mission/system/objects/CMakeLists.txt
index 0290a311..1b669047 100644
--- a/mission/system/objects/CMakeLists.txt
+++ b/mission/system/objects/CMakeLists.txt
@@ -7,6 +7,7 @@ target_sources(
           TcsSubsystem.cpp
           PayloadSubsystem.cpp
           AcsBoardAssembly.cpp
+          SyrlinksAssembly.cpp
           Stack5VHandler.cpp
           SusAssembly.cpp
           RwAssembly.cpp
diff --git a/mission/system/objects/ComSubsystem.cpp b/mission/system/objects/ComSubsystem.cpp
index 9df7e72b..b9ed0a05 100644
--- a/mission/system/objects/ComSubsystem.cpp
+++ b/mission/system/objects/ComSubsystem.cpp
@@ -30,6 +30,7 @@ void ComSubsystem::performChildOperation() {
   if (countdownActive) {
     checkTransmitterCountdown();
   }
+
   Subsystem::performChildOperation();
 }
 
@@ -184,7 +185,7 @@ void ComSubsystem::startRxAndTxLowRateSeq() {
 
 void ComSubsystem::checkTransmitterCountdown() {
   if (transmitterCountdown.hasTimedOut()) {
-  	triggerEvent(TX_TIMER_EXPIRED, transmitterTimeout);
+    triggerEvent(TX_TIMER_EXPIRED, transmitterTimeout);
     startTransition(com::Submode::RX_ONLY, SUBMODE_NONE);
     countdownActive = false;
   }
diff --git a/mission/system/objects/ComSubsystem.h b/mission/system/objects/ComSubsystem.h
index 831b4cec..854a877b 100644
--- a/mission/system/objects/ComSubsystem.h
+++ b/mission/system/objects/ComSubsystem.h
@@ -1,24 +1,23 @@
 #ifndef MISSION_SYSTEM_COMSUBSYSTEM_H_
 #define MISSION_SYSTEM_COMSUBSYSTEM_H_
 
+#include <common/config/eive/eventSubsystemIds.h>
 #include <fsfw/events/EventMessage.h>
 #include <fsfw/parameters/HasParametersIF.h>
 #include <fsfw/parameters/ParameterHelper.h>
 #include <fsfw/subsystem/Subsystem.h>
-#include <common/config/eive/eventSubsystemIds.h>
 
 #include "mission/comDefs.h"
 
 class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF {
  public:
+  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::COM_SUBSYSTEM;
 
-	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::COM_SUBSYSTEM;
-
-	//! [EXPORT] : [COMMENT] The transmit timer to protect the Syrlinks expired
-	//! P1: The current timer value
-	static const Event TX_TIMER_EXPIRED = MAKE_EVENT(1, severity::INFO);
-	//! [EXPORT] : [COMMENT] Transmitter will be turned on due to detection of bitlock
-	static const Event BIT_LOCK_TX_ON = MAKE_EVENT(2, severity::INFO);
+  //! [EXPORT] : [COMMENT] The transmit timer to protect the Syrlinks expired
+  //! P1: The current timer value
+  static const Event TX_TIMER_EXPIRED = MAKE_EVENT(1, severity::INFO);
+  //! [EXPORT] : [COMMENT] Transmitter will be turned on due to detection of bitlock
+  static const Event BIT_LOCK_TX_ON = MAKE_EVENT(2, severity::INFO);
 
   /**
    * @brief Constructor
@@ -27,7 +26,8 @@ class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF {
    * @param maxNumberOfSequences
    * @param maxNumberOfTables
    * @param transmitterTimeout	Maximum time the transmitter of the syrlinks
-   * 														will be enabled
+   * 														will
+   * be enabled
    */
   ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables,
                uint32_t transmitterTimeout);
diff --git a/mission/system/objects/DualLaneAssemblyBase.cpp b/mission/system/objects/DualLaneAssemblyBase.cpp
index a55553a0..94f35cf0 100644
--- a/mission/system/objects/DualLaneAssemblyBase.cpp
+++ b/mission/system/objects/DualLaneAssemblyBase.cpp
@@ -235,3 +235,8 @@ void DualLaneAssemblyBase::setPreferredSide(duallane::Submodes submode) {
   }
   this->defaultSubmode = submode;
 }
+
+ReturnValue_t DualLaneAssemblyBase::checkAndHandleHealthState(Mode_t deviceMode,
+                                                              Submode_t deviceSubmode) {
+  return returnvalue::OK;
+}
diff --git a/mission/system/objects/DualLaneAssemblyBase.h b/mission/system/objects/DualLaneAssemblyBase.h
index eadfb77f..4aaf9060 100644
--- a/mission/system/objects/DualLaneAssemblyBase.h
+++ b/mission/system/objects/DualLaneAssemblyBase.h
@@ -74,6 +74,7 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
   MessageQueueId_t getEventReceptionQueue() override;
 
   bool sideSwitchTransition(Mode_t mode, Submode_t submode);
+  ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode);
 
   /**
    * Implemented by user. Will be called if a full mode operation has finished.
diff --git a/mission/system/objects/SyrlinksAssembly.cpp b/mission/system/objects/SyrlinksAssembly.cpp
new file mode 100644
index 00000000..b25fcbc4
--- /dev/null
+++ b/mission/system/objects/SyrlinksAssembly.cpp
@@ -0,0 +1,29 @@
+#include "SyrlinksAssembly.h"
+
+#include <eive/objects.h>
+
+using namespace returnvalue;
+
+SyrlinksAssembly::SyrlinksAssembly(object_id_t objectId) : AssemblyBase(objectId) {}
+
+ReturnValue_t SyrlinksAssembly::commandChildren(Mode_t mode, Submode_t submode) {
+  commandTable[0].setMode(mode);
+  commandTable[0].setSubmode(submode);
+  HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
+  executeTable(iter);
+  return returnvalue::OK;
+}
+
+ReturnValue_t SyrlinksAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
+  if (childrenMap[objects::SYRLINKS_HANDLER].mode != wantedMode) {
+    return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t SyrlinksAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
+  if (mode == MODE_ON or mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_OFF) {
+    return returnvalue::OK;
+  }
+  return returnvalue::FAILED;
+}
diff --git a/mission/system/objects/SyrlinksAssembly.h b/mission/system/objects/SyrlinksAssembly.h
new file mode 100644
index 00000000..a44a3502
--- /dev/null
+++ b/mission/system/objects/SyrlinksAssembly.h
@@ -0,0 +1,17 @@
+#ifndef MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_
+#define MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_
+#include <fsfw/devicehandlers/AssemblyBase.h>
+
+class SyrlinksAssembly : public AssemblyBase {
+ public:
+  SyrlinksAssembly(object_id_t objectId);
+
+ private:
+  FixedArrayList<ModeListEntry, 1> commandTable;
+
+  ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
+  ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
+  ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
+};
+
+#endif /* MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_ */
diff --git a/mission/system/tree/comModeTree.cpp b/mission/system/tree/comModeTree.cpp
index 9dc85a78..dd3aa7a1 100644
--- a/mission/system/tree/comModeTree.cpp
+++ b/mission/system/tree/comModeTree.cpp
@@ -105,11 +105,11 @@ void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh) {
   // Build RX Only table. We could track the state of the CCSDS IP core handler
   // as well but I do not think this is necessary because enabling that should
   // not interfere with the Syrlinks Handler.
-  iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second);
+  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second);
   check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc);
 
   // Build RX Only transition 0
-  iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second);
+  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TRANS_0.second);
   check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TRANS_0.first, &COM_TABLE_RX_ONLY_TRANS_0.second)),
         ctxc);
 
@@ -147,7 +147,7 @@ void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) {
   };
 
   // Build RX and TX low datarate table.
-  iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
+  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
       COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second);
   iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_LOW),
       COM_TABLE_RX_AND_TX_LOW_RATE_TGT.second);
@@ -163,7 +163,7 @@ void buildTxAndRxLowRateSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build TX and RX low transition 1
-  iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
+  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_LOW_DATARATE,
       COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second);
   check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.first,
                                &COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1.second)),
@@ -199,7 +199,7 @@ void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) {
   };
 
   // Build RX and TX high datarate table.
-  iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
+  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
       COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second);
   iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_HIGH),
       COM_TABLE_RX_AND_TX_HIGH_RATE_TGT.second);
@@ -215,7 +215,7 @@ void buildTxAndRxHighRateSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build TX and RX high transition 1
-  iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
+  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_HIGH_DATARATE,
       COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second);
   check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.first,
                                &COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1.second)),
@@ -253,7 +253,7 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) {
   };
 
   // Build RX and TX default datarate table.
-  iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
+  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
       COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second);
   iht(objects::CCSDS_HANDLER, ON, static_cast<Submode_t>(::com::CcsdsSubmode::DATARATE_DEFAULT),
       COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT.second);
@@ -269,7 +269,7 @@ void buildTxAndRxDefaultRateSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build TX and RX default transition 1
-  iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
+  iht(objects::SYRLINKS_ASSY, NML, ::com::Submode::RX_AND_TX_DEFAULT_DATARATE,
       COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second);
   check(ss.addTable(TableEntry(COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.first,
                                &COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1.second)),

From d045498761e88b58b10645cdee540029ea523127 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 1 Mar 2023 19:03:18 +0100
Subject: [PATCH 02/40] allow quicker transition

---
 mission/system/tree/system.cpp | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp
index 5ec10a8c..710698a1 100644
--- a/mission/system/tree/system.cpp
+++ b/mission/system/tree/system.cpp
@@ -87,19 +87,19 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
   // consecutive commanding of TCS and ACS can lead to SPI issues.
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
   iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_SAFE_TRANS_0.second);
+  iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second);
+  iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 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);
+  //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);
+  //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);
@@ -129,19 +129,19 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
 
   // Build SAFE transition 0
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
+  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_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);
+  //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);
+  //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);

From 90175e5c9d21857b51391f072524c4b7c1e39ee1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 11:27:19 +0100
Subject: [PATCH 03/40] add health handling

---
 bsp_q7s/memory/LocalParameterHandler.h      |  3 ++-
 mission/devices/SusHandler.h                |  1 +
 mission/system/objects/SyrlinksAssembly.cpp | 18 ++++++++++++++++++
 mission/system/objects/SyrlinksAssembly.h   |  2 ++
 4 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/bsp_q7s/memory/LocalParameterHandler.h b/bsp_q7s/memory/LocalParameterHandler.h
index 82c5df31..ed0bb2bd 100644
--- a/bsp_q7s/memory/LocalParameterHandler.h
+++ b/bsp_q7s/memory/LocalParameterHandler.h
@@ -19,7 +19,8 @@ class LocalParameterHandler : public NVMParameterBase {
    * @brief	Constructor
    *
    * @param sdRelativeName	Absolute name of json file relative to mount
-   * 												directory of SD card. E.g. conf/example.json
+   * 												directory of SD card.
+   * E.g. conf/example.json
    * @param sdcMan	Pointer to SD card manager
    */
   LocalParameterHandler(std::string sdRelativeName, SdCardMountedIF* sdcMan);
diff --git a/mission/devices/SusHandler.h b/mission/devices/SusHandler.h
index 9f4b35ba..6eb781db 100644
--- a/mission/devices/SusHandler.h
+++ b/mission/devices/SusHandler.h
@@ -3,6 +3,7 @@
 
 #include <eive/eventSubsystemIds.h>
 #include <eive/resultClassIds.h>
+
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 #include "mission/devices/devicedefinitions/acsPolling.h"
diff --git a/mission/system/objects/SyrlinksAssembly.cpp b/mission/system/objects/SyrlinksAssembly.cpp
index b25fcbc4..209097d9 100644
--- a/mission/system/objects/SyrlinksAssembly.cpp
+++ b/mission/system/objects/SyrlinksAssembly.cpp
@@ -10,6 +10,12 @@ ReturnValue_t SyrlinksAssembly::commandChildren(Mode_t mode, Submode_t submode)
   commandTable[0].setMode(mode);
   commandTable[0].setSubmode(submode);
   HybridIterator<ModeListEntry> iter(commandTable.begin(), commandTable.end());
+  if (recoveryState == RECOVERY_IDLE) {
+    ReturnValue_t result = checkAndHandleHealthState(mode, submode);
+    if (result == NEED_TO_CHANGE_HEALTH) {
+      return OK;
+    }
+  }
   executeTable(iter);
   return returnvalue::OK;
 }
@@ -27,3 +33,15 @@ ReturnValue_t SyrlinksAssembly::isModeCombinationValid(Mode_t mode, Submode_t su
   }
   return returnvalue::FAILED;
 }
+
+ReturnValue_t SyrlinksAssembly::checkAndHandleHealthState(Mode_t deviceMode,
+                                                          Submode_t deviceSubmode) {
+  HealthState health = healthHelper.healthTable->getHealth(objects::SYRLINKS_HANDLER);
+  if (health == FAULTY or health == PERMANENT_FAULTY) {
+    overwriteDeviceHealth(objects::SYRLINKS_HANDLER, health);
+    return NEED_TO_CHANGE_HEALTH;
+  } else if (health == EXTERNAL_CONTROL) {
+    modeHelper.setForced(true);
+  }
+  return OK;
+}
diff --git a/mission/system/objects/SyrlinksAssembly.h b/mission/system/objects/SyrlinksAssembly.h
index a44a3502..6d7744ad 100644
--- a/mission/system/objects/SyrlinksAssembly.h
+++ b/mission/system/objects/SyrlinksAssembly.h
@@ -12,6 +12,8 @@ class SyrlinksAssembly : public AssemblyBase {
   ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
   ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
+
+  ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode);
 };
 
 #endif /* MISSION_SYSTEM_OBJECTS_SYRLINKSASSEMBLY_H_ */

From 5f17b1f6e84356cc16a351ab063a67311590ca35 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 11:36:35 +0100
Subject: [PATCH 04/40] allow recovery for syrlinks

---
 mission/system/fdir/SyrlinksFdir.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/mission/system/fdir/SyrlinksFdir.cpp b/mission/system/fdir/SyrlinksFdir.cpp
index 524a94f6..7f7ee42d 100644
--- a/mission/system/fdir/SyrlinksFdir.cpp
+++ b/mission/system/fdir/SyrlinksFdir.cpp
@@ -47,8 +47,8 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) {
       }
       // else
       if (missedReplyCount.incrementAndCheck()) {
-        // handleRecovery(event->getEvent());
-        triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
+        handleRecovery(event->getEvent());
+        //triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
       }
       break;
     case StorageManagerIF::GET_DATA_FAILED:
@@ -80,7 +80,7 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) {
       break;
     case Fuse::POWER_BELOW_LOW_LIMIT:
       // Device might got stuck during boot, retry.
-      // handleRecovery(event->getEvent());
+      handleRecovery(event->getEvent());
       triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
       break;
       //****Thermal*****

From 6104bf7adb1289cd92a1f89b68c3109b4c811c9e Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 13:39:29 +0100
Subject: [PATCH 05/40] set MGM3100 set valid

---
 mission/devices/MgmRm3100CustomHandler.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/mission/devices/MgmRm3100CustomHandler.cpp b/mission/devices/MgmRm3100CustomHandler.cpp
index 685de23d..90fe58e2 100644
--- a/mission/devices/MgmRm3100CustomHandler.cpp
+++ b/mission/devices/MgmRm3100CustomHandler.cpp
@@ -91,6 +91,7 @@ ReturnValue_t MgmRm3100CustomHandler::interpretDeviceReply(DeviceCommandId_t id,
     }
 
     PoolReadGuard pg(&primaryDataset);
+    primaryDataset.setValidity(true, true);
     for (uint8_t idx = 0; idx < 3; idx++) {
       primaryDataset.fieldStrengths[idx] = reply->mgmValuesRaw[idx] * reply->scaleFactors[idx];
     }

From 14188a4f3bef7f112171bb64d300f3fea5c030a4 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 14:42:42 +0100
Subject: [PATCH 06/40] bugfixes imtq

---
 CHANGELOG.md                         |  4 ++++
 mission/controller/AcsController.cpp |  2 --
 mission/devices/ImtqHandler.cpp      | 33 +++++++++++++++-------------
 mission/devices/ImtqHandler.h        |  2 +-
 tmtc                                 |  2 +-
 5 files changed, 24 insertions(+), 19 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 739067cc..54edd113 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -20,6 +20,10 @@ will consitute of a breaking change warranting a new major release:
 
 - Moved polling of all SPI parts to the same PST.
 
+## Fixed
+
+- IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements.
+
 # [v1.33.0]
 
 eive-tmtc: v2.16.2
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index f3269285..c0853695 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -661,7 +661,6 @@ void AcsController::announceMode(bool recursive) {
 }
 
 void AcsController::copyMgmData() {
-  ACS::SensorValues sensorValues;
   {
     PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -806,7 +805,6 @@ void AcsController::copySusData() {
 }
 
 void AcsController::copyGyrData() {
-  ACS::SensorValues sensorValues;
   {
     PoolReadGuard pg(&sensorValues.gyr0AdisSet);
     if (pg.getReadResult() == returnvalue::OK) {
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index aa935ce5..5c5bdb61 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -312,7 +312,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
     result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
     if (result == returnvalue::OK) {
-      fillRawMtmDataset(rawMgmMeasurement);
+      fillRawMtmDataset(rawMtmNoTorque, rawMgmMeasurement);
     } else {
       status = result;
     }
@@ -323,7 +323,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
     result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);
     if (result == returnvalue::OK) {
-      fillRawMtmDataset(calibMgmMeasurement);
+      fillCalibratedMtmDataset(calibMgmMeasurement);
     } else {
       status = result;
     }
@@ -345,7 +345,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
     result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
     if (result == returnvalue::OK) {
-      fillRawMtmDataset(rawMgmMeasurement);
+      fillRawMtmDataset(rawMtmWithTorque, rawMgmMeasurement);
     } else {
       status = result;
     }
@@ -361,7 +361,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     } else {
       status = result;
     }
-    fillEngHkDataset(hkDatasetNoTorque, engHkReply);
+    fillEngHkDataset(hkDatasetWithTorque, engHkReply);
     if (firstReplyCycle) {
       firstReplyCycle = false;
     }
@@ -847,8 +847,11 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
   }
 }
 
-void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
-  PoolReadGuard rg(&rawMtmNoTorque);
+void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
+  PoolReadGuard rg(&set);
+  if(rg.getReadResult() != returnvalue::OK) {
+    sif::error << "ImtqHandler::fillRawMtmDataset: Lock failure" << std::endl;
+  }
   unsigned int offset = 2;
   size_t deSerLen = 16;
   const uint8_t* dataStart = packet + offset;
@@ -876,18 +879,18 @@ void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
   if (res != returnvalue::OK) {
     return;
   }
-  rawMtmNoTorque.mtmRawNt[0] = xRaw * 7.5;
-  rawMtmNoTorque.mtmRawNt[1] = yRaw * 7.5;
-  rawMtmNoTorque.mtmRawNt[2] = zRaw * 7.5;
-  rawMtmNoTorque.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
-  rawMtmNoTorque.setValidity(true, true);
+  set.mtmRawNt[0] = static_cast<float>(xRaw) * 7.5;
+  set.mtmRawNt[1] = static_cast<float>(yRaw) * 7.5;
+  set.mtmRawNt[2] = static_cast<float>(zRaw) * 7.5;
+  set.coilActuationStatus = static_cast<uint8_t>(coilActStatus);
+  set.setValidity(true, true);
   if (debugMode) {
 #if OBSW_VERBOSE_LEVEL >= 1
-    sif::info << "IMTQ raw MTM measurement X: " << rawMtmNoTorque.mtmRawNt[0] << " nT" << std::endl;
-    sif::info << "IMTQ raw MTM measurement Y: " << rawMtmNoTorque.mtmRawNt[1] << " nT" << std::endl;
-    sif::info << "IMTQ raw MTM measurement Z: " << rawMtmNoTorque.mtmRawNt[2] << " nT" << std::endl;
+    sif::info << "IMTQ raw MTM measurement X: " << set.mtmRawNt[0] << " nT" << std::endl;
+    sif::info << "IMTQ raw MTM measurement Y: " << set.mtmRawNt[1] << " nT" << std::endl;
+    sif::info << "IMTQ raw MTM measurement Z: " << set.mtmRawNt[2] << " nT" << std::endl;
     sif::info << "IMTQ coil actuation status during MTM measurement: "
-              << (unsigned int)rawMtmNoTorque.coilActuationStatus.value << std::endl;
+              << (unsigned int)set.coilActuationStatus.value << std::endl;
 #endif
   }
 }
diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h
index a674f85b..419fad33 100644
--- a/mission/devices/ImtqHandler.h
+++ b/mission/devices/ImtqHandler.h
@@ -159,7 +159,7 @@ class ImtqHandler : public DeviceHandlerBase {
    * @param packet    Pointer to the reply data requested with the GET_RAW_MTM_MEASUREMENTS
    *                  command.
    */
-  void fillRawMtmDataset(const uint8_t* packet);
+  void fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet);
 
   /**
    * @brief   This function handles all self test results. This comprises parsing the error byte
diff --git a/tmtc b/tmtc
index 350ffda6..e5eb2e47 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 350ffda6c61b76dc9a6bbf08cec168c29c08136f
+Subproject commit e5eb2e479c6d2ec71e440c0e72cef75a6f43f441

From eaa0fc6cbeb0ce3da0d0ac23b0a7856a8c0cda38 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 14:43:50 +0100
Subject: [PATCH 07/40] bump changelog

---
 CHANGELOG.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 739067cc..c9c811bc 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -19,6 +19,8 @@ will consitute of a breaking change warranting a new major release:
 ## Changed
 
 - Moved polling of all SPI parts to the same PST.
+- Allow quicker transition for the EIVE system component by allowing consecutive TCS and ACS
+  component commanding again.
 
 # [v1.33.0]
 

From 0056ebcb6297cde0755a627cb0f96190d7539e95 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 14:46:20 +0100
Subject: [PATCH 08/40] bump changelog

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 54edd113..0dc4d96e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -23,6 +23,7 @@ will consitute of a breaking change warranting a new major release:
 ## Fixed
 
 - IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements.
+- Set RM3100 dataset to valid.
 
 # [v1.33.0]
 

From 9067c4180ea3d11efcdad61eff4d99d3da18ba18 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 14:48:00 +0100
Subject: [PATCH 09/40] bump tmtc

---
 tmtc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tmtc b/tmtc
index e5eb2e47..2dd850f0 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit e5eb2e479c6d2ec71e440c0e72cef75a6f43f441
+Subproject commit 2dd850f0725d37256c17576bf7d3ae4423184044

From 0fc17624f383c1463f23d5959e49a0e11f035fbc Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 14:59:27 +0100
Subject: [PATCH 10/40] sus brd assy health handling

---
 bsp_q7s/core/CoreController.cpp        |   2 +-
 common/config/eive/definitions.h       |   2 +-
 mission/core/pollingSeqTables.cpp      | 224 ++++++++++++++++---------
 mission/devices/ImtqHandler.cpp        |   2 +-
 mission/devices/SusHandler.h           |   1 +
 mission/system/objects/SusAssembly.cpp |  26 +++
 mission/system/objects/SusAssembly.h   |   1 +
 mission/system/tree/system.cpp         |  10 +-
 8 files changed, 182 insertions(+), 86 deletions(-)

diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp
index 8977bab7..01757f28 100644
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@@ -1224,7 +1224,7 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
       }
     }
     wordIdx++;
-    if(wordIdx >= 10) {
+    if (wordIdx >= 10) {
       break;
     }
   }
diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h
index 3990ac26..99ce4437 100644
--- a/common/config/eive/definitions.h
+++ b/common/config/eive/definitions.h
@@ -83,7 +83,7 @@ static constexpr float SCHED_BLOCK_7_PERIOD = static_cast<float>(SCHED_BLOCK_7_R
 static constexpr float SCHED_BLOCK_8_PERIOD = static_cast<float>(SCHED_BLOCK_8_PLPCDU_MS) / 400.0;
 static constexpr float SCHED_BLOCK_9_PERIOD = static_cast<float>(SCHED_BLOCK_9_RAD_SENS_MS) / 400.0;
 
-}  // namespace acs
+}  // namespace spiSched
 
 }  // namespace config
 
diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp
index 780b0e7f..88b5b95a 100644
--- a/mission/core/pollingSeqTables.cpp
+++ b/mission/core/pollingSeqTables.cpp
@@ -251,53 +251,77 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           DeviceHandlerIF::GET_WRITE);
 
     thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
   }
 
   if (cfg.scheduleStr) {
@@ -313,15 +337,20 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
   if (cfg.scheduleAcsBoard) {
     if (enableAside) {
       // A side
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
 
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
@@ -331,23 +360,31 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
     }
     if (enableBside) {
       // B side
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
 
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
@@ -357,11 +394,14 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
     }
     if (enableAside) {
       thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
@@ -371,21 +411,29 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
     }
     if (enableBside) {
@@ -396,21 +444,29 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
     }
   }
@@ -429,7 +485,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           imtq::ComStep::READ_MEASURE_GET);
   }
 
-  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::spiSched::SCHED_BLOCK_4_PERIOD, 0);
+  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::spiSched::SCHED_BLOCK_4_PERIOD,
+                        0);
 
   if (cfg.scheduleImtq) {
     // This is the torquing cycle.
@@ -491,23 +548,34 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           DeviceHandlerIF::GET_READ);
   }
 
-  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::spiSched::SCHED_BLOCK_RTD_PERIOD, 0);
+  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::spiSched::SCHED_BLOCK_RTD_PERIOD,
+                        0);
 
 #if OBSW_ADD_PL_PCDU == 1
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::GET_READ);
 #endif
 
 #if OBSW_ADD_RAD_SENSORS == 1
   /* Radiation sensor */
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::GET_READ);
 #endif
   return returnvalue::OK;
 }
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index 5c5bdb61..0072dd08 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -849,7 +849,7 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
 
 void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
   PoolReadGuard rg(&set);
-  if(rg.getReadResult() != returnvalue::OK) {
+  if (rg.getReadResult() != returnvalue::OK) {
     sif::error << "ImtqHandler::fillRawMtmDataset: Lock failure" << std::endl;
   }
   unsigned int offset = 2;
diff --git a/mission/devices/SusHandler.h b/mission/devices/SusHandler.h
index 9f4b35ba..6eb781db 100644
--- a/mission/devices/SusHandler.h
+++ b/mission/devices/SusHandler.h
@@ -3,6 +3,7 @@
 
 #include <eive/eventSubsystemIds.h>
 #include <eive/resultClassIds.h>
+
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 #include "mission/devices/devicedefinitions/acsPolling.h"
diff --git a/mission/system/objects/SusAssembly.cpp b/mission/system/objects/SusAssembly.cpp
index 62a47616..9008e7e0 100644
--- a/mission/system/objects/SusAssembly.cpp
+++ b/mission/system/objects/SusAssembly.cpp
@@ -24,6 +24,12 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
     modeTable[idx].setMode(MODE_OFF);
     modeTable[idx].setSubmode(SUBMODE_NONE);
   }
+  if (recoveryState == RecoveryState::RECOVERY_IDLE) {
+    result = checkAndHandleHealthStates(mode, submode);
+    if (result == NEED_TO_CHANGE_HEALTH) {
+      return returnvalue::OK;
+    }
+  }
   if (recoveryState != RecoveryState::RECOVERY_STARTED) {
     if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
       result = handleNormalOrOnModeCmd(mode, submode);
@@ -148,3 +154,23 @@ void SusAssembly::refreshHelperModes() {
     helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
   }
 }
+
+ReturnValue_t SusAssembly::checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode) {
+  using namespace returnvalue;
+  ReturnValue_t status = returnvalue::OK;
+  auto overwriteHealthForOneDev = [&](object_id_t dev) {
+    HealthState health = healthHelper.healthTable->getHealth(dev);
+    if (health == FAULTY or health == PERMANENT_FAULTY) {
+      overwriteDeviceHealth(dev, health);
+      status = NEED_TO_CHANGE_HEALTH;
+    } else if (health == EXTERNAL_CONTROL) {
+      modeHelper.setForced(true);
+    }
+  };
+  if (deviceSubmode == duallane::DUAL_MODE) {
+    for (uint8_t idx = 0; idx < 12; idx++) {
+      overwriteHealthForOneDev(helper.susIds[idx]);
+    }
+  }
+  return status;
+}
diff --git a/mission/system/objects/SusAssembly.h b/mission/system/objects/SusAssembly.h
index e95803df..e993bee4 100644
--- a/mission/system/objects/SusAssembly.h
+++ b/mission/system/objects/SusAssembly.h
@@ -66,6 +66,7 @@ class SusAssembly : public DualLaneAssemblyBase {
   void powerStateMachine(Mode_t mode, Submode_t submode);
   ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
   void refreshHelperModes();
+  ReturnValue_t checkAndHandleHealthStates(Mode_t deviceMode, Submode_t deviceSubmode);
 };
 
 #endif /* MISSION_SYSTEM_SUSASSEMBLY_H_ */
diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp
index 710698a1..195b6905 100644
--- a/mission/system/tree/system.cpp
+++ b/mission/system/tree/system.cpp
@@ -93,13 +93,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build SAFE transition 1
-  //check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_1.first, &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);
+  // 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);
@@ -135,13 +135,13 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build SAFE transition 1
-  //check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_1.first, &EIVE_TABLE_IDLE_TRANS_1.second)),
-   //     ctxc);
+  // 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);
+  // 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);

From 718a440442ce40db891296ba8fb6fd90f8e24c7d Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:05:12 +0100
Subject: [PATCH 11/40] changelog update

---
 CHANGELOG.md                           |   4 +
 bsp_q7s/memory/LocalParameterHandler.h |   4 +-
 common/config/eive/definitions.h       |   2 +-
 mission/core/pollingSeqTables.cpp      | 224 ++++++++++++++++---------
 mission/devices/ImtqHandler.cpp        |   2 +-
 mission/system/fdir/SyrlinksFdir.cpp   |   2 +-
 mission/system/tree/system.cpp         |  10 +-
 7 files changed, 160 insertions(+), 88 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index baad285b..af6d3c87 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -27,6 +27,10 @@ will consitute of a breaking change warranting a new major release:
 - IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements.
 - Set RM3100 dataset to valid.
 
+## Added
+
+- Added Syrlinks Assembly object to allow recovery handling and to fix faulty FDIR behaviour.
+
 # [v1.33.0]
 
 eive-tmtc: v2.16.2
diff --git a/bsp_q7s/memory/LocalParameterHandler.h b/bsp_q7s/memory/LocalParameterHandler.h
index ed0bb2bd..3e800fd1 100644
--- a/bsp_q7s/memory/LocalParameterHandler.h
+++ b/bsp_q7s/memory/LocalParameterHandler.h
@@ -19,8 +19,8 @@ class LocalParameterHandler : public NVMParameterBase {
    * @brief	Constructor
    *
    * @param sdRelativeName	Absolute name of json file relative to mount
-   * 												directory of SD card.
-   * E.g. conf/example.json
+   * 												directory of SD
+   * card. E.g. conf/example.json
    * @param sdcMan	Pointer to SD card manager
    */
   LocalParameterHandler(std::string sdRelativeName, SdCardMountedIF* sdcMan);
diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h
index 3990ac26..99ce4437 100644
--- a/common/config/eive/definitions.h
+++ b/common/config/eive/definitions.h
@@ -83,7 +83,7 @@ static constexpr float SCHED_BLOCK_7_PERIOD = static_cast<float>(SCHED_BLOCK_7_R
 static constexpr float SCHED_BLOCK_8_PERIOD = static_cast<float>(SCHED_BLOCK_8_PLPCDU_MS) / 400.0;
 static constexpr float SCHED_BLOCK_9_PERIOD = static_cast<float>(SCHED_BLOCK_9_RAD_SENS_MS) / 400.0;
 
-}  // namespace acs
+}  // namespace spiSched
 
 }  // namespace config
 
diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp
index 780b0e7f..88b5b95a 100644
--- a/mission/core/pollingSeqTables.cpp
+++ b/mission/core/pollingSeqTables.cpp
@@ -251,53 +251,77 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           DeviceHandlerIF::GET_WRITE);
 
     thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
   }
 
   if (cfg.scheduleStr) {
@@ -313,15 +337,20 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
   if (cfg.scheduleAcsBoard) {
     if (enableAside) {
       // A side
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
 
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
@@ -331,23 +360,31 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
     }
     if (enableBside) {
       // B side
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
 
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
@@ -357,11 +394,14 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
     }
     if (enableAside) {
       thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
@@ -371,21 +411,29 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
     }
     if (enableBside) {
@@ -396,21 +444,29 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
     }
   }
@@ -429,7 +485,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           imtq::ComStep::READ_MEASURE_GET);
   }
 
-  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::spiSched::SCHED_BLOCK_4_PERIOD, 0);
+  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::spiSched::SCHED_BLOCK_4_PERIOD,
+                        0);
 
   if (cfg.scheduleImtq) {
     // This is the torquing cycle.
@@ -491,23 +548,34 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           DeviceHandlerIF::GET_READ);
   }
 
-  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::spiSched::SCHED_BLOCK_RTD_PERIOD, 0);
+  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::spiSched::SCHED_BLOCK_RTD_PERIOD,
+                        0);
 
 #if OBSW_ADD_PL_PCDU == 1
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::GET_READ);
 #endif
 
 #if OBSW_ADD_RAD_SENSORS == 1
   /* Radiation sensor */
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::GET_READ);
 #endif
   return returnvalue::OK;
 }
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index 5c5bdb61..0072dd08 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -849,7 +849,7 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
 
 void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
   PoolReadGuard rg(&set);
-  if(rg.getReadResult() != returnvalue::OK) {
+  if (rg.getReadResult() != returnvalue::OK) {
     sif::error << "ImtqHandler::fillRawMtmDataset: Lock failure" << std::endl;
   }
   unsigned int offset = 2;
diff --git a/mission/system/fdir/SyrlinksFdir.cpp b/mission/system/fdir/SyrlinksFdir.cpp
index 7f7ee42d..2bdbd21d 100644
--- a/mission/system/fdir/SyrlinksFdir.cpp
+++ b/mission/system/fdir/SyrlinksFdir.cpp
@@ -48,7 +48,7 @@ ReturnValue_t SyrlinksFdir::eventReceived(EventMessage* event) {
       // else
       if (missedReplyCount.incrementAndCheck()) {
         handleRecovery(event->getEvent());
-        //triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
+        // triggerEvent(syrlinks::FDIR_REACTION_IGNORED, event->getEvent(), 0);
       }
       break;
     case StorageManagerIF::GET_DATA_FAILED:
diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp
index 710698a1..195b6905 100644
--- a/mission/system/tree/system.cpp
+++ b/mission/system/tree/system.cpp
@@ -93,13 +93,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build SAFE transition 1
-  //check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_1.first, &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);
+  // 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);
@@ -135,13 +135,13 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build SAFE transition 1
-  //check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_1.first, &EIVE_TABLE_IDLE_TRANS_1.second)),
-   //     ctxc);
+  // 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);
+  // 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);

From 4b37a196448bfa96a542175a9cee944d6fd9e7a1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:21:45 +0100
Subject: [PATCH 12/40] try to use more locks with timeouts

---
 bsp_q7s/fs/SdCardManager.cpp         | 24 ++++++++++++------------
 bsp_q7s/fs/SdCardManager.h           |  4 +++-
 fsfw                                 |  2 +-
 mission/config/torquer.h             |  2 ++
 mission/controller/AcsController.cpp |  2 +-
 mission/devices/ImtqHandler.cpp      |  2 +-
 mission/devices/PcduHandler.cpp      | 16 +++++++++-------
 mission/devices/PcduHandler.h        |  5 ++++-
 8 files changed, 33 insertions(+), 24 deletions(-)

diff --git a/bsp_q7s/fs/SdCardManager.cpp b/bsp_q7s/fs/SdCardManager.cpp
index f8da9bee..45c9f0d0 100644
--- a/bsp_q7s/fs/SdCardManager.cpp
+++ b/bsp_q7s/fs/SdCardManager.cpp
@@ -20,14 +20,14 @@
 SdCardManager* SdCardManager::INSTANCE = nullptr;
 
 SdCardManager::SdCardManager() : SystemObject(objects::SDC_MANAGER), cmdExecutor(256) {
-  mutex = MutexFactory::instance()->createMutex();
-  ReturnValue_t result = mutex->lockMutex();
+  sdLock = MutexFactory::instance()->createMutex();
+  ReturnValue_t result = sdLock->lockMutex();
   if (result != returnvalue::OK) {
     sif::error << "SdCardManager::SdCardManager: Mutex lock failed" << std::endl;
   }
   uint8_t prefSdRaw = 0;
   result = scratch::readNumber(scratch::PREFERED_SDC_KEY, prefSdRaw);
-  if (mutex->unlockMutex() != returnvalue::OK) {
+  if (sdLock->unlockMutex() != returnvalue::OK) {
     sif::error << "SdCardManager::SdCardManager: Mutex unlock failed" << std::endl;
   }
 
@@ -195,7 +195,7 @@ ReturnValue_t SdCardManager::setSdCardState(sd::SdCard sdCard, bool on) {
 
 ReturnValue_t SdCardManager::getSdCardsStatus(SdStatePair& active) {
   using namespace std;
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, lockType, lockTimeout);
   if (not filesystem::exists(SD_STATE_FILE)) {
     return STATUS_FILE_NEXISTS;
   }
@@ -378,7 +378,7 @@ void SdCardManager::processSdStatusLine(std::pair<sd::SdState, sd::SdState>& act
 }
 
 std::optional<sd::SdCard> SdCardManager::getPreferredSdCard() const {
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, lockType, lockTimeout);
   auto res = mg.getLockResult();
   if (res != returnvalue::OK) {
     sif::error << "SdCardManager::getPreferredSdCard: Lock error" << std::endl;
@@ -387,7 +387,7 @@ std::optional<sd::SdCard> SdCardManager::getPreferredSdCard() const {
 }
 
 ReturnValue_t SdCardManager::setPreferredSdCard(sd::SdCard sdCard) {
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, lockType, lockTimeout);
   if (sdCard == sd::SdCard::BOTH) {
     return returnvalue::FAILED;
   }
@@ -399,7 +399,7 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() {
   if (cmdExecutor.getCurrentState() == CommandExecutor::States::PENDING) {
     return CommandExecutor::COMMAND_PENDING;
   }
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, lockType, lockTimeout);
   // Use q7hw utility and pipe the command output into the state file
   std::string updateCmd = "q7hw sd info all > " + std::string(SD_STATE_FILE);
   cmdExecutor.load(updateCmd, blocking, printCmdOutput);
@@ -411,7 +411,7 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() {
 }
 
 const char* SdCardManager::getCurrentMountPrefix() const {
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, lockType, lockTimeout);
   if (currentPrefix.has_value()) {
     return currentPrefix.value().c_str();
   }
@@ -464,7 +464,7 @@ void SdCardManager::setPrintCommandOutput(bool print) { this->printCmdOutput = p
 
 bool SdCardManager::isSdCardUsable(std::optional<sd::SdCard> sdCard) {
   {
-    MutexGuard mg(mutex);
+    MutexGuard mg(sdLock, lockType, lockTimeout);
     if (markedUnusable) {
       return false;
     }
@@ -560,7 +560,7 @@ ReturnValue_t SdCardManager::performFsck(sd::SdCard sdcard, bool printOutput, in
 }
 
 void SdCardManager::setActiveSdCard(sd::SdCard sdCard) {
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, lockType, lockTimeout);
   sdInfo.active = sdCard;
   if (sdInfo.active == sd::SdCard::SLOT_0) {
     currentPrefix = config::SD_0_MOUNT_POINT;
@@ -570,7 +570,7 @@ void SdCardManager::setActiveSdCard(sd::SdCard sdCard) {
 }
 
 std::optional<sd::SdCard> SdCardManager::getActiveSdCard() const {
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, lockType, lockTimeout);
   if (markedUnusable) {
     return std::nullopt;
   }
@@ -578,6 +578,6 @@ std::optional<sd::SdCard> SdCardManager::getActiveSdCard() const {
 }
 
 void SdCardManager::markUnusable() {
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, lockType, lockTimeout);
   markedUnusable = true;
 }
diff --git a/bsp_q7s/fs/SdCardManager.h b/bsp_q7s/fs/SdCardManager.h
index 77055589..1b538dad 100644
--- a/bsp_q7s/fs/SdCardManager.h
+++ b/bsp_q7s/fs/SdCardManager.h
@@ -223,7 +223,9 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
   bool sdCardActive = true;
   bool printCmdOutput = true;
   bool markedUnusable = false;
-  MutexIF* mutex = nullptr;
+  MutexIF* sdLock = nullptr;
+  MutexIF::TimeoutType lockType = MutexIF::TimeoutType::WAITING;
+  uint32_t lockTimeout = 40;
 
   SdCardManager();
 
diff --git a/fsfw b/fsfw
index 511d07c0..f8409754 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 511d07c0c78de7b1850e341dfcf8be7589f3c523
+Subproject commit f84097543e59a3564eae4ac19b7118102728c8a9
diff --git a/mission/config/torquer.h b/mission/config/torquer.h
index 10a27991..8241eb83 100644
--- a/mission/config/torquer.h
+++ b/mission/config/torquer.h
@@ -9,6 +9,8 @@ namespace torquer {
 // Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down
 // time of the MGT
 static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20;
+static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+static constexpr uint32_t LOCK_TIMEOUT = 20;
 
 MutexIF* lazyLock();
 extern bool TORQUEING;
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index c0853695..5aa12523 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -429,7 +429,7 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
                                               uint16_t rampTime) {
   {
     PoolReadGuard pg(&dipoleSet);
-    MutexGuard mg(torquer::lazyLock());
+    MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT);
     torquer::NEW_ACTUATION_FLAG = true;
     dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
   }
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index 5c5bdb61..fadf5a63 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -214,7 +214,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
                    << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
                    << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
       }
-      MutexGuard mg(torquer::lazyLock());
+      MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT);
       torquer::TORQUEING = true;
       torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
       rawPacket = commandBuffer;
diff --git a/mission/devices/PcduHandler.cpp b/mission/devices/PcduHandler.cpp
index 46106796..5ed227cd 100644
--- a/mission/devices/PcduHandler.cpp
+++ b/mission/devices/PcduHandler.cpp
@@ -18,7 +18,7 @@ PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize)
   auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
   commandQueue = QueueFactory::instance()->createMessageQueue(
       cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
-  pwrMutex = MutexFactory::instance()->createMutex();
+  pwrLock = MutexFactory::instance()->createMutex();
 }
 
 PCDUHandler::~PCDUHandler() {}
@@ -41,7 +41,7 @@ ReturnValue_t PCDUHandler::performOperation(uint8_t counter) {
     if (pg.getReadResult() == returnvalue::OK) {
       if (switcherSet.p60Dock5VStack.value != switchState) {
         triggerEvent(power::SWITCH_HAS_CHANGED, switchState, pcdu::Switches::P60_DOCK_5V_STACK);
-        MutexGuard mg(pwrMutex);
+        MutexGuard mg(pwrLock);
         switchStates[pcdu::P60_DOCK_5V_STACK] = switchState;
       }
       switcherSet.p60Dock5VStack.setValid(true);
@@ -179,7 +179,7 @@ void PCDUHandler::updatePdu2SwitchStates() {
       switcherSet.pdu2Switches[idx] = pdu2CoreHk.outputEnables[idx];
     }
     switcherSet.pdu2Switches.setValid(true);
-    MutexGuard mg(pwrMutex);
+    MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH0_Q7S, pdu2CoreHk.outputEnables[Channels::Q7S]);
 
     checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8,
@@ -216,7 +216,7 @@ void PCDUHandler::updatePdu1SwitchStates() {
       switcherSet.pdu1Switches[idx] = pdu1CoreHk.outputEnables[idx];
     }
     switcherSet.pdu1Switches.setValid(true);
-    MutexGuard mg(pwrMutex);
+    MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH0_TCS_BOARD_3V3,
                             pdu1CoreHk.outputEnables[Channels::TCS_BOARD_3V3]);
     checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH1_SYRLINKS_12V,
@@ -402,9 +402,11 @@ ReturnValue_t PCDUHandler::getSwitchState(uint8_t switchNr) const {
     sif::debug << "PCDUHandler::getSwitchState: Invalid switch number" << std::endl;
     return returnvalue::FAILED;
   }
-  pwrMutex->lockMutex();
-  uint8_t currentState = switchStates[switchNr];
-  pwrMutex->unlockMutex();
+  uint8_t currentState = 0;
+  {
+    MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+      currentState = switchStates[switchNr];
+  }
   if (currentState == 1) {
     return PowerSwitchIF::SWITCH_ON;
   } else {
diff --git a/mission/devices/PcduHandler.h b/mission/devices/PcduHandler.h
index d1f3996b..45bbd392 100644
--- a/mission/devices/PcduHandler.h
+++ b/mission/devices/PcduHandler.h
@@ -51,7 +51,10 @@ class PCDUHandler : public PowerSwitchIF,
 
  private:
   uint32_t pstIntervalMs = 0;
-  MutexIF* pwrMutex = nullptr;
+  MutexIF* pwrLock = nullptr;
+  static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+  static constexpr uint32_t LOCK_TIMEOUT = 20;
+  static constexpr char LOCK_CTX[] = "PcduHandler";
 
   /** Housekeeping manager. Handles updates of local pool variables. */
   LocalDataPoolManager poolManager;

From 7b539e2fa5089131e4cd9862805066c64fbf33b3 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:22:21 +0100
Subject: [PATCH 13/40] rework lock handling

---
 bsp_q7s/core/CoreController.cpp   |   2 +-
 common/config/eive/definitions.h  |   2 +-
 mission/core/pollingSeqTables.cpp | 224 +++++++++++++++++++-----------
 mission/devices/ImtqHandler.cpp   |   2 +-
 mission/devices/PcduHandler.cpp   |   4 +-
 mission/devices/SusHandler.h      |   1 +
 mission/system/tree/system.cpp    |  10 +-
 7 files changed, 157 insertions(+), 88 deletions(-)

diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp
index 8977bab7..01757f28 100644
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@@ -1224,7 +1224,7 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
       }
     }
     wordIdx++;
-    if(wordIdx >= 10) {
+    if (wordIdx >= 10) {
       break;
     }
   }
diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h
index 3990ac26..99ce4437 100644
--- a/common/config/eive/definitions.h
+++ b/common/config/eive/definitions.h
@@ -83,7 +83,7 @@ static constexpr float SCHED_BLOCK_7_PERIOD = static_cast<float>(SCHED_BLOCK_7_R
 static constexpr float SCHED_BLOCK_8_PERIOD = static_cast<float>(SCHED_BLOCK_8_PLPCDU_MS) / 400.0;
 static constexpr float SCHED_BLOCK_9_PERIOD = static_cast<float>(SCHED_BLOCK_9_RAD_SENS_MS) / 400.0;
 
-}  // namespace acs
+}  // namespace spiSched
 
 }  // namespace config
 
diff --git a/mission/core/pollingSeqTables.cpp b/mission/core/pollingSeqTables.cpp
index 780b0e7f..88b5b95a 100644
--- a/mission/core/pollingSeqTables.cpp
+++ b/mission/core/pollingSeqTables.cpp
@@ -251,53 +251,77 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           DeviceHandlerIF::GET_WRITE);
 
     thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
     thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                          length * config::spiSched::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
+                          length * config::spiSched::SCHED_BLOCK_1_PERIOD,
+                          DeviceHandlerIF::GET_READ);
   }
 
   if (cfg.scheduleStr) {
@@ -313,15 +337,20 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
   if (cfg.scheduleAcsBoard) {
     if (enableAside) {
       // A side
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
 
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
@@ -331,23 +360,31 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
     }
     if (enableBside) {
       // B side
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
 
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
@@ -357,11 +394,14 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
     }
     if (enableAside) {
       thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
@@ -371,21 +411,29 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
     }
     if (enableBside) {
@@ -396,21 +444,29 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::spiSched::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
     }
   }
@@ -429,7 +485,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           imtq::ComStep::READ_MEASURE_GET);
   }
 
-  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::spiSched::SCHED_BLOCK_4_PERIOD, 0);
+  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::spiSched::SCHED_BLOCK_4_PERIOD,
+                        0);
 
   if (cfg.scheduleImtq) {
     // This is the torquing cycle.
@@ -491,23 +548,34 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           DeviceHandlerIF::GET_READ);
   }
 
-  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::spiSched::SCHED_BLOCK_RTD_PERIOD, 0);
+  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::spiSched::SCHED_BLOCK_RTD_PERIOD,
+                        0);
 
 #if OBSW_ADD_PL_PCDU == 1
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::GET_READ);
 #endif
 
 #if OBSW_ADD_RAD_SENSORS == 1
   /* Radiation sensor */
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::GET_READ);
 #endif
   return returnvalue::OK;
 }
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index fadf5a63..c6dfe955 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -849,7 +849,7 @@ void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
 
 void ImtqHandler::fillRawMtmDataset(imtq::RawMtmMeasurementSet& set, const uint8_t* packet) {
   PoolReadGuard rg(&set);
-  if(rg.getReadResult() != returnvalue::OK) {
+  if (rg.getReadResult() != returnvalue::OK) {
     sif::error << "ImtqHandler::fillRawMtmDataset: Lock failure" << std::endl;
   }
   unsigned int offset = 2;
diff --git a/mission/devices/PcduHandler.cpp b/mission/devices/PcduHandler.cpp
index 5ed227cd..f5873fcf 100644
--- a/mission/devices/PcduHandler.cpp
+++ b/mission/devices/PcduHandler.cpp
@@ -41,7 +41,7 @@ ReturnValue_t PCDUHandler::performOperation(uint8_t counter) {
     if (pg.getReadResult() == returnvalue::OK) {
       if (switcherSet.p60Dock5VStack.value != switchState) {
         triggerEvent(power::SWITCH_HAS_CHANGED, switchState, pcdu::Switches::P60_DOCK_5V_STACK);
-        MutexGuard mg(pwrLock);
+        MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
         switchStates[pcdu::P60_DOCK_5V_STACK] = switchState;
       }
       switcherSet.p60Dock5VStack.setValid(true);
@@ -405,7 +405,7 @@ ReturnValue_t PCDUHandler::getSwitchState(uint8_t switchNr) const {
   uint8_t currentState = 0;
   {
     MutexGuard mg(pwrLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-      currentState = switchStates[switchNr];
+    currentState = switchStates[switchNr];
   }
   if (currentState == 1) {
     return PowerSwitchIF::SWITCH_ON;
diff --git a/mission/devices/SusHandler.h b/mission/devices/SusHandler.h
index 9f4b35ba..6eb781db 100644
--- a/mission/devices/SusHandler.h
+++ b/mission/devices/SusHandler.h
@@ -3,6 +3,7 @@
 
 #include <eive/eventSubsystemIds.h>
 #include <eive/resultClassIds.h>
+
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 #include "mission/devices/devicedefinitions/acsPolling.h"
diff --git a/mission/system/tree/system.cpp b/mission/system/tree/system.cpp
index 710698a1..195b6905 100644
--- a/mission/system/tree/system.cpp
+++ b/mission/system/tree/system.cpp
@@ -93,13 +93,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build SAFE transition 1
-  //check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_1.first, &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);
+  // 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);
@@ -135,13 +135,13 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build SAFE transition 1
-  //check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_1.first, &EIVE_TABLE_IDLE_TRANS_1.second)),
-   //     ctxc);
+  // 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);
+  // 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);

From dfb1e88f552d2afada9b8ed68e1a37564e388222 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:32:12 +0100
Subject: [PATCH 14/40] lockslockslocks

---
 linux/devices/AcsBoardPolling.cpp         | 34 +++++++++++------------
 linux/devices/AcsBoardPolling.h           |  3 ++
 mission/config/comCfg.cpp                 |  4 +--
 mission/config/torquer.h                  |  1 +
 mission/controller/AcsController.cpp      |  3 +-
 mission/devices/HeaterHandler.cpp         | 18 ++++++------
 mission/devices/HeaterHandler.h           |  5 +++-
 mission/devices/ImtqHandler.cpp           |  3 +-
 mission/system/objects/Stack5VHandler.cpp |  8 +++---
 mission/system/objects/Stack5VHandler.h   |  4 +++
 10 files changed, 48 insertions(+), 35 deletions(-)

diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp
index 871cfdd0..6a16d816 100644
--- a/linux/devices/AcsBoardPolling.cpp
+++ b/linux/devices/AcsBoardPolling.cpp
@@ -105,7 +105,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::Adis1650XRequest*>(sendData);
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != adis.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         adis.type = req->type;
@@ -135,7 +135,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::GyroL3gRequest*>(sendData);
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != gyro.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         std::memcpy(gyro.sensorCfg, req->ctrlRegs, 5);
@@ -154,7 +154,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::MgmLis3Request*>(sendData);
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != mgm.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         mgm.performStartup = true;
@@ -173,7 +173,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData);
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != mgm.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         mgm.performStartup = true;
@@ -218,7 +218,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       break;
     }
   }
-  MutexGuard mg(ipcLock);
+  MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (state == InternalState::IDLE) {
     state = InternalState::BUSY;
     semaphore->release();
@@ -238,7 +238,7 @@ ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** b
   if (spiCookie == nullptr) {
     return returnvalue::FAILED;
   }
-  MutexGuard mg(ipcLock);
+  MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   auto handleAdisReply = [&](GyroAdis& gyro) {
     std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply));
     *buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
@@ -297,7 +297,7 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
   acs::SimpleSensorMode mode;
   bool gyroPerformStartup;
   {
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mode = l3g.mode;
     gyroPerformStartup = l3g.performStartup;
   }
@@ -320,7 +320,7 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
       if (result != returnvalue::OK) {
         l3g.replyResult = returnvalue::OK;
       }
-      MutexGuard mg(ipcLock);
+      MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       // Cross check configuration as verification that communication is working
       for (uint8_t idx = 0; idx < 5; idx++) {
         if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
@@ -345,7 +345,7 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
       l3g.replyResult = returnvalue::FAILED;
       return;
     }
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     // The regular read function always returns the full sensor config as well. Use that
     // to verify communications.
     for (uint8_t idx = 0; idx < 5; idx++) {
@@ -444,7 +444,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
   bool cdHasTimedOut = false;
   bool mustPerformStartup = false;
   {
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mode = gyro.mode;
     cdHasTimedOut = gyro.countdown.hasTimedOut();
     mustPerformStartup = gyro.performStartup;
@@ -478,7 +478,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
         gyro.replyResult = returnvalue::FAILED;
         return;
       }
-      MutexGuard mg(ipcLock);
+      MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       gyro.ownReply.cfgWasSet = true;
       gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3];
       gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5];
@@ -525,7 +525,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
       return;
     }
 
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     gyro.ownReply.dataWasSet = true;
     gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3];
     gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5];
@@ -545,7 +545,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
   acs::SimpleSensorMode mode;
   bool mustPerformStartup = false;
   {
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mode = mgm.mode;
     mustPerformStartup = mgm.performStartup;
   }
@@ -605,7 +605,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
       return;
     }
     {
-      MutexGuard mg(ipcLock);
+      MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       mgm.ownReply.dataWasSet = true;
       mgm.ownReply.sensitivity = mgmLis3::getSensitivityFactor(mgmLis3::getSensitivity(mgm.cfg[1]));
       mgm.ownReply.mgmValuesRaw[0] =
@@ -627,7 +627,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
       mgm.replyResult = result;
       return;
     }
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mgm.ownReply.temperatureWasSet = true;
     mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1];
   }
@@ -638,7 +638,7 @@ void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
   acs::SimpleSensorMode mode;
   bool mustPerformStartup = false;
   {
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mode = mgm.mode;
     mustPerformStartup = mgm.performStartup;
   }
@@ -712,7 +712,7 @@ void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
       mgm.replyResult = result;
       return;
     }
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     for (uint8_t idx = 0; idx < 3; idx++) {
       // Hardcoded, but note that the gain depends on the cycle count
       // value which is configurable!
diff --git a/linux/devices/AcsBoardPolling.h b/linux/devices/AcsBoardPolling.h
index 9d642dc8..e3c3bd24 100644
--- a/linux/devices/AcsBoardPolling.h
+++ b/linux/devices/AcsBoardPolling.h
@@ -22,6 +22,9 @@ class AcsBoardPolling : public SystemObject,
  private:
   enum class InternalState { IDLE, BUSY } state = InternalState::IDLE;
   MutexIF* ipcLock;
+  static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+  static constexpr uint32_t LOCK_TIMEOUT = 20;
+  static constexpr char LOCK_CTX[] = "AcsBoardPolling";
   SemaphoreIF* semaphore;
   std::array<uint8_t, 32> cmdBuf;
 
diff --git a/mission/config/comCfg.cpp b/mission/config/comCfg.cpp
index 9e350a9d..8387214d 100644
--- a/mission/config/comCfg.cpp
+++ b/mission/config/comCfg.cpp
@@ -9,12 +9,12 @@ MutexIF* DATARATE_LOCK = nullptr;
 MutexIF* lazyLock();
 
 com::Datarate com::getCurrentDatarate() {
-  MutexGuard mg(lazyLock());
+  MutexGuard mg(lazyLock(), MutexIF::TimeoutType::WAITING, 20, "com");
   return DATARATE_CFG_RAW;
 }
 
 void com::setCurrentDatarate(com::Datarate newRate) {
-  MutexGuard mg(lazyLock());
+  MutexGuard mg(lazyLock(), MutexIF::TimeoutType::WAITING, 20, "com");
   DATARATE_CFG_RAW = newRate;
 }
 
diff --git a/mission/config/torquer.h b/mission/config/torquer.h
index 8241eb83..bb5a101f 100644
--- a/mission/config/torquer.h
+++ b/mission/config/torquer.h
@@ -11,6 +11,7 @@ namespace torquer {
 static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20;
 static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
 static constexpr uint32_t LOCK_TIMEOUT = 20;
+static constexpr char LOCK_CTX[] = "torquer";
 
 MutexIF* lazyLock();
 extern bool TORQUEING;
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 5aa12523..58b9dbfc 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -429,7 +429,8 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
                                               uint16_t rampTime) {
   {
     PoolReadGuard pg(&dipoleSet);
-    MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT);
+    MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
+                  torquer::LOCK_CTX);
     torquer::NEW_ACTUATION_FLAG = true;
     dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
   }
diff --git a/mission/devices/HeaterHandler.cpp b/mission/devices/HeaterHandler.cpp
index 2cde7f1e..150c4778 100644
--- a/mission/devices/HeaterHandler.cpp
+++ b/mission/devices/HeaterHandler.cpp
@@ -30,8 +30,8 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, H
   if (mainLineSwitcher == nullptr) {
     throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
   }
-  heaterHealthAndStateMutex = MutexFactory::instance()->createMutex();
-  if (heaterHealthAndStateMutex == nullptr) {
+  handlerLock = MutexFactory::instance()->createMutex();
+  if (handlerLock == nullptr) {
     throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
   }
   auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
@@ -144,7 +144,7 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
   if (action == SwitchAction::SET_SWITCH_ON) {
     HasHealthIF::HealthState health;
     {
-      MutexGuard mg(heaterHealthAndStateMutex);
+      MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       health = heater.healthDevice->getHealth();
     }
     if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
@@ -270,7 +270,7 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
       } else {
         triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
         {
-          MutexGuard mg(heaterHealthAndStateMutex);
+          MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
           heater.switchState = ON;
         }
       }
@@ -320,7 +320,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
       triggerEvent(GPIO_PULL_LOW_FAILED, result);
     } else {
       {
-        MutexGuard mg(heaterHealthAndStateMutex);
+        MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
         heater.switchState = OFF;
       }
       triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
@@ -346,7 +346,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
 }
 
 HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const {
-  MutexGuard mg(heaterHealthAndStateMutex);
+  MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   return heaterVec.at(switchNr).switchState;
 }
 
@@ -396,7 +396,7 @@ object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectI
 
 ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& statesBuf) {
   {
-    MutexGuard mg(heaterHealthAndStateMutex);
+    MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (mg.getLockResult() != returnvalue::OK) {
       return returnvalue::FAILED;
     }
@@ -409,7 +409,7 @@ ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& stat
 
 bool HeaterHandler::allSwitchesOff() {
   bool allSwitchesOrd = false;
-  MutexGuard mg(heaterHealthAndStateMutex);
+  MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
   for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
     allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
@@ -442,7 +442,7 @@ uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
 HasHealthIF::HealthState HeaterHandler::getHealth(heater::Switchers heater) {
   auto* healthDev = heaterVec.at(heater).healthDevice;
   if (healthDev != nullptr) {
-    MutexGuard mg(heaterHealthAndStateMutex);
+    MutexGuard mg(handlerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     return healthDev->getHealth();
   }
   return HasHealthIF::HealthState::FAULTY;
diff --git a/mission/devices/HeaterHandler.h b/mission/devices/HeaterHandler.h
index ffc38440..42f42abf 100644
--- a/mission/devices/HeaterHandler.h
+++ b/mission/devices/HeaterHandler.h
@@ -136,7 +136,10 @@ class HeaterHandler : public ExecutableObjectIF,
 
   HeaterMap heaterVec = {};
 
-  MutexIF* heaterHealthAndStateMutex = nullptr;
+  MutexIF* handlerLock = nullptr;
+  static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+  static constexpr uint32_t LOCK_TIMEOUT = 20;
+  static constexpr char LOCK_CTX[] = "HeaterHandler";
 
   HeaterHelper helper;
   ModeHelper modeHelper;
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index c6dfe955..4d4f3650 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -214,7 +214,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
                    << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
                    << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
       }
-      MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT);
+      MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
+                    torquer::LOCK_CTX);
       torquer::TORQUEING = true;
       torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
       rawPacket = commandBuffer;
diff --git a/mission/system/objects/Stack5VHandler.cpp b/mission/system/objects/Stack5VHandler.cpp
index 1a3141c4..6c3c94d9 100644
--- a/mission/system/objects/Stack5VHandler.cpp
+++ b/mission/system/objects/Stack5VHandler.cpp
@@ -5,7 +5,7 @@ Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) {
 }
 
 ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander, bool updateStates) {
-  MutexGuard mg(stackLock);
+  MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (updateStates) {
     updateInternalStates();
   }
@@ -27,7 +27,7 @@ ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander, bool updateSt
 }
 
 ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander, bool updateStates) {
-  MutexGuard mg(stackLock);
+  MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (updateStates) {
     updateInternalStates();
   }
@@ -55,12 +55,12 @@ ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander, bool updateS
 }
 
 bool Stack5VHandler::isSwitchOn() {
-  MutexGuard mg(stackLock);
+  MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   return updateInternalStates();
 }
 
 void Stack5VHandler::update() {
-  MutexGuard mg(stackLock);
+  MutexGuard mg(stackLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   updateInternalStates();
 }
 
diff --git a/mission/system/objects/Stack5VHandler.h b/mission/system/objects/Stack5VHandler.h
index 46fc963a..364d744b 100644
--- a/mission/system/objects/Stack5VHandler.h
+++ b/mission/system/objects/Stack5VHandler.h
@@ -21,7 +21,11 @@ class Stack5VHandler {
   void update();
 
  private:
+  static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+  static constexpr uint32_t LOCK_TIMEOUT = 20;
+
   MutexIF* stackLock;
+  static constexpr char LOCK_CTX[] = "Stack5VHandler";
   PowerSwitchIF& switcher;
   bool switchIsOn = false;
   bool targetState = false;

From 7b571bc007202e1fd1a2691c5fd7d6befe8d45d5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:38:20 +0100
Subject: [PATCH 15/40] refactore more lock handling

---
 fsfw                                 |  2 +-
 linux/devices/Max31865RtdPolling.cpp | 14 +++++++-------
 linux/devices/Max31865RtdPolling.h   |  5 ++++-
 3 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/fsfw b/fsfw
index f8409754..245886c5 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit f84097543e59a3564eae4ac19b7118102728c8a9
+Subproject commit 245886c55500b9e70ba71eab68c46d44af9f6836
diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp
index e59c2ef2..3071975d 100644
--- a/linux/devices/Max31865RtdPolling.cpp
+++ b/linux/devices/Max31865RtdPolling.cpp
@@ -19,7 +19,7 @@ static constexpr uint8_t BASE_CFG =
 Max31865RtdPolling::Max31865RtdPolling(object_id_t objectId, SpiComIF* lowLevelComIF,
                                        GpioIF* gpioIF)
     : SystemObject(objectId), rtds(EiveMax31855::NUM_RTDS), comIF(lowLevelComIF), gpioIF(gpioIF) {
-  readerMutex = MutexFactory::instance()->createMutex();
+  readerLock = MutexFactory::instance()->createMutex();
 }
 
 ReturnValue_t Max31865RtdPolling::performOperation(uint8_t operationCode) {
@@ -63,7 +63,7 @@ bool Max31865RtdPolling::periodicInitHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerMutex);
+    MutexGuard mg(readerLock);
     if (mg.getLockResult() != returnvalue::OK) {
       sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
       return false;
@@ -119,7 +119,7 @@ ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerMutex);
+    MutexGuard mg(readerLock);
     if (mg.getLockResult() != returnvalue::OK) {
       sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
       return returnvalue::FAILED;
@@ -144,7 +144,7 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerMutex);
+    MutexGuard mg(readerLock);
     if (mg.getLockResult() != returnvalue::OK) {
       sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
       return returnvalue::FAILED;
@@ -200,7 +200,7 @@ ReturnValue_t Max31865RtdPolling::initializeInterface(CookieIF* cookie) {
     throw std::invalid_argument("Invalid RTD index");
   }
   rtds[rtdCookie->idx] = rtdCookie;
-  MutexGuard mg(readerMutex);
+  MutexGuard mg(readerLock);
   if (dbLen == 0) {
     dbLen = rtdCookie->db.getSerializedSize();
   }
@@ -216,7 +216,7 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
   if (sendLen < 1) {
     return returnvalue::OK;
   }
-  MutexGuard mg(readerMutex);
+  MutexGuard mg(readerLock);
   if (mg.getLockResult() != returnvalue::OK) {
     sif::warning << "Max31865RtdReader::sendMessage: Mutex lock failed" << std::endl;
     return returnvalue::FAILED;
@@ -312,7 +312,7 @@ ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t
 
 ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                       size_t* size) {
-  MutexGuard mg(readerMutex);
+  MutexGuard mg(readerLock);
   if (mg.getLockResult() != returnvalue::OK) {
     // TODO: Emit warning
     return returnvalue::FAILED;
diff --git a/linux/devices/Max31865RtdPolling.h b/linux/devices/Max31865RtdPolling.h
index a34c8e53..da27de68 100644
--- a/linux/devices/Max31865RtdPolling.h
+++ b/linux/devices/Max31865RtdPolling.h
@@ -48,7 +48,10 @@ class Max31865RtdPolling : public SystemObject,
   std::vector<Max31865ReaderCookie*> rtds;
   std::array<uint8_t, 4> cmdBuf = {};
   size_t dbLen = 0;
-  MutexIF* readerMutex;
+  MutexIF* readerLock;
+  static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+  static constexpr uint32_t LOCK_TIMEOUT = 20;
+  static constexpr char LOCK_CTX[] = "Max31865RtdPolling";
 
   SpiComIF* comIF;
   GpioIF* gpioIF;

From ad4ab9835652dd057692aa5aa1ba04e134dbdc1e Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:41:09 +0100
Subject: [PATCH 16/40] lock context for sd card manager

---
 bsp_q7s/fs/SdCardManager.cpp | 18 +++++++++---------
 bsp_q7s/fs/SdCardManager.h   |  5 +++--
 2 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/bsp_q7s/fs/SdCardManager.cpp b/bsp_q7s/fs/SdCardManager.cpp
index 45c9f0d0..1663f7cc 100644
--- a/bsp_q7s/fs/SdCardManager.cpp
+++ b/bsp_q7s/fs/SdCardManager.cpp
@@ -195,7 +195,7 @@ ReturnValue_t SdCardManager::setSdCardState(sd::SdCard sdCard, bool on) {
 
 ReturnValue_t SdCardManager::getSdCardsStatus(SdStatePair& active) {
   using namespace std;
-  MutexGuard mg(sdLock, lockType, lockTimeout);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (not filesystem::exists(SD_STATE_FILE)) {
     return STATUS_FILE_NEXISTS;
   }
@@ -378,7 +378,7 @@ void SdCardManager::processSdStatusLine(std::pair<sd::SdState, sd::SdState>& act
 }
 
 std::optional<sd::SdCard> SdCardManager::getPreferredSdCard() const {
-  MutexGuard mg(sdLock, lockType, lockTimeout);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   auto res = mg.getLockResult();
   if (res != returnvalue::OK) {
     sif::error << "SdCardManager::getPreferredSdCard: Lock error" << std::endl;
@@ -387,7 +387,7 @@ std::optional<sd::SdCard> SdCardManager::getPreferredSdCard() const {
 }
 
 ReturnValue_t SdCardManager::setPreferredSdCard(sd::SdCard sdCard) {
-  MutexGuard mg(sdLock, lockType, lockTimeout);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (sdCard == sd::SdCard::BOTH) {
     return returnvalue::FAILED;
   }
@@ -399,7 +399,7 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() {
   if (cmdExecutor.getCurrentState() == CommandExecutor::States::PENDING) {
     return CommandExecutor::COMMAND_PENDING;
   }
-  MutexGuard mg(sdLock, lockType, lockTimeout);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   // Use q7hw utility and pipe the command output into the state file
   std::string updateCmd = "q7hw sd info all > " + std::string(SD_STATE_FILE);
   cmdExecutor.load(updateCmd, blocking, printCmdOutput);
@@ -411,7 +411,7 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() {
 }
 
 const char* SdCardManager::getCurrentMountPrefix() const {
-  MutexGuard mg(sdLock, lockType, lockTimeout);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (currentPrefix.has_value()) {
     return currentPrefix.value().c_str();
   }
@@ -464,7 +464,7 @@ void SdCardManager::setPrintCommandOutput(bool print) { this->printCmdOutput = p
 
 bool SdCardManager::isSdCardUsable(std::optional<sd::SdCard> sdCard) {
   {
-    MutexGuard mg(sdLock, lockType, lockTimeout);
+    MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (markedUnusable) {
       return false;
     }
@@ -560,7 +560,7 @@ ReturnValue_t SdCardManager::performFsck(sd::SdCard sdcard, bool printOutput, in
 }
 
 void SdCardManager::setActiveSdCard(sd::SdCard sdCard) {
-  MutexGuard mg(sdLock, lockType, lockTimeout);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   sdInfo.active = sdCard;
   if (sdInfo.active == sd::SdCard::SLOT_0) {
     currentPrefix = config::SD_0_MOUNT_POINT;
@@ -570,7 +570,7 @@ void SdCardManager::setActiveSdCard(sd::SdCard sdCard) {
 }
 
 std::optional<sd::SdCard> SdCardManager::getActiveSdCard() const {
-  MutexGuard mg(sdLock, lockType, lockTimeout);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (markedUnusable) {
     return std::nullopt;
   }
@@ -578,6 +578,6 @@ std::optional<sd::SdCard> SdCardManager::getActiveSdCard() const {
 }
 
 void SdCardManager::markUnusable() {
-  MutexGuard mg(sdLock, lockType, lockTimeout);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   markedUnusable = true;
 }
diff --git a/bsp_q7s/fs/SdCardManager.h b/bsp_q7s/fs/SdCardManager.h
index 1b538dad..0a7b92c9 100644
--- a/bsp_q7s/fs/SdCardManager.h
+++ b/bsp_q7s/fs/SdCardManager.h
@@ -224,8 +224,9 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
   bool printCmdOutput = true;
   bool markedUnusable = false;
   MutexIF* sdLock = nullptr;
-  MutexIF::TimeoutType lockType = MutexIF::TimeoutType::WAITING;
-  uint32_t lockTimeout = 40;
+  static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+  static constexpr uint32_t LOCK_TIMEOUT = 40;
+  static constexpr char LOCK_CTX[] = "SdCardManager";
 
   SdCardManager();
 

From f43a93842e9a478929bb00f93852765a629071ac Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:41:49 +0100
Subject: [PATCH 17/40] max31865 lock improvements

---
 linux/devices/Max31865RtdPolling.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp
index 3071975d..004e7c60 100644
--- a/linux/devices/Max31865RtdPolling.cpp
+++ b/linux/devices/Max31865RtdPolling.cpp
@@ -63,7 +63,7 @@ bool Max31865RtdPolling::periodicInitHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock);
+    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (mg.getLockResult() != returnvalue::OK) {
       sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
       return false;
@@ -119,7 +119,7 @@ ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock);
+    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (mg.getLockResult() != returnvalue::OK) {
       sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
       return returnvalue::FAILED;
@@ -144,7 +144,7 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock);
+    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (mg.getLockResult() != returnvalue::OK) {
       sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
       return returnvalue::FAILED;
@@ -200,7 +200,7 @@ ReturnValue_t Max31865RtdPolling::initializeInterface(CookieIF* cookie) {
     throw std::invalid_argument("Invalid RTD index");
   }
   rtds[rtdCookie->idx] = rtdCookie;
-  MutexGuard mg(readerLock);
+  MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (dbLen == 0) {
     dbLen = rtdCookie->db.getSerializedSize();
   }
@@ -216,7 +216,7 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
   if (sendLen < 1) {
     return returnvalue::OK;
   }
-  MutexGuard mg(readerLock);
+  MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (mg.getLockResult() != returnvalue::OK) {
     sif::warning << "Max31865RtdReader::sendMessage: Mutex lock failed" << std::endl;
     return returnvalue::FAILED;
@@ -312,7 +312,7 @@ ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t
 
 ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                       size_t* size) {
-  MutexGuard mg(readerLock);
+  MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (mg.getLockResult() != returnvalue::OK) {
     // TODO: Emit warning
     return returnvalue::FAILED;

From 25dfb54be01d1164b8976e5f83b97066e4c08915 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 15:44:07 +0100
Subject: [PATCH 18/40] bump changelog

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index baad285b..ed671555 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -21,6 +21,7 @@ will consitute of a breaking change warranting a new major release:
 - Moved polling of all SPI parts to the same PST.
 - Allow quicker transition for the EIVE system component by allowing consecutive TCS and ACS
   component commanding again.
+- Changed a lot of lock guards to use timeouts
 
 ## Fixed
 

From 6530ad8f1a9b227b042c109ee1d4df052cd103c8 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 17:08:44 +0100
Subject: [PATCH 19/40] re-gen objects

---
 .../fsfwconfig/events/translateEvents.cpp     |  2 +-
 .../fsfwconfig/objects/translateObjects.cpp   | 11 ++++++----
 bsp_q7s/core/scheduling.cpp                   |  4 ++--
 common/config/eive/objects.h                  |  2 +-
 generators/bsp_hosted_objects.csv             |  3 ++-
 generators/bsp_q7s_objects.csv                |  3 ++-
 generators/events/translateEvents.cpp         |  2 +-
 generators/objects/translateObjects.cpp       | 11 ++++++----
 linux/fsfwconfig/events/translateEvents.cpp   |  2 +-
 linux/fsfwconfig/objects/translateObjects.cpp | 11 ++++++----
 mission/core/GenericFactory.cpp               |  2 +-
 mission/system/tree/acsModeTree.cpp           | 20 +++++++++----------
 tmtc                                          |  2 +-
 13 files changed, 43 insertions(+), 32 deletions(-)

diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
index 23a43738..8acf72a4 100644
--- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp
+++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
  * @brief    Auto-generated event translation file. Contains 267 translations.
  * @details
- * Generated on: 2023-03-01 18:34:32
+ * Generated on: 2023-03-02 17:08:11
  */
 #include "translateEvents.h"
 
diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
index 5cfde56e..da4101a4 100644
--- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
+++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 157 translations.
- * Generated on: 2023-03-01 18:34:32
+ * Contains 158 translations.
+ * Generated on: 2023-03-02 17:08:11
  */
 #include "translateObjects.h"
 
@@ -142,8 +142,9 @@ const char *HEATER_7_HPA_STRING = "HEATER_7_HPA";
 const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS";
 const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS";
 const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS";
-const char *RW_ASS_STRING = "RW_ASS";
+const char *RW_ASSY_STRING = "RW_ASSY";
 const char *CAM_SWITCHER_STRING = "CAM_SWITCHER";
+const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY";
 const char *TM_FUNNEL_STRING = "TM_FUNNEL";
 const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL";
 const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL";
@@ -439,9 +440,11 @@ const char *translateObject(object_id_t object) {
     case 0x73000003:
       return TCS_BOARD_ASS_STRING;
     case 0x73000004:
-      return RW_ASS_STRING;
+      return RW_ASSY_STRING;
     case 0x73000006:
       return CAM_SWITCHER_STRING;
+    case 0x73000007:
+      return SYRLINKS_ASSY_STRING;
     case 0x73000100:
       return TM_FUNNEL_STRING;
     case 0x73000101:
diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp
index c0959222..93f5228f 100644
--- a/bsp_q7s/core/scheduling.cpp
+++ b/bsp_q7s/core/scheduling.cpp
@@ -247,9 +247,9 @@ void scheduling::initTasks() {
   }
 #endif /* OBSW_ADD_ACS_HANDLERS */
 #if OBSW_ADD_RW == 1
-  result = acsSysTask->addComponent(objects::RW_ASS);
+  result = acsSysTask->addComponent(objects::RW_ASSY);
   if (result != returnvalue::OK) {
-    scheduling::printAddObjectError("RW_ASS", objects::RW_ASS);
+    scheduling::printAddObjectError("RW_ASS", objects::RW_ASSY);
   }
 #endif
 #if OBSW_ADD_SUS_BOARD_ASS == 1
diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h
index b5715b55..d294a40e 100644
--- a/common/config/eive/objects.h
+++ b/common/config/eive/objects.h
@@ -142,7 +142,7 @@ enum commonObjects : uint32_t {
   ACS_BOARD_ASS = 0x73000001,
   SUS_BOARD_ASS = 0x73000002,
   TCS_BOARD_ASS = 0x73000003,
-  RW_ASS = 0x73000004,
+  RW_ASSY = 0x73000004,
   CAM_SWITCHER = 0x73000006,
   SYRLINKS_ASSY = 0x73000007,
   EIVE_SYSTEM = 0x73010000,
diff --git a/generators/bsp_hosted_objects.csv b/generators/bsp_hosted_objects.csv
index f20c8d40..f85bfd73 100644
--- a/generators/bsp_hosted_objects.csv
+++ b/generators/bsp_hosted_objects.csv
@@ -134,8 +134,9 @@
 0x73000001;ACS_BOARD_ASS
 0x73000002;SUS_BOARD_ASS
 0x73000003;TCS_BOARD_ASS
-0x73000004;RW_ASS
+0x73000004;RW_ASSY
 0x73000006;CAM_SWITCHER
+0x73000007;SYRLINKS_ASSY
 0x73000100;TM_FUNNEL
 0x73000101;PUS_TM_FUNNEL
 0x73000102;CFDP_TM_FUNNEL
diff --git a/generators/bsp_q7s_objects.csv b/generators/bsp_q7s_objects.csv
index 7dd8b859..7002b3ac 100644
--- a/generators/bsp_q7s_objects.csv
+++ b/generators/bsp_q7s_objects.csv
@@ -139,8 +139,9 @@
 0x73000001;ACS_BOARD_ASS
 0x73000002;SUS_BOARD_ASS
 0x73000003;TCS_BOARD_ASS
-0x73000004;RW_ASS
+0x73000004;RW_ASSY
 0x73000006;CAM_SWITCHER
+0x73000007;SYRLINKS_ASSY
 0x73000100;TM_FUNNEL
 0x73000101;PUS_TM_FUNNEL
 0x73000102;CFDP_TM_FUNNEL
diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp
index 23a43738..8acf72a4 100644
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
  * @brief    Auto-generated event translation file. Contains 267 translations.
  * @details
- * Generated on: 2023-03-01 18:34:32
+ * Generated on: 2023-03-02 17:08:11
  */
 #include "translateEvents.h"
 
diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp
index 2eca7d22..1fe48b77 100644
--- a/generators/objects/translateObjects.cpp
+++ b/generators/objects/translateObjects.cpp
@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 161 translations.
- * Generated on: 2023-03-01 18:34:32
+ * Contains 162 translations.
+ * Generated on: 2023-03-02 17:08:11
  */
 #include "translateObjects.h"
 
@@ -147,8 +147,9 @@ const char *HEATER_7_HPA_STRING = "HEATER_7_HPA";
 const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS";
 const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS";
 const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS";
-const char *RW_ASS_STRING = "RW_ASS";
+const char *RW_ASSY_STRING = "RW_ASSY";
 const char *CAM_SWITCHER_STRING = "CAM_SWITCHER";
+const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY";
 const char *TM_FUNNEL_STRING = "TM_FUNNEL";
 const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL";
 const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL";
@@ -453,9 +454,11 @@ const char *translateObject(object_id_t object) {
     case 0x73000003:
       return TCS_BOARD_ASS_STRING;
     case 0x73000004:
-      return RW_ASS_STRING;
+      return RW_ASSY_STRING;
     case 0x73000006:
       return CAM_SWITCHER_STRING;
+    case 0x73000007:
+      return SYRLINKS_ASSY_STRING;
     case 0x73000100:
       return TM_FUNNEL_STRING;
     case 0x73000101:
diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp
index 23a43738..8acf72a4 100644
--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
  * @brief    Auto-generated event translation file. Contains 267 translations.
  * @details
- * Generated on: 2023-03-01 18:34:32
+ * Generated on: 2023-03-02 17:08:11
  */
 #include "translateEvents.h"
 
diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp
index 2eca7d22..1fe48b77 100644
--- a/linux/fsfwconfig/objects/translateObjects.cpp
+++ b/linux/fsfwconfig/objects/translateObjects.cpp
@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 161 translations.
- * Generated on: 2023-03-01 18:34:32
+ * Contains 162 translations.
+ * Generated on: 2023-03-02 17:08:11
  */
 #include "translateObjects.h"
 
@@ -147,8 +147,9 @@ const char *HEATER_7_HPA_STRING = "HEATER_7_HPA";
 const char *ACS_BOARD_ASS_STRING = "ACS_BOARD_ASS";
 const char *SUS_BOARD_ASS_STRING = "SUS_BOARD_ASS";
 const char *TCS_BOARD_ASS_STRING = "TCS_BOARD_ASS";
-const char *RW_ASS_STRING = "RW_ASS";
+const char *RW_ASSY_STRING = "RW_ASSY";
 const char *CAM_SWITCHER_STRING = "CAM_SWITCHER";
+const char *SYRLINKS_ASSY_STRING = "SYRLINKS_ASSY";
 const char *TM_FUNNEL_STRING = "TM_FUNNEL";
 const char *PUS_TM_FUNNEL_STRING = "PUS_TM_FUNNEL";
 const char *CFDP_TM_FUNNEL_STRING = "CFDP_TM_FUNNEL";
@@ -453,9 +454,11 @@ const char *translateObject(object_id_t object) {
     case 0x73000003:
       return TCS_BOARD_ASS_STRING;
     case 0x73000004:
-      return RW_ASS_STRING;
+      return RW_ASSY_STRING;
     case 0x73000006:
       return CAM_SWITCHER_STRING;
+    case 0x73000007:
+      return SYRLINKS_ASSY_STRING;
     case 0x73000100:
       return TM_FUNNEL_STRING;
     case 0x73000101:
diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp
index 9e929e83..8f54c05d 100644
--- a/mission/core/GenericFactory.cpp
+++ b/mission/core/GenericFactory.cpp
@@ -236,7 +236,7 @@ void ObjectFactory::createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t the
                                  std::array<DeviceHandlerBase*, 4> rws,
                                  std::array<object_id_t, 4> rwIds) {
   RwHelper rwHelper(rwIds);
-  auto* rwAss = new RwAssembly(objects::RW_ASS, &pwrSwitcher, theSwitch, rwHelper);
+  auto* rwAss = new RwAssembly(objects::RW_ASSY, &pwrSwitcher, theSwitch, rwHelper);
   for (size_t idx = 0; idx < rwIds.size(); idx++) {
     ReturnValue_t result = rws[idx]->connectModeTreeParent(*rwAss);
     if (result != returnvalue::OK) {
diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp
index 62fb76ed..b191c1e1 100644
--- a/mission/system/tree/acsModeTree.cpp
+++ b/mission/system/tree/acsModeTree.cpp
@@ -110,7 +110,7 @@ Subsystem& satsystem::acs::init() {
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
-  iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
+  iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
   iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
   check(ACS_SUBSYSTEM.addTable(
             TableEntry(ACS_TABLE_PTG_TRANS_0.first, &ACS_TABLE_PTG_TRANS_0.second)),
@@ -165,7 +165,7 @@ 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::RW_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
+  iht(objects::RW_ASSY, 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);
 
   // Build OFF sequence
@@ -207,7 +207,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::IMTQ_HANDLER, 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);
+  iht(objects::RW_ASSY, 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);
 
@@ -262,7 +262,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
   iht(objects::STAR_TRACKER, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
-  iht(objects::RW_ASS, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
+  iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_DETUMBLE_TRANS_0.second);
   check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_0.second, ACS_TABLE_DETUMBLE_TRANS_0.first, false,
                     true),
         ctxc);
@@ -305,7 +305,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
   // Build IDLE target
   iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second);
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second);
-  iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
+  iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
   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);
@@ -316,7 +316,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
   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);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
-  iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
+  iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
   iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
   ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true);
 
@@ -358,7 +358,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
-  iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
+  iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true),
         ctxc);
@@ -407,7 +407,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
-  iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
+  iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first,
                                &ACS_TABLE_PTG_TARGET_NADIR_TGT.second)),
@@ -458,7 +458,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
-  iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
+  iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   check(ss.addTable(
             TableEntry(ACS_TABLE_PTG_TARGET_GS_TGT.first, &ACS_TABLE_PTG_TARGET_GS_TGT.second)),
@@ -508,7 +508,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
-  iht(objects::RW_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
+  iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   iht(objects::STAR_TRACKER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first,
                                &ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second)),
diff --git a/tmtc b/tmtc
index 2dd850f0..68c46a29 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 2dd850f0725d37256c17576bf7d3ae4423184044
+Subproject commit 68c46a299fd643a34510de315277a16e744999fe

From 196aca241e8e49b18a2cf40ead3f1a6dc0499605 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 17:25:32 +0100
Subject: [PATCH 20/40] add missing command table init

---
 mission/system/objects/SyrlinksAssembly.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/mission/system/objects/SyrlinksAssembly.cpp b/mission/system/objects/SyrlinksAssembly.cpp
index 209097d9..88c946a9 100644
--- a/mission/system/objects/SyrlinksAssembly.cpp
+++ b/mission/system/objects/SyrlinksAssembly.cpp
@@ -4,7 +4,13 @@
 
 using namespace returnvalue;
 
-SyrlinksAssembly::SyrlinksAssembly(object_id_t objectId) : AssemblyBase(objectId) {}
+SyrlinksAssembly::SyrlinksAssembly(object_id_t objectId) : AssemblyBase(objectId) {
+  ModeListEntry entry;
+  entry.setObject(objects::SYRLINKS_HANDLER);
+  entry.setMode(MODE_OFF);
+  entry.setSubmode(SUBMODE_NONE);
+  commandTable.insert(entry);
+}
 
 ReturnValue_t SyrlinksAssembly::commandChildren(Mode_t mode, Submode_t submode) {
   commandTable[0].setMode(mode);

From 35caddbfc44b18a6e5bcd118ba2f4be36cdc9273 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 17:27:47 +0100
Subject: [PATCH 21/40] handle children lost mode

---
 bsp_q7s/memory/LocalParameterHandler.h      | 4 ++--
 mission/system/objects/SyrlinksAssembly.cpp | 4 ++++
 mission/system/objects/SyrlinksAssembly.h   | 1 +
 3 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/bsp_q7s/memory/LocalParameterHandler.h b/bsp_q7s/memory/LocalParameterHandler.h
index 3e800fd1..4aaa8cd5 100644
--- a/bsp_q7s/memory/LocalParameterHandler.h
+++ b/bsp_q7s/memory/LocalParameterHandler.h
@@ -19,8 +19,8 @@ class LocalParameterHandler : public NVMParameterBase {
    * @brief	Constructor
    *
    * @param sdRelativeName	Absolute name of json file relative to mount
-   * 												directory of SD
-   * card. E.g. conf/example.json
+   * 												directory of
+   * SD card. E.g. conf/example.json
    * @param sdcMan	Pointer to SD card manager
    */
   LocalParameterHandler(std::string sdRelativeName, SdCardMountedIF* sdcMan);
diff --git a/mission/system/objects/SyrlinksAssembly.cpp b/mission/system/objects/SyrlinksAssembly.cpp
index 88c946a9..b5e50924 100644
--- a/mission/system/objects/SyrlinksAssembly.cpp
+++ b/mission/system/objects/SyrlinksAssembly.cpp
@@ -51,3 +51,7 @@ ReturnValue_t SyrlinksAssembly::checkAndHandleHealthState(Mode_t deviceMode,
   }
   return OK;
 }
+
+void SyrlinksAssembly::handleChildrenLostMode(ReturnValue_t result) {
+  startTransition(mode, submode);
+}
diff --git a/mission/system/objects/SyrlinksAssembly.h b/mission/system/objects/SyrlinksAssembly.h
index 6d7744ad..314474d3 100644
--- a/mission/system/objects/SyrlinksAssembly.h
+++ b/mission/system/objects/SyrlinksAssembly.h
@@ -12,6 +12,7 @@ class SyrlinksAssembly : public AssemblyBase {
   ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
   ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
+  void handleChildrenLostMode(ReturnValue_t result) override;
 
   ReturnValue_t checkAndHandleHealthState(Mode_t deviceMode, Submode_t deviceSubmode);
 };

From d13e593f8994154a7e8b2bb987e93f27b3eeea1a Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 17:35:36 +0100
Subject: [PATCH 22/40] more useful printout

---
 bsp_q7s/memory/LocalParameterHandler.h |  4 ++--
 mission/core/GenericFactory.cpp        |  5 +++--
 mission/tmtc/TmFunnelBase.cpp          |  8 ++++----
 mission/tmtc/TmFunnelBase.h            | 13 +++++++++----
 4 files changed, 18 insertions(+), 12 deletions(-)

diff --git a/bsp_q7s/memory/LocalParameterHandler.h b/bsp_q7s/memory/LocalParameterHandler.h
index 4aaa8cd5..cdbcff06 100644
--- a/bsp_q7s/memory/LocalParameterHandler.h
+++ b/bsp_q7s/memory/LocalParameterHandler.h
@@ -19,8 +19,8 @@ class LocalParameterHandler : public NVMParameterBase {
    * @brief	Constructor
    *
    * @param sdRelativeName	Absolute name of json file relative to mount
-   * 												directory of
-   * SD card. E.g. conf/example.json
+   * 												directory
+   * of SD card. E.g. conf/example.json
    * @param sdcMan	Pointer to SD card manager
    */
   LocalParameterHandler(std::string sdRelativeName, SdCardMountedIF* sdcMan);
diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp
index 8f54c05d..87b08b49 100644
--- a/mission/core/GenericFactory.cpp
+++ b/mission/core/GenericFactory.cpp
@@ -139,9 +139,10 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
       new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR);
   new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
 
-  PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, *tmStore, *ipcStore, 50);
+  PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, "CfdpTmFunnel", *tmStore, *ipcStore,
+                                       50);
   *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, config::EIVE_CFDP_APID);
-  PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, *tmStore, *ipcStore,
+  PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, "PusTmFunnel", *tmStore, *ipcStore,
                                       config::MAX_PUS_FUNNEL_QUEUE_DEPTH);
   *pusFunnel = new PusTmFunnel(pusFunnelCfg, *timeStamper, sdcMan);
 #if OBSW_ADD_TCPIP_SERVERS == 1
diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp
index 6f788fc7..78a13fab 100644
--- a/mission/tmtc/TmFunnelBase.cpp
+++ b/mission/tmtc/TmFunnelBase.cpp
@@ -5,7 +5,7 @@
 #include "fsfw/ipc/QueueFactory.h"
 
 TmFunnelBase::TmFunnelBase(FunnelCfg cfg)
-    : SystemObject(cfg.objectId), tmStore(cfg.tmStore), ipcStore(cfg.ipcStore) {
+    : SystemObject(cfg.objectId), name(cfg.name), tmStore(cfg.tmStore), ipcStore(cfg.ipcStore) {
   tmQueue = QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth);
 }
 
@@ -37,8 +37,7 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
           message.setStorageId(storeId);
         } else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-          sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
-                     << std::endl;
+          sif::error << name << "::handlePacket: Store too full to create data copy" << std::endl;
 #endif
         }
       } else {
@@ -48,7 +47,8 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
     result = tmQueue->sendMessage(dest.queueId, &message);
     if (result != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
+      sif::error << name << "::handlePacket: Error sending TM to downlink handler " << dest.name
+                 << std::endl;
 #endif
       tmStore.deleteData(message.getStorageId());
     }
diff --git a/mission/tmtc/TmFunnelBase.h b/mission/tmtc/TmFunnelBase.h
index af65771f..ef3c6c6a 100644
--- a/mission/tmtc/TmFunnelBase.h
+++ b/mission/tmtc/TmFunnelBase.h
@@ -12,10 +12,15 @@
 class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
  public:
   struct FunnelCfg {
-    FunnelCfg(object_id_t objId, StorageManagerIF& tmStore, StorageManagerIF& ipcStore,
-              uint32_t tmMsgDepth)
-        : objectId(objId), tmStore(tmStore), ipcStore(ipcStore), tmMsgDepth(tmMsgDepth) {}
+    FunnelCfg(object_id_t objId, const char* name, StorageManagerIF& tmStore,
+              StorageManagerIF& ipcStore, uint32_t tmMsgDepth)
+        : objectId(objId),
+          name(name),
+          tmStore(tmStore),
+          ipcStore(ipcStore),
+          tmMsgDepth(tmMsgDepth) {}
     object_id_t objectId;
+    const char* name;
     StorageManagerIF& tmStore;
     StorageManagerIF& ipcStore;
     uint32_t tmMsgDepth;
@@ -30,6 +35,7 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
   ~TmFunnelBase() override;
 
  protected:
+  const char* name;
   StorageManagerIF& tmStore;
   StorageManagerIF& ipcStore;
 
@@ -43,7 +49,6 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
   };
 
   std::vector<Destination> destinations;
-
   MessageQueueIF* tmQueue = nullptr;
 };
 

From 3e2338f3a47b1e1607abc3d648904663a78116ff Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 17:52:36 +0100
Subject: [PATCH 23/40] fixed units used in controler calculations

---
 mission/controller/AcsController.cpp        | 19 +++++---
 mission/controller/acs/control/Detumble.cpp | 14 +++---
 mission/controller/acs/control/SafeCtrl.cpp | 54 +++++++++------------
 mission/controller/acs/control/SafeCtrl.h   | 12 ++---
 4 files changed, 46 insertions(+), 53 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index c0853695..22d29294 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -157,20 +157,23 @@ void AcsController::performSafe() {
   guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
   // if MEKF is working
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
-  bool magMomMtqValid = false;
   if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) {
-    safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
-                      mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
-                      susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
-                      mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
-                      sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
+    result = safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
+                               mgmDataProcessed.magIgrfModel.value,
+                               mgmDataProcessed.magIgrfModel.isValid(),
+                               susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
+                               mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(),
+                               sunTargetDir, satRateSafe, &errAng, magMomMtq);
   } else {
-    safeCtrl.safeNoMekf(
+    result = safeCtrl.safeNoMekf(
         now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
         susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
         mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
         mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),
-        sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
+        sunTargetDir, satRateSafe, &errAng, magMomMtq);
+  }
+  if (result == returnvalue::FAILED) {
+    // ToDo: this should never ever happen or we are dead. prob add an event at least
   }
 
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp
index 705bf599..0e131942 100644
--- a/mission/controller/acs/control/Detumble.cpp
+++ b/mission/controller/acs/control/Detumble.cpp
@@ -1,11 +1,3 @@
-
-/*
- * Detumble.cpp
- *
- *  Created on: 17 Aug 2022
- *      Author: Robin Marquardt
- */
-
 #include "Detumble.h"
 
 #include <fsfw/globalfunctions/constants.h>
@@ -31,6 +23,12 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
   if (!magRateValid || !magFieldValid) {
     return DETUMBLE_NO_SENSORDATA;
   }
+
+  // change unit from uT to T
+  double magFieldT[3] = {0, 0, 0}, magRateT[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(magField, 1e-6, magFieldT, 3);
+  VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
+
   double gain = detumbleParameter->gainD;
   double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
   VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index aa04cbb6..3a69e343 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -32,15 +32,13 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
                                  double *magFieldModel, bool magFieldModelValid,
                                  double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
                                  bool rateMekfValid, double *sunDirRef, double *satRatRef,
-                                 double *outputAngle, double *outputMagMomB, bool *outputValid) {
+                                 double *outputAngle, double *outputMagMomB) {
   if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) {
-    *outputValid = false;
     return SAFECTRL_MEKF_INPUT_INVALID;
   }
 
-  double kRate = 0, kAlign = 0;
-  kRate = safeModeControllerParameters->k_rate_mekf;
-  kAlign = safeModeControllerParameters->k_align_mekf;
+  double kRate = safeModeControllerParameters->k_rate_mekf;
+  double kAlign = safeModeControllerParameters->k_align_mekf;
 
   //	Calc sunDirB ,magFieldB with mekf output and model
   double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@@ -49,22 +47,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
   MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
   MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
 
-  double crossSun[3] = {0, 0, 0};
+  // change unit from uT to T
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
 
+  double crossSun[3] = {0, 0, 0};
   VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
   double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
 
   //	calc angle alpha between sunDirRef and sunDIr
-  double alpha = 0, dotSun = 0;
-  dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
-  alpha = acos(dotSun);
+  double dotSun = VectorOperations<double>::dot(sunDirRef, sunDirB);
+  double alpha = acos(dotSun);
 
   //	Law Torque calculations
   double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0},
          torqueAll[3] = {0, 0, 0};
 
-  double scalarFac = 0;
-  scalarFac = kAlign * alpha / normCrossSun;
+  double scalarFac = kAlign * alpha / normCrossSun;
   VectorOperations<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
 
   double rateSafeMode[3] = {0, 0, 0};
@@ -82,23 +80,22 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
   VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3);
 
   *outputAngle = alpha;
-  *outputValid = true;
-
   return returnvalue::OK;
 }
 
 // Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
-void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
+ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
                           bool sunRateBValid, double *magFieldB, bool magFieldBValid,
                           double *magRateB, bool magRateBValid, double *sunDirRef,
-                          double *satRateRef, double *outputAngle, double *outputMagMomB,
-                          bool *outputValid) {
+                          double *satRateRef, double *outputAngle, double *outputMagMomB) {
   // Check for invalid Inputs
   if (!susDirBValid || !magFieldBValid || !magRateBValid) {
-    *outputValid = false;
-    return;
+    return returnvalue::FAILED;
   }
 
+  // change unit from uT to T
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
+
   // normalize sunDir and magDir
   double magDirB[3] = {0, 0, 0};
   VectorOperations<double>::normalize(magFieldB, magDirB, 3);
@@ -108,13 +105,11 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
   double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
 
   // Rate parallel to sun direction and magnetic field direction
-  double rateParaSun = 0, rateParaMag = 0;
-  double dotSunRateMag = 0, dotmagRateSun = 0, rateFactor = 0;
-  dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
-  dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
-  rateFactor = 1 - pow(cosAngleSunMag, 2);
-  rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
-  rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
+  double dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
+  double dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
+  double rateFactor = 1 - pow(cosAngleSunMag, 2);
+  double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
+  double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
 
   // Full rate or estimate
   double estSatRate[3] = {0, 0, 0};
@@ -130,7 +125,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
    * is sufficiently large */
   double angleSunMag = acos(cosAngleSunMag);
   if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
-    return;
+    return returnvalue::FAILED;
   }
 
   // Rate for Torque Calculation
@@ -138,9 +133,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
   VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
 
   // Torque Align calculation
-  double kRateNoMekf = 0, kAlignNoMekf = 0;
-  kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
-  kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
+  double kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
+  double kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
 
   double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
   double crossSusSunRef[3] = {0, 0, 0};
@@ -171,5 +165,5 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
 
   std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
   *outputAngle = angleAlignErr;
-  *outputValid = true;
+  return returnvalue::OK;
 }
diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h
index 1784f9ca..92d20313 100644
--- a/mission/controller/acs/control/SafeCtrl.h
+++ b/mission/controller/acs/control/SafeCtrl.h
@@ -23,14 +23,12 @@ class SafeCtrl {
                          bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
                          double *satRateMekf, bool rateMekfValid, double *sunDirRef,
                          double *satRatRef,  // From Guidance (!)
-                         double *outputAngle, double *outputMagMomB, bool *outputValid);
+                         double *outputAngle, double *outputMagMomB);
 
-  void safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
-                  bool sunRateBValid, double *magFieldB, bool magFieldBValid, double *magRateB,
-                  bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle,
-                  double *outputMagMomB, bool *outputValid);
-
-  void idleSunPointing();  // with reaction wheels
+  ReturnValue_t safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
+                           bool sunRateBValid, double *magFieldB, bool magFieldBValid,
+                           double *magRateB, bool magRateBValid, double *sunDirRef,
+                           double *satRateRef, double *outputAngle, double *outputMagMomB);
 
  protected:
  private:

From 3ade660132192be2683b7d458173c7bcb201a389 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 17:53:59 +0100
Subject: [PATCH 24/40] changelog

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index baad285b..fb173bc4 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -26,6 +26,7 @@ will consitute of a breaking change warranting a new major release:
 
 - IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements.
 - Set RM3100 dataset to valid.
+- Fixed units in calculation of ACS control laws safe and detumble.
 
 # [v1.33.0]
 

From 00834de1b77b062a929b954adc017c70c439fda5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 17:55:47 +0100
Subject: [PATCH 25/40] printout tweak, bump tmtc

---
 mission/devices/ImtqHandler.cpp | 2 +-
 tmtc                            | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index 0072dd08..c3b14727 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -395,7 +395,7 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
   } else if (sid == negZselfTestDataset.getSid()) {
     return &negZselfTestDataset;
   } else {
-    sif::error << "IMTQHandler::getDataSetHandle: Invalid sid" << std::endl;
+    sif::error << "ImtqHandler::getDataSetHandle: Invalid SID"<< std::endl;
     return nullptr;
   }
 }
diff --git a/tmtc b/tmtc
index 2dd850f0..77fbcede 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 2dd850f0725d37256c17576bf7d3ae4423184044
+Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc

From 4ec362d8496654161271186862447428929dbd75 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 17:56:10 +0100
Subject: [PATCH 26/40] afmt

---
 mission/devices/ImtqHandler.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index c3b14727..b95a09c0 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -395,7 +395,7 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
   } else if (sid == negZselfTestDataset.getSid()) {
     return &negZselfTestDataset;
   } else {
-    sif::error << "ImtqHandler::getDataSetHandle: Invalid SID"<< std::endl;
+    sif::error << "ImtqHandler::getDataSetHandle: Invalid SID" << std::endl;
     return nullptr;
   }
 }

From 7288a5edbd0a1bc5b34184ff0135694653e198fb Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 2 Mar 2023 18:27:27 +0100
Subject: [PATCH 27/40] go to off immediately

---
 mission/devices/SusHandler.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mission/devices/SusHandler.cpp b/mission/devices/SusHandler.cpp
index 228ea082..886c7cec 100644
--- a/mission/devices/SusHandler.cpp
+++ b/mission/devices/SusHandler.cpp
@@ -37,7 +37,7 @@ void SusHandler::doShutDown() {
     updatePeriodicReply(false, REPLY);
     commandExecuted = false;
     internalState = InternalState::NONE;
-    setMode(_MODE_POWER_DOWN);
+    setMode(MODE_OFF);
   }
 }
 

From 1cf3fbaabe8a45bcf40feed4c9428fa823b862f2 Mon Sep 17 00:00:00 2001
From: Jakob Meier <meierj@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 10:48:59 +0100
Subject: [PATCH 28/40] fixed that mgm value was overwritten in wrong unit

---
 mission/controller/acs/control/SafeCtrl.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index 3a69e343..9ef9acf5 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -94,7 +94,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
   }
 
   // change unit from uT to T
-  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
+  double magFieldBT[3] = {0,0,0};
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
   // normalize sunDir and magDir
   double magDirB[3] = {0, 0, 0};

From ff50203fbb463bfb66df5715b47fa2ba3e7e4d21 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 3 Mar 2023 11:04:08 +0100
Subject: [PATCH 29/40] smaller lock tweaks

---
 linux/devices/AcsBoardPolling.cpp           | 87 +++++++++++----------
 mission/controller/acs/control/SafeCtrl.cpp |  9 ++-
 tmtc                                        |  2 +-
 3 files changed, 50 insertions(+), 48 deletions(-)

diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp
index 6a16d816..aad802d2 100644
--- a/linux/devices/AcsBoardPolling.cpp
+++ b/linux/devices/AcsBoardPolling.cpp
@@ -25,7 +25,7 @@ AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF,
 
 ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
   while (true) {
-    ipcLock->lockMutex();
+    ipcLock->lockMutex(LOCK_TYPE, LOCK_TIMEOUT);
     state = InternalState::IDLE;
     ipcLock->unlockMutex();
     semaphore->acquire();
@@ -173,7 +173,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData);
-    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != mgm.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         mgm.performStartup = true;
@@ -184,45 +183,47 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
     }
     return returnvalue::OK;
   };
-  switch (spiCookie->getChipSelectPin()) {
-    case (gpioIds::MGM_0_LIS3_CS): {
-      handleLis3Request(mgm0Lis3);
-      break;
+  {
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    switch (spiCookie->getChipSelectPin()) {
+      case (gpioIds::MGM_0_LIS3_CS): {
+        handleLis3Request(mgm0Lis3);
+        break;
+      }
+      case (gpioIds::MGM_1_RM3100_CS): {
+        handleRm3100Request(mgm1Rm3100);
+        break;
+      }
+      case (gpioIds::MGM_2_LIS3_CS): {
+        handleLis3Request(mgm2Lis3);
+        break;
+      }
+      case (gpioIds::MGM_3_RM3100_CS): {
+        handleRm3100Request(mgm3Rm3100);
+        break;
+      }
+      case (gpioIds::GYRO_0_ADIS_CS): {
+        handleAdisRequest(gyro0Adis);
+        break;
+      }
+      case (gpioIds::GYRO_2_ADIS_CS): {
+        handleAdisRequest(gyro2Adis);
+        break;
+      }
+      case (gpioIds::GYRO_1_L3G_CS): {
+        handleL3gRequest(gyro1L3g);
+        break;
+      }
+      case (gpioIds::GYRO_3_L3G_CS): {
+        handleL3gRequest(gyro3L3g);
+        break;
+      }
     }
-    case (gpioIds::MGM_1_RM3100_CS): {
-      handleRm3100Request(mgm1Rm3100);
-      break;
-    }
-    case (gpioIds::MGM_2_LIS3_CS): {
-      handleLis3Request(mgm2Lis3);
-      break;
-    }
-    case (gpioIds::MGM_3_RM3100_CS): {
-      handleRm3100Request(mgm3Rm3100);
-      break;
-    }
-    case (gpioIds::GYRO_0_ADIS_CS): {
-      handleAdisRequest(gyro0Adis);
-      break;
-    }
-    case (gpioIds::GYRO_2_ADIS_CS): {
-      handleAdisRequest(gyro2Adis);
-      break;
-    }
-    case (gpioIds::GYRO_1_L3G_CS): {
-      handleL3gRequest(gyro1L3g);
-      break;
-    }
-    case (gpioIds::GYRO_3_L3G_CS): {
-      handleL3gRequest(gyro3L3g);
-      break;
+    if (state == InternalState::IDLE) {
+      state = InternalState::BUSY;
     }
   }
-  MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-  if (state == InternalState::IDLE) {
-    state = InternalState::BUSY;
-    semaphore->release();
-  }
+  semaphore->release();
   return returnvalue::OK;
 }
 
@@ -294,8 +295,8 @@ ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** b
 
 void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
   ReturnValue_t result;
-  acs::SimpleSensorMode mode;
-  bool gyroPerformStartup;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+  bool gyroPerformStartup = false;
   {
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mode = l3g.mode;
@@ -440,7 +441,7 @@ ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen
 
 void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
   ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
   bool cdHasTimedOut = false;
   bool mustPerformStartup = false;
   {
@@ -542,7 +543,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
 
 void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
   ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
   bool mustPerformStartup = false;
   {
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
@@ -635,7 +636,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
 
 void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
   ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
   bool mustPerformStartup = false;
   {
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index 3a69e343..e6b45318 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -84,10 +84,11 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
 }
 
 // Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
-ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
-                          bool sunRateBValid, double *magFieldB, bool magFieldBValid,
-                          double *magRateB, bool magRateBValid, double *sunDirRef,
-                          double *satRateRef, double *outputAngle, double *outputMagMomB) {
+ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid,
+                                   double *sunRateB, bool sunRateBValid, double *magFieldB,
+                                   bool magFieldBValid, double *magRateB, bool magRateBValid,
+                                   double *sunDirRef, double *satRateRef, double *outputAngle,
+                                   double *outputMagMomB) {
   // Check for invalid Inputs
   if (!susDirBValid || !magFieldBValid || !magRateBValid) {
     return returnvalue::FAILED;
diff --git a/tmtc b/tmtc
index 77fbcede..74e6d6fe 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc
+Subproject commit 74e6d6fe5f10121e8444662a79fa10fada3d978c

From 60afb573a0f47eba4865b99d2f338ec487c40250 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 3 Mar 2023 11:06:30 +0100
Subject: [PATCH 30/40] comment

---
 linux/devices/AcsBoardPolling.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp
index aad802d2..a9128242 100644
--- a/linux/devices/AcsBoardPolling.cpp
+++ b/linux/devices/AcsBoardPolling.cpp
@@ -39,7 +39,7 @@ ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
     mgmRm3100Handler(mgm3Rm3100);
     mgmLis3Handler(mgm0Lis3);
     mgmLis3Handler(mgm2Lis3);
-    // To prevent task being not reactivated by tardy tasks
+    // To prevent task being reactivated by tardy tasks
     TaskFactory::delayTask(20);
   }
   return returnvalue::OK;

From fcba76b7f1098e3d062e8f32c3340cd77caf116a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Fri, 3 Mar 2023 11:43:47 +0100
Subject: [PATCH 31/40] rework lock handling in rtd handler

---
 linux/devices/Max31865RtdPolling.cpp        | 82 +++++++++++----------
 linux/devices/Max31865RtdPolling.h          |  5 +-
 mission/controller/acs/control/SafeCtrl.cpp |  9 ++-
 tmtc                                        |  2 +-
 4 files changed, 54 insertions(+), 44 deletions(-)

diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp
index 004e7c60..6cfb2000 100644
--- a/linux/devices/Max31865RtdPolling.cpp
+++ b/linux/devices/Max31865RtdPolling.cpp
@@ -56,19 +56,28 @@ bool Max31865RtdPolling::rtdIsActive(uint8_t idx) {
   return false;
 }
 
-bool Max31865RtdPolling::periodicInitHandling() {
+ReturnValue_t Max31865RtdPolling::periodicInitHandling() {
   using namespace MAX31865;
   ReturnValue_t result = returnvalue::OK;
   for (auto& rtd : rtds) {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
-      return false;
+    bool mustPerformInitHandling = false;
+    bool doWriteLowThreshold = false;
+    bool doWriteHighThreshold = false;
+    {
+      MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+      if (mg.getLockResult() != returnvalue::OK) {
+        sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
+        continue;
+      }
+      mustPerformInitHandling =
+          (rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut();
+      doWriteHighThreshold = rtd->writeHighThreshold;
+      doWriteLowThreshold = rtd->writeLowThreshold;
     }
-    if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
+    if (mustPerformInitHandling) {
       // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
       // or hardware specific issue where the CS needs to be pulled high and then low again
       // between transfers
@@ -77,13 +86,13 @@ bool Max31865RtdPolling::periodicInitHandling() {
         handleSpiError(rtd, result, "writeCfgReg");
         continue;
       }
-      if (rtd->writeLowThreshold) {
+      if (doWriteLowThreshold) {
         result = writeLowThreshold(rtd->spiCookie, rtd->lowThreshold);
         if (result != returnvalue::OK) {
           handleSpiError(rtd, result, "writeLowThreshold");
         }
       }
-      if (rtd->writeHighThreshold) {
+      if (doWriteHighThreshold) {
         result = writeHighThreshold(rtd->spiCookie, rtd->highThreshold);
         if (result != returnvalue::OK) {
           handleSpiError(rtd, result, "writeHighThreshold");
@@ -93,23 +102,12 @@ bool Max31865RtdPolling::periodicInitHandling() {
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "clearFaultStatus");
       }
+      MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       rtd->db.configured = true;
       rtd->db.active = true;
     }
   }
-  bool someRtdUsable = false;
-  for (auto& rtd : rtds) {
-    if (rtd == nullptr) {
-      continue;
-    }
-    if (rtdIsActive(rtd->idx)) {
-#if OBSW_RTD_AUTO_MODE == 0
-      result = writeBiasSel(Bias::ON, rtd->spiCookie, BASE_CFG);
-#endif
-      someRtdUsable = true;
-    }
-  }
-  return someRtdUsable;
+  return returnvalue::OK;
 }
 
 ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
@@ -119,12 +117,8 @@ ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
+    updateActiveRtdsArray();
+    if (activeRtdsArray[rtd->idx]) {
       ReturnValue_t result = writeCfgReg(rtd->spiCookie, BASE_CFG | (1 << CfgBitPos::ONE_SHOT));
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "writeCfgReg");
@@ -144,12 +138,8 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
+    updateActiveRtdsArray();
+    if (activeRtdsArray[rtd->idx]) {
       // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
       // or hardware specific issue where the CS needs to be pulled high and then low again
       // between transfers
@@ -166,6 +156,7 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
         handleSpiError(rtd, result, "readRtdVal");
         continue;
       }
+      MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       if (faultBitSet) {
         rtd->db.faultBitSet = faultBitSet;
       }
@@ -212,6 +203,10 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
   if (cookie == nullptr) {
     return returnvalue::FAILED;
   }
+  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
+  if (rtdCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
   // Empty command.. don't fail for now
   if (sendLen < 1) {
     return returnvalue::OK;
@@ -221,7 +216,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
     sif::warning << "Max31865RtdReader::sendMessage: Mutex lock failed" << std::endl;
     return returnvalue::FAILED;
   }
-  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
   uint8_t cmdRaw = sendData[0];
   if (cmdRaw > EiveMax31855::RtdCommands::NUM_CMDS) {
     sif::warning << "Max31865RtdReader::sendMessage: Invalid command" << std::endl;
@@ -312,15 +306,15 @@ ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t
 
 ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                       size_t* size) {
+  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
+  if (rtdCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
   MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (mg.getLockResult() != returnvalue::OK) {
     // TODO: Emit warning
     return returnvalue::FAILED;
   }
-  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
-  if (rtdCookie == nullptr) {
-    return returnvalue::FAILED;
-  }
   uint8_t* exchangePtr = rtdCookie->exchangeBuf.data();
   size_t serLen = 0;
   auto result = rtdCookie->db.serialize(&exchangePtr, &serLen, rtdCookie->exchangeBuf.size(),
@@ -461,6 +455,18 @@ ReturnValue_t Max31865RtdPolling::readNFromReg(SpiCookie* cookie, uint8_t reg, s
   return returnvalue::OK;
 }
 
+ReturnValue_t Max31865RtdPolling::updateActiveRtdsArray() {
+  MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+  if (mg.getLockResult() != returnvalue::OK) {
+    sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
+    return returnvalue::FAILED;
+  }
+  for (const auto& rtd : rtds) {
+    activeRtdsArray[rtd->idx] = rtdIsActive(rtd->idx);
+  }
+  return returnvalue::OK;
+}
+
 ReturnValue_t Max31865RtdPolling::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
                                                  const char* ctx) {
   cookie->db.spiErrorCount.value += 1;
diff --git a/linux/devices/Max31865RtdPolling.h b/linux/devices/Max31865RtdPolling.h
index da27de68..ef73fb8b 100644
--- a/linux/devices/Max31865RtdPolling.h
+++ b/linux/devices/Max31865RtdPolling.h
@@ -47,6 +47,7 @@ class Max31865RtdPolling : public SystemObject,
  private:
   std::vector<Max31865ReaderCookie*> rtds;
   std::array<uint8_t, 4> cmdBuf = {};
+  std::array<bool, 12> activeRtdsArray{};
   size_t dbLen = 0;
   MutexIF* readerLock;
   static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
@@ -59,7 +60,7 @@ class Max31865RtdPolling : public SystemObject,
   uint32_t csTimeoutMs = spi::RTD_CS_TIMEOUT;
   MutexIF* csLock = nullptr;
 
-  bool periodicInitHandling();
+  ReturnValue_t periodicInitHandling();
   ReturnValue_t periodicReadReqHandling();
   ReturnValue_t periodicReadHandling();
 
@@ -84,6 +85,8 @@ class Max31865RtdPolling : public SystemObject,
   ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
   ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
 
+  ReturnValue_t updateActiveRtdsArray();
+
   ReturnValue_t handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result, const char* ctx);
 };
 
diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index 3a69e343..e6b45318 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -84,10 +84,11 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
 }
 
 // Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
-ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
-                          bool sunRateBValid, double *magFieldB, bool magFieldBValid,
-                          double *magRateB, bool magRateBValid, double *sunDirRef,
-                          double *satRateRef, double *outputAngle, double *outputMagMomB) {
+ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid,
+                                   double *sunRateB, bool sunRateBValid, double *magFieldB,
+                                   bool magFieldBValid, double *magRateB, bool magRateBValid,
+                                   double *sunDirRef, double *satRateRef, double *outputAngle,
+                                   double *outputMagMomB) {
   // Check for invalid Inputs
   if (!susDirBValid || !magFieldBValid || !magRateBValid) {
     return returnvalue::FAILED;
diff --git a/tmtc b/tmtc
index 77fbcede..74e6d6fe 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc
+Subproject commit 74e6d6fe5f10121e8444662a79fa10fada3d978c

From ec2ec32af420e3004e09e5915c013142a45e7a88 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 11:56:31 +0100
Subject: [PATCH 32/40] actually use the stuff i wrote

---
 mission/controller/acs/control/SafeCtrl.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index 9ef9acf5..32a77591 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -99,7 +99,7 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
 
   // normalize sunDir and magDir
   double magDirB[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(magFieldB, magDirB, 3);
+  VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
   VectorOperations<double>::normalize(susDirB, susDirB, 3);
 
   // Cosinus angle between sunDir and magDir
@@ -160,8 +160,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
   // Magnetic moment
   double magMomB[3] = {0, 0, 0};
   double crossMagFieldTorque[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
-  double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
+  VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
+  double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
   VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
 
   std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));

From 7a7f25a63837f530a835358693a9e3f99a037187 Mon Sep 17 00:00:00 2001
From: Jakob Meier <meierj@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 14:36:52 +0100
Subject: [PATCH 33/40] lets see if this fixes issues

---
 CHANGELOG.md                                 | 3 +++
 fsfw                                         | 2 +-
 linux/devices/AcsBoardPolling.cpp            | 1 -
 linux/devices/GpsHyperionLinuxController.cpp | 5 ++---
 linux/devices/GpsHyperionLinuxController.h   | 4 ++--
 linux/devices/Max31865RtdPolling.cpp         | 2 --
 linux/devices/RwPollingTask.cpp              | 2 +-
 7 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index b3fea248..496d9cfe 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -28,6 +28,9 @@ will consitute of a breaking change warranting a new major release:
 - IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements.
 - Set RM3100 dataset to valid.
 - Fixed units in calculation of ACS control laws safe and detumble.
+- Bump FSFW for change in Countdown: Use system clock instead of reading uptime from file
+  to prevent possible race condition.
+- GPS: No fix for 30 minutes considered a faulty now instead of 5 hours.
 
 ## Added
 
diff --git a/fsfw b/fsfw
index 245886c5..78cf0031 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 245886c55500b9e70ba71eab68c46d44af9f6836
+Subproject commit 78cf00315d5bb9a05d47976a564d16c5978f1f41
diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp
index 6a16d816..5685cbde 100644
--- a/linux/devices/AcsBoardPolling.cpp
+++ b/linux/devices/AcsBoardPolling.cpp
@@ -110,7 +110,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         adis.type = req->type;
         adis.countdown.setTimeout(adis1650x::START_UP_TIME);
-        adis.countdown.resetTimer();
         if (adis.type == adis1650x::Type::ADIS16507) {
           adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
         } else if (adis.type == adis1650x::Type::ADIS16505) {
diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp
index 3b396504..e3982a1c 100644
--- a/linux/devices/GpsHyperionLinuxController.cpp
+++ b/linux/devices/GpsHyperionLinuxController.cpp
@@ -20,7 +20,6 @@
 GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                                                        bool debugHyperionGps)
     : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {
-  timeUpdateCd.resetTimer();
 }
 
 GpsHyperionLinuxController::~GpsHyperionLinuxController() {
@@ -196,8 +195,8 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
     if (mode != MODE_OFF) {
       if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
         sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed "
-                     << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
-        triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
+                     << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
+        triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
         oneShotSwitches.cantGetFixSwitch = false;
       }
       modeIsSet = false;
diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h
index df08d8a4..f00724bd 100644
--- a/linux/devices/GpsHyperionLinuxController.h
+++ b/linux/devices/GpsHyperionLinuxController.h
@@ -23,7 +23,8 @@
  */
 class GpsHyperionLinuxController : public ExtendedControllerBase {
  public:
-  static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 60 * 5;
+  // 30 minutes
+  static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 30;
 
   enum ReadModes { SHM = 0, SOCKET = 1 };
 
@@ -79,7 +80,6 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
 
   bool debugHyperionGps = false;
   int32_t noModeSetCntr = 0;
-  Countdown timeUpdateCd = Countdown(60);
 
   // Returns true if the function should be called again or false if other
   // controller handling can be done.
diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp
index 004e7c60..53aad8fe 100644
--- a/linux/devices/Max31865RtdPolling.cpp
+++ b/linux/devices/Max31865RtdPolling.cpp
@@ -240,7 +240,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
     case (EiveMax31855::RtdCommands::ON): {
       if (not rtdCookie->on) {
         rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS);
-        rtdCookie->cd.resetTimer();
         rtdCookie->on = true;
         rtdCookie->db.active = false;
         rtdCookie->db.configured = false;
@@ -253,7 +252,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
     case (EiveMax31855::RtdCommands::ACTIVE): {
       if (not rtdCookie->on) {
         rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS);
-        rtdCookie->cd.resetTimer();
         rtdCookie->on = true;
         rtdCookie->db.active = true;
         rtdCookie->db.configured = false;
diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp
index 2d05ac8b..7dff31bb 100644
--- a/linux/devices/RwPollingTask.cpp
+++ b/linux/devices/RwPollingTask.cpp
@@ -220,7 +220,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf
   }
   pullCsLow(gpioId, gpioIF);
   bool lastByteWasFrameMarker = false;
-  Countdown cd(3000);
+  Countdown cd(2000);
   size_t readIdx = 0;
 
   while (true) {

From 53133573e137f104543206bf49ec3263fc0e5b06 Mon Sep 17 00:00:00 2001
From: Jakob Meier <meierj@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 14:58:03 +0100
Subject: [PATCH 34/40] measurement base code and comment

---
 fsfw                                         |  2 +-
 linux/devices/AcsBoardPolling.cpp            | 21 ++++++++++++--------
 linux/devices/GpsHyperionLinuxController.cpp |  3 +--
 3 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/fsfw b/fsfw
index 78cf0031..64537d44 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 78cf00315d5bb9a05d47976a564d16c5978f1f41
+Subproject commit 64537d442a335500bdc87ff382099de65f0b7aa7
diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp
index 5685cbde..3b35167d 100644
--- a/linux/devices/AcsBoardPolling.cpp
+++ b/linux/devices/AcsBoardPolling.cpp
@@ -4,6 +4,7 @@
 #include <fsfw/globalfunctions/arrayprinter.h>
 #include <fsfw/tasks/SemaphoreFactory.h>
 #include <fsfw/tasks/TaskFactory.h>
+#include <fsfw/timemanager/Stopwatch.h>
 #include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
 #include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
 #include <fsfw_hal/linux/UnixFileGuard.h>
@@ -31,14 +32,18 @@ ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
     semaphore->acquire();
     // Give all tasks or the PST some time to submit all consecutive requests.
     TaskFactory::delayTask(2);
-    gyroAdisHandler(gyro0Adis);
-    gyroAdisHandler(gyro2Adis);
-    gyroL3gHandler(gyro1L3g);
-    gyroL3gHandler(gyro3L3g);
-    mgmRm3100Handler(mgm1Rm3100);
-    mgmRm3100Handler(mgm3Rm3100);
-    mgmLis3Handler(mgm0Lis3);
-    mgmLis3Handler(mgm2Lis3);
+    {
+      // Measured to take 0-1 ms in debug build.
+      // Stopwatch watch;
+      gyroAdisHandler(gyro0Adis);
+      gyroAdisHandler(gyro2Adis);
+      gyroL3gHandler(gyro1L3g);
+      gyroL3gHandler(gyro3L3g);
+      mgmRm3100Handler(mgm1Rm3100);
+      mgmRm3100Handler(mgm3Rm3100);
+      mgmLis3Handler(mgm0Lis3);
+      mgmLis3Handler(mgm2Lis3);
+    }
     // To prevent task being not reactivated by tardy tasks
     TaskFactory::delayTask(20);
   }
diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp
index e3982a1c..3beb0d87 100644
--- a/linux/devices/GpsHyperionLinuxController.cpp
+++ b/linux/devices/GpsHyperionLinuxController.cpp
@@ -19,8 +19,7 @@
 
 GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                                                        bool debugHyperionGps)
-    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {
-}
+    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {}
 
 GpsHyperionLinuxController::~GpsHyperionLinuxController() {
   gps_stream(&gps, WATCH_DISABLE, nullptr);

From 59e165be4f95e742b677ae7d7cd553a13a322c80 Mon Sep 17 00:00:00 2001
From: Jakob Meier <meierj@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 15:10:52 +0100
Subject: [PATCH 35/40] lockfix

---
 linux/devices/AcsBoardPolling.cpp    | 3 ---
 linux/devices/Max31865RtdPolling.cpp | 1 +
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp
index 6a16d816..4742910d 100644
--- a/linux/devices/AcsBoardPolling.cpp
+++ b/linux/devices/AcsBoardPolling.cpp
@@ -105,7 +105,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::Adis1650XRequest*>(sendData);
-    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != adis.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         adis.type = req->type;
@@ -135,7 +134,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::GyroL3gRequest*>(sendData);
-    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != gyro.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         std::memcpy(gyro.sensorCfg, req->ctrlRegs, 5);
@@ -154,7 +152,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::MgmLis3Request*>(sendData);
-    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != mgm.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         mgm.performStartup = true;
diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp
index 004e7c60..e9053af7 100644
--- a/linux/devices/Max31865RtdPolling.cpp
+++ b/linux/devices/Max31865RtdPolling.cpp
@@ -26,6 +26,7 @@ ReturnValue_t Max31865RtdPolling::performOperation(uint8_t operationCode) {
   using namespace MAX31865;
   ReturnValue_t result = returnvalue::OK;
   static_cast<void>(result);
+  // Measured to take 0-1 ms in debug build
   // Stopwatch watch;
   if (periodicInitHandling()) {
 #if OBSW_RTD_AUTO_MODE == 0

From 05be1708908f8a32bdc4feb5fba0ac5f26d435c6 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 16:06:38 +0100
Subject: [PATCH 36/40] hotfixees

---
 CHANGELOG.md                                |  1 +
 fsfw                                        |  2 +-
 linux/devices/Max31865RtdPolling.cpp        | 17 +++++------------
 mission/controller/acs/control/SafeCtrl.cpp |  2 +-
 mission/core/GenericFactory.cpp             |  6 ++++--
 5 files changed, 12 insertions(+), 16 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 496d9cfe..590a2cfd 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -22,6 +22,7 @@ will consitute of a breaking change warranting a new major release:
 - Allow quicker transition for the EIVE system component by allowing consecutive TCS and ACS
   component commanding again.
 - Changed a lot of lock guards to use timeouts
+- Queue sizes of TCP/UDP servers increased from 20 to 50
 
 ## Fixed
 
diff --git a/fsfw b/fsfw
index 6e17e455..6006c97e 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 6e17e45506b0d9834d3ae9ded6f044e13e3c4abd
+Subproject commit 6006c97e48b7e6dc3b45a832bdd027a510b67f16
diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp
index 2edd726f..db13a76c 100644
--- a/linux/devices/Max31865RtdPolling.cpp
+++ b/linux/devices/Max31865RtdPolling.cpp
@@ -28,17 +28,10 @@ ReturnValue_t Max31865RtdPolling::performOperation(uint8_t operationCode) {
   static_cast<void>(result);
   // Measured to take 0-1 ms in debug build
   // Stopwatch watch;
-  if (periodicInitHandling()) {
-#if OBSW_RTD_AUTO_MODE == 0
-    // 10 ms delay for VBIAS startup
-    TaskFactory::delayTask(10);
-#endif
-  } else {
-    // No devices usable (e.g. TCS board off)
-    return returnvalue::OK;
-  }
-
+  periodicInitHandling();
 #if OBSW_RTD_AUTO_MODE == 0
+  // 10 ms delay for VBIAS startup
+  TaskFactory::delayTask(10);
   result = periodicReadReqHandling();
   if (result != returnvalue::OK) {
     return result;
@@ -113,12 +106,12 @@ ReturnValue_t Max31865RtdPolling::periodicInitHandling() {
 
 ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
   using namespace MAX31865;
+  updateActiveRtdsArray();
   // Now request one shot config for all active RTDs
   for (auto& rtd : rtds) {
     if (rtd == nullptr) {
       continue;
     }
-    updateActiveRtdsArray();
     if (activeRtdsArray[rtd->idx]) {
       ReturnValue_t result = writeCfgReg(rtd->spiCookie, BASE_CFG | (1 << CfgBitPos::ONE_SHOT));
       if (result != returnvalue::OK) {
@@ -134,12 +127,12 @@ ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
 ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
   using namespace MAX31865;
   auto result = returnvalue::OK;
+  updateActiveRtdsArray();
   // Now read the RTD values
   for (auto& rtd : rtds) {
     if (rtd == nullptr) {
       continue;
     }
-    updateActiveRtdsArray();
     if (activeRtdsArray[rtd->idx]) {
       // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
       // or hardware specific issue where the CS needs to be pulled high and then low again
diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index 23a86551..5c665156 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -95,7 +95,7 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
   }
 
   // change unit from uT to T
-  double magFieldBT[3] = {0,0,0};
+  double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
   // normalize sunDir and magDir
diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp
index 87b08b49..b6119eda 100644
--- a/mission/core/GenericFactory.cpp
+++ b/mission/core/GenericFactory.cpp
@@ -117,14 +117,16 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
 
 #if OBSW_ADD_TCPIP_SERVERS == 1
 #if OBSW_ADD_TMTC_UDP_SERVER == 1
-  auto udpBridge = new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR);
+  auto udpBridge =
+      new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 50);
   new UdpTcPollingTask(objects::UDP_TMTC_POLLING_TASK, objects::UDP_TMTC_SERVER);
   sif::info << "Created UDP server for TMTC commanding with listener port "
             << udpBridge->getUdpPort() << std::endl;
   udpBridge->setMaxNumberOfPacketsStored(config::MAX_STORED_CMDS_UDP);
 #endif
 #if OBSW_ADD_TMTC_TCP_SERVER == 1
-  auto tcpBridge = new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR);
+  auto tcpBridge =
+      new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 50);
   TcpTmTcServer::TcpConfig cfg(true, true);
   auto tcpServer = new TcpTmTcServer(objects::TCP_TMTC_POLLING_TASK, objects::TCP_TMTC_SERVER, cfg);
   // TCP is stream based. Use packet ID as start marker when parsing for space packets

From 3b383b98df182be0c6321b5e468c7fb637264db5 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 16:45:25 +0100
Subject: [PATCH 37/40] prep v1.34.0

---
 CHANGELOG.md   | 22 ++++++++++++++--------
 CMakeLists.txt |  2 +-
 fsfw           |  2 +-
 tmtc           |  2 +-
 4 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 590a2cfd..90845628 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+# [v1.34.0] 2023-03-03
+
+This might include the fix for the race condition where CPU usage jumped to 200 %. The race
+condition was traced to the `Countdown` class, more specifically to the `getUptime` function where
+the `/proc/uptime` file is read.
+
 ## Changed
 
 - Moved polling of all SPI parts to the same PST.
@@ -37,7 +43,7 @@ will consitute of a breaking change warranting a new major release:
 
 - Added Syrlinks Assembly object to allow recovery handling and to fix faulty FDIR behaviour.
 
-# [v1.33.0]
+# [v1.33.0] 2023-03-01
 
 eive-tmtc: v2.16.2
 
@@ -51,7 +57,7 @@ eive-tmtc: v2.16.2
 - Linux GPS handler now checks the individual `*_SET` flags when analysing the `gpsd` struct.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/400
 
-# [v1.32.0]
+# [v1.32.0] 2023-02-24
 
 eive-tmtc: v2.16.1
 
@@ -92,7 +98,7 @@ eive-tmtc: v2.16.1
 - `RwDummy` now initializes with a non faulty state
 
 
-# [v1.31.1]
+# [v1.31.1] 2023-02-23
 
 ## Fixed
 
@@ -102,7 +108,7 @@ eive-tmtc: v2.16.1
   for actuator control which lead to a crash.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/403
 
-# [v1.31.0]
+# [v1.31.0] 2023-02-23
 
 eive-tmtc: v2.16.0
 
@@ -143,7 +149,7 @@ COM PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/364
 - `MekfData` now includes `mekfStatus`
 - `CtrlValData` now includes `tgtRotRate`
 
-# [v1.30.0]
+# [v1.30.0] 2023-02-22
 
 eive-tmtc: v2.14.0
 
@@ -160,7 +166,7 @@ Event IDs for PDEC handler have changed in a breaking manner.
   an event is triggered and the task is delayed for 400 ms.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/393
 
-# [v1.29.1]
+# [v1.29.1] 2023-02-21
 
 ## Fixed
 
@@ -172,7 +178,7 @@ Event IDs for PDEC handler have changed in a breaking manner.
   Issue: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/388
 - Disable stopwatch in MAX31865 polling task
 
-# [v1.29.0]
+# [v1.29.0] 2023-02-21
 
 eive-tmtc: v2.13.0
 
@@ -193,7 +199,7 @@ eive-tmtc: v2.13.0
   will be part of the TCS tree.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/351
 
-# [v1.28.1]
+# [v1.28.1] 2023-02-21
 
 ## Fixed
 
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f477d205..588bf233 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 1)
-set(OBSW_VERSION_MINOR 33)
+set(OBSW_VERSION_MINOR 34)
 set(OBSW_VERSION_REVISION 0)
 
 # set(CMAKE_VERBOSE TRUE)
diff --git a/fsfw b/fsfw
index 6006c97e..33de1520 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 6006c97e48b7e6dc3b45a832bdd027a510b67f16
+Subproject commit 33de15205b4560c54a108e35609536374b026c22
diff --git a/tmtc b/tmtc
index 783d5a8e..a9e59ae4 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 783d5a8ed56a9683fc75d2aaffcabe82af34ffa9
+Subproject commit a9e59ae42b3f2f3a13a0bc96f56524f2d78e1eba

From 0d175951ec7044805dae33d0f56f69ecfa135ef7 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 16:49:21 +0100
Subject: [PATCH 38/40] some missing changelog entries

---
 CHANGELOG.md | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 90845628..80fd32f4 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -29,6 +29,8 @@ the `/proc/uptime` file is read.
   component commanding again.
 - Changed a lot of lock guards to use timeouts
 - Queue sizes of TCP/UDP servers increased from 20 to 50
+- Significantly simplified and improved lock guard handling in TCS and ACS board polling
+  tasks.
 
 ## Fixed
 
@@ -37,7 +39,10 @@ the `/proc/uptime` file is read.
 - Fixed units in calculation of ACS control laws safe and detumble.
 - Bump FSFW for change in Countdown: Use system clock instead of reading uptime from file
   to prevent possible race condition.
-- GPS: No fix for 30 minutes considered a faulty now instead of 5 hours.
+- GPS: No fix considered a fault now after 30 minutes instead of 5 hours.
+- SUS Assembly FDIR: Prevent permanent SAFE mode fallbacks by introducing special health
+  handling.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/418/files
 
 ## Added
 

From 982db82ddbacdd59f0dc60cd80355bc50028ca02 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 16:52:10 +0100
Subject: [PATCH 39/40] bump tmtc

---
 tmtc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tmtc b/tmtc
index a9e59ae4..94ae2d16 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit a9e59ae42b3f2f3a13a0bc96f56524f2d78e1eba
+Subproject commit 94ae2d16e21ade8f89723b2e62356967a67b171d

From e6b0b4048c7cf1af1a568b15068c65c1978b478f Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Fri, 3 Mar 2023 16:52:38 +0100
Subject: [PATCH 40/40] changelog eive-tmtc version

---
 CHANGELOG.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 80fd32f4..0e279f6e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -18,6 +18,8 @@ will consitute of a breaking change warranting a new major release:
 
 # [v1.34.0] 2023-03-03
 
+eive-tmtc: v2.16.3
+
 This might include the fix for the race condition where CPU usage jumped to 200 %. The race
 condition was traced to the `Countdown` class, more specifically to the `getUptime` function where
 the `/proc/uptime` file is read.