Merge remote-tracking branch 'origin/develop' into feature_imtq_assy

2023-03-03 17:03:55 +01:00 · 2023-03-03 17:03:55 +01:00 · f209ac6b2d
commit f209ac6b2d
parent 04e3d2486d b20266f153
25 changed files with 244 additions and 195 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -16,27 +16,45 @@ will consitute of a breaking change warranting a new major release:

 # [unreleased]

+## Added
+
+- Add IMTQ assembly
+
+# [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.
+
 ## 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.
+- 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

 - 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 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

 - Added Syrlinks Assembly object to allow recovery handling and to fix faulty FDIR behaviour.

-## Added
-
- Add IMTQ assembly
-
-# [v1.33.0]
+# [v1.33.0] 2023-03-01

 eive-tmtc: v2.16.2

@ -50,7 +68,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

@ -91,7 +109,7 @@ eive-tmtc: v2.16.1
 - `RwDummy` now initializes with a non faulty state


-# [v1.31.1]
+# [v1.31.1] 2023-02-23

 ## Fixed

@ -101,7 +119,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

@ -142,7 +160,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

@ -159,7 +177,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

@ -171,7 +189,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

@ -192,7 +210,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

--- 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)
--- 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, 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(mutex);
+  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(mutex);
+  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(mutex);
+  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(mutex);
+  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(mutex);
+    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(mutex);
+  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(mutex);
+  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(mutex);
+  MutexGuard mg(sdLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
  markedUnusable = true;
 }
--- a/bsp_q7s/fs/SdCardManager.h
+++ b/bsp_q7s/fs/SdCardManager.h
@ -223,7 +223,10 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
  bool sdCardActive = true;
  bool printCmdOutput = true;
  bool markedUnusable = false;
-  MutexIF* mutex = nullptr;
+  MutexIF* sdLock = nullptr;
+  static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+  static constexpr uint32_t LOCK_TIMEOUT = 40;
+  static constexpr char LOCK_CTX[] = "SdCardManager";

  SdCardManager();

--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 511d07c0c78de7b1850e341dfcf8be7589f3c523
+Subproject commit 33de15205b4560c54a108e35609536374b026c22
--- 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>
@ -25,12 +26,15 @@ 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();
    // Give all tasks or the PST some time to submit all consecutive requests.
    TaskFactory::delayTask(2);
+    {
+      // Measured to take 0-1 ms in debug build.
+      // Stopwatch watch;
      gyroAdisHandler(gyro0Adis);
      gyroAdisHandler(gyro2Adis);
      gyroL3gHandler(gyro1L3g);
@ -39,6 +43,7 @@ ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
      mgmRm3100Handler(mgm3Rm3100);
      mgmLis3Handler(mgm0Lis3);
      mgmLis3Handler(mgm2Lis3);
+    }
    // To prevent task being not reactivated by tardy tasks
    TaskFactory::delayTask(20);
  }
@ -105,12 +110,10 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
      return returnvalue::FAILED;
    }
    auto* req = reinterpret_cast<const acs::Adis1650XRequest*>(sendData);
-    MutexGuard mg(ipcLock);
    if (req->mode != adis.mode) {
      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) {
@ -135,7 +138,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);
    if (req->mode != gyro.mode) {
      if (req->mode == acs::SimpleSensorMode::NORMAL) {
        std::memcpy(gyro.sensorCfg, req->ctrlRegs, 5);
@ -154,7 +156,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);
    if (req->mode != mgm.mode) {
      if (req->mode == acs::SimpleSensorMode::NORMAL) {
        mgm.performStartup = true;
@ -173,7 +174,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);
    if (req->mode != mgm.mode) {
      if (req->mode == acs::SimpleSensorMode::NORMAL) {
        mgm.performStartup = true;
@ -184,6 +184,8 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
    }
    return returnvalue::OK;
  };
+  {
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    switch (spiCookie->getChipSelectPin()) {
      case (gpioIds::MGM_0_LIS3_CS): {
        handleLis3Request(mgm0Lis3);
@ -218,11 +220,11 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
        break;
      }
    }
-  MutexGuard mg(ipcLock);
    if (state == InternalState::IDLE) {
      state = InternalState::BUSY;
-    semaphore->release();
    }
+  }
+  semaphore->release();
  return returnvalue::OK;
 }

@ -238,7 +240,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);
@ -294,10 +296,10 @@ 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);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    mode = l3g.mode;
    gyroPerformStartup = l3g.performStartup;
  }
@ -320,7 +322,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 +347,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++) {
@ -440,11 +442,11 @@ 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;
  {
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    mode = gyro.mode;
    cdHasTimedOut = gyro.countdown.hasTimedOut();
    mustPerformStartup = gyro.performStartup;
@ -478,7 +480,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 +527,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];
@ -542,10 +544,10 @@ 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);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    mode = mgm.mode;
    mustPerformStartup = mgm.performStartup;
  }
@ -605,7 +607,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 +629,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];
  }
@ -635,10 +637,10 @@ 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);
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    mode = mgm.mode;
    mustPerformStartup = mgm.performStartup;
  }
@ -712,7 +714,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!
--- 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;

--- a/linux/devices/GpsHyperionLinuxController.cpp
+++ b/linux/devices/GpsHyperionLinuxController.cpp
@ -19,9 +19,7 @@

 GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                                                       bool debugHyperionGps)
-    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {
-  timeUpdateCd.resetTimer();
-}
+    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {}

 GpsHyperionLinuxController::~GpsHyperionLinuxController() {
  gps_stream(&gps, WATCH_DISABLE, nullptr);
@ -196,8 +194,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;
--- 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.
--- a/linux/devices/Max31865RtdPolling.cpp
+++ b/linux/devices/Max31865RtdPolling.cpp
@ -19,25 +19,19 @@ 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) {
  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()) {
+  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;
-  }
-
-#if OBSW_RTD_AUTO_MODE == 0
  result = periodicReadReqHandling();
  if (result != returnvalue::OK) {
    return result;
@ -56,19 +50,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(readerMutex);
+    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;
-      return false;
+        continue;
      }
-    if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
+      mustPerformInitHandling =
+          (rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut();
+      doWriteHighThreshold = rtd->writeHighThreshold;
+      doWriteLowThreshold = rtd->writeLowThreshold;
+    }
+    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 +80,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,38 +96,23 @@ 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() {
  using namespace MAX31865;
+  updateActiveRtdsArray();
  // Now request one shot config for all active RTDs
  for (auto& rtd : rtds) {
    if (rtd == nullptr) {
      continue;
    }
-    MutexGuard mg(readerMutex);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
+    if (activeRtdsArray[rtd->idx]) {
      ReturnValue_t result = writeCfgReg(rtd->spiCookie, BASE_CFG | (1 << CfgBitPos::ONE_SHOT));
      if (result != returnvalue::OK) {
        handleSpiError(rtd, result, "writeCfgReg");
@ -139,17 +127,13 @@ 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;
    }
-    MutexGuard mg(readerMutex);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
+    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 +150,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;
      }
@ -200,7 +185,7 @@ ReturnValue_t Max31865RtdPolling::initializeInterface(CookieIF* cookie) {
    throw std::invalid_argument("Invalid RTD index");
  }
  rtds[rtdCookie->idx] = rtdCookie;
-  MutexGuard mg(readerMutex);
+  MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
  if (dbLen == 0) {
    dbLen = rtdCookie->db.getSerializedSize();
  }
@ -212,16 +197,19 @@ 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;
  }
-  MutexGuard mg(readerMutex);
+  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;
  }
-  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;
@ -240,7 +228,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 +240,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;
@ -312,15 +298,15 @@ ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t

 ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                      size_t* size) {
-  MutexGuard mg(readerMutex);
-  if (mg.getLockResult() != returnvalue::OK) {
-    // TODO: Emit warning
-    return returnvalue::FAILED;
-  }
  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;
+  }
  uint8_t* exchangePtr = rtdCookie->exchangeBuf.data();
  size_t serLen = 0;
  auto result = rtdCookie->db.serialize(&exchangePtr, &serLen, rtdCookie->exchangeBuf.size(),
@ -461,6 +447,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;
--- a/linux/devices/Max31865RtdPolling.h
+++ b/linux/devices/Max31865RtdPolling.h
@ -47,8 +47,12 @@ 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* 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;
@ -56,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();

@ -81,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);
 };

--- 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) {
--- 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;
 }

--- a/mission/config/torquer.h
+++ b/mission/config/torquer.h
@ -9,6 +9,9 @@ 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;
+static constexpr char LOCK_CTX[] = "torquer";

 MutexIF* lazyLock();
 extern bool TORQUEING;
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@ -432,7 +432,8 @@ 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::LOCK_CTX);
    torquer::NEW_ACTUATION_FLAG = true;
    dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
  }
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@ -84,21 +84,23 @@ 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;
  }

  // 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};
-  VectorOperations<double>::normalize(magFieldB, magDirB, 3);
+  VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
  VectorOperations<double>::normalize(susDirB, susDirB, 3);

  // Cosinus angle between sunDir and magDir
@ -159,8 +161,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));
--- 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
--- 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;
--- 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;
--- 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());
+      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;
--- 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, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
        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 {
--- 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;
--- 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();
 }

--- 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;
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc
+Subproject commit 94ae2d16e21ade8f89723b2e62356967a67b171d