Merge pull request 'prep v1.36.0' (#449) from prep_v1.36.0 into develop

Reviewed-on: #449

CHANGELOG.md

@@ -16,7 +16,133 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
-# [v1.32.0]
+# [v1.36.0] 2023-03-08
+
+eive-tmtc: v2.17.2
+
+## Added
+
+- Star Tracker Assembly
+- New `REBOOT_COUNTER` and `INDIVIDUAL_BOOT_COUNTS` events. The first contains the total boot count
+  as a u64, the second one contains the individual boot counts as 4 u16. Add new core controller
+  action command `ANNOUNCE_BOOT_COUNTS` with action ID 3 which triggers both events. These events
+  will also be triggered on each reboot.
+
+## Changed
+
+- Persistent TM stores will now create new files on each reboot.
+- Fast ACS subsystem commanding: Command SUS board consecutively with other devices now
+- Star Tracker: Use ground confguration for EM and flight config for FM by default.
+
+## Fixed
+
+- Command TCS controller off first for TCS subsystem transition to off.
+- Health handling for TCS board assembly
+- Mode fallback from IDLE mode to SAFE mode due to ACS errors/events now works properly for
+  the ACS subsystem
+- Bugfix in IDLE transition for system.
+- `std::filesystem` API usages: Avoid exceptions by using variants which return an error code
+  instead of throwing exceptions.
+- GPS fix loss was not reported if mode is unset.
+- Star Tracker: OFF to NORMAL transition now posssible. Requires FSFW bump which sets
+  transition source modes properly for those transitions.
+  FSFW PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/131
+- Star Tracker JSON initialization is now done during object initization instead of redoing it
+  when building a command. This avoids missed deadlines issues in the ACS PST.
+- Allow arbitrary submodes for dual lane boards to avoid FDIR reactions of subsystem components.
+  Bump FSFW to allow this.
+- PUS 15 was not scheduled
+- Transmitter timeout set to 2 minutes instead of 15 minutes. This will prevent to discharge the
+  battery in case the syrlinks starts transmitting due to detection of unintentional bitlock. This
+  happened e.g. on ground when the uplink to the flying latop was established.
+- ACS system components are now always scheduled (EM specific)
+
+# [v1.35.1] 2023-03-04
+
+## Fixed
+
+- ACS Board 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
+- Watchdog fixes
+- IMTQ timing fixes
+
+## Added
+
+- Add IMTQ assembly
+
+# [v1.35.0] 2023-03-04
+
+eive-tmtc: v2.16.4
+
+## Added
+
+- Improved the OBSW watchdog by adding a watch functionality. The watch functionality is optional
+  and has to be enabled specifically by the application being watched by the watchdog when
+  starting the watchdog. If the watch functionality is enabled and the OBSW has not pinged
+  the watchdog via the FIFO for 2 minutes, the watchdog will restart the OBSW service via systemd.
+  The primary OBSW will only activate the watch functionality if it is the OBSW inside the
+  `/usr/bin` directory. This allows debugging the system by leaving flashed or manually copied
+  debugging images 2 minutes to start the watchdog without the watch functionality.
+
+## Fixed
+
+- Bumped FSFW: `Countdown` and `Stopwatch` use new monotonic clock API now.
+- IMTQ: Various fixes, most notably missing buffer time after starting MGM measurement
+  and corrections for actuator commanding.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/430
+
+# [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
+
+- The SD card prefix is now set earlier inside the `CoreController` constructor
+- The watchdog handling was moved outside the `CoreController` into the main loop.
+- 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.
+
+# [v1.33.0] 2023-03-01
+
+eive-tmtc: v2.16.2
+
+## Changed
+
+- Move ACS board polling to separate worker thread.
+- Move SUS board polling to separate worker thread.
+
+## Fixed
+
+- 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] 2023-02-24
 
 eive-tmtc: v2.16.1
 
@@ -57,7 +183,7 @@ eive-tmtc: v2.16.1
 - `RwDummy` now initializes with a non faulty state
 
 
-# [v1.31.1]
+# [v1.31.1] 2023-02-23
 
 ## Fixed
 
@@ -67,7 +193,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
 
@@ -108,7 +234,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
 
@@ -125,7 +251,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
 
@@ -137,7 +263,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
 
@@ -158,7 +284,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
 

CMakeLists.txt

@@ -10,7 +10,7 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 1)
-set(OBSW_VERSION_MINOR 32)
+set(OBSW_VERSION_MINOR 36)
 set(OBSW_VERSION_REVISION 0)
 
 # set(CMAKE_VERBOSE TRUE)
@@ -71,11 +71,13 @@ if(EIVE_Q7S_EM)
       1
       CACHE STRING "Q7S EM configuration")
   set(INIT_VAL 0)
+  set(OBSW_STAR_TRACKER_GROUND_CONFIG 1)
 else()
   set(OBSW_Q7S_EM
       0
       CACHE STRING "Q7S EM configuration")
   set(INIT_VAL 1)
+  set(OBSW_STAR_TRACKER_GROUND_CONFIG 0)
 endif()
 set(OBSW_ADD_MGT
     ${INIT_VAL}
@@ -295,8 +297,10 @@ include(BuildType)
 set_build_type()
 
 set(FSFW_DEBUG_INFO 0)
+set(Q7S_CHECK_FOR_ALREADY_RUNNING_IMG 0)
 if(RELEASE_BUILD MATCHES 0)
   set(FSFW_DEBUG_INFO 1)
+  set(Q7S_CHECK_FOR_ALREADY_RUNNING_IMG 1)
 endif()
 
 # Configuration files

bsp_hosted/ObjectFactory.cpp

@@ -30,7 +30,6 @@
 #include <dummies/CoreControllerDummy.h>
 
 #include "dummies/helpers.h"
-#include "mission/utility/GlobalConfigHandler.h"
 
 #ifdef PLATFORM_UNIX
 #include <fsfw_hal/linux/serial/SerialComIF.h>

bsp_hosted/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 263 translations.
+ * @brief    Auto-generated event translation file. Contains 269 translations.
  * @details
- * Generated on: 2023-02-24 16:57:00
+ * Generated on: 2023-03-08 16:44:32
  */
 #include "translateEvents.h"
 
@@ -158,6 +158,8 @@ const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC";
 const char *TOO_MANY_IRQS_STRING = "TOO_MANY_IRQS";
 const char *POLL_SYSCALL_ERROR_PDEC_STRING = "POLL_SYSCALL_ERROR_PDEC";
 const char *WRITE_SYSCALL_ERROR_PDEC_STRING = "WRITE_SYSCALL_ERROR_PDEC";
+const char *PDEC_RESET_FAILED_STRING = "PDEC_RESET_FAILED";
+const char *OPEN_IRQ_FILE_FAILED_STRING = "OPEN_IRQ_FILE_FAILED";
 const char *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED";
 const char *IMAGE_DOWNLOAD_FAILED_STRING = "IMAGE_DOWNLOAD_FAILED";
 const char *IMAGE_UPLOAD_SUCCESSFUL_STRING = "IMAGE_UPLOAD_SUCCESSFUL";
@@ -255,7 +257,8 @@ const char *REBOOT_HW_STRING = "REBOOT_HW";
 const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE";
 const char *VERSION_INFO_STRING = "VERSION_INFO";
 const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO";
-const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION";
+const char *REBOOT_COUNTER_STRING = "REBOOT_COUNTER";
+const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS";
 const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE";
 const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE";
 const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING";
@@ -263,6 +266,9 @@ const char *PLOC_OVERHEATING_STRING = "PLOC_OVERHEATING";
 const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING";
 const char *HPA_OVERHEATING_STRING = "HPA_OVERHEATING";
 const char *PLPCDU_OVERHEATING_STRING = "PLPCDU_OVERHEATING";
+const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED";
+const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON";
+const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION";
 
 const char *translateEvents(Event event) {
   switch ((event & 0xFFFF)) {
@@ -572,6 +578,10 @@ const char *translateEvents(Event event) {
       return POLL_SYSCALL_ERROR_PDEC_STRING;
     case (12409):
       return WRITE_SYSCALL_ERROR_PDEC_STRING;
+    case (12410):
+      return PDEC_RESET_FAILED_STRING;
+    case (12411):
+      return OPEN_IRQ_FILE_FAILED_STRING;
     case (12500):
       return IMAGE_UPLOAD_FAILED_STRING;
     case (12501):
@@ -766,22 +776,30 @@ const char *translateEvents(Event event) {
       return VERSION_INFO_STRING;
     case (14006):
       return CURRENT_IMAGE_INFO_STRING;
+    case (14007):
+      return REBOOT_COUNTER_STRING;
+    case (14008):
+      return INDIVIDUAL_BOOT_COUNTS_STRING;
     case (14100):
-      return POSSIBLE_FILE_CORRUPTION_STRING;
-    case (14200):
       return NO_VALID_SENSOR_TEMPERATURE_STRING;
-    case (14201):
+    case (14101):
       return NO_HEALTHY_HEATER_AVAILABLE_STRING;
-    case (14202):
+    case (14102):
       return SYRLINKS_OVERHEATING_STRING;
-    case (14203):
+    case (14103):
       return PLOC_OVERHEATING_STRING;
-    case (14204):
+    case (14104):
       return OBC_OVERHEATING_STRING;
-    case (14205):
+    case (14105):
       return HPA_OVERHEATING_STRING;
-    case (14206):
+    case (14106):
       return PLPCDU_OVERHEATING_STRING;
+    case (14201):
+      return TX_TIMER_EXPIRED_STRING;
+    case (14202):
+      return BIT_LOCK_TX_ON_STRING;
+    case (14300):
+      return POSSIBLE_FILE_CORRUPTION_STRING;
     default:
       return "UNKNOWN_EVENT";
   }

bsp_hosted/fsfwconfig/objects/translateObjects.cpp

@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 154 translations.
- * Generated on: 2023-02-24 16:57:00
+ * Contains 160 translations.
+ * Generated on: 2023-03-08 16:44:32
  */
 #include "translateObjects.h"
 
@@ -85,8 +85,11 @@ const char *SYRLINKS_HANDLER_STRING = "SYRLINKS_HANDLER";
 const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF";
 const char *DUMMY_COM_IF_STRING = "DUMMY_COM_IF";
 const char *SCEX_UART_READER_STRING = "SCEX_UART_READER";
-const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF";
 const char *UART_COM_IF_STRING = "UART_COM_IF";
+const char *ACS_BOARD_POLLING_TASK_STRING = "ACS_BOARD_POLLING_TASK";
+const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK";
+const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF";
+const char *SUS_POLLING_TASK_STRING = "SUS_POLLING_TASK";
 const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR";
 const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR";
 const char *TCP_TMTC_SERVER_STRING = "TCP_TMTC_SERVER";
@@ -139,8 +142,11 @@ 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 *IMTQ_ASSY_STRING = "IMTQ_ASSY";
+const char *STR_ASSY_STRING = "STR_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";
@@ -321,10 +327,16 @@ const char *translateObject(object_id_t object) {
       return DUMMY_COM_IF_STRING;
     case 0x49010006:
       return SCEX_UART_READER_STRING;
-    case 0x49020006:
-      return SPI_RTD_COM_IF_STRING;
     case 0x49030003:
       return UART_COM_IF_STRING;
+    case 0x49060004:
+      return ACS_BOARD_POLLING_TASK_STRING;
+    case 0x49060005:
+      return RW_POLLING_TASK_STRING;
+    case 0x49060006:
+      return SPI_RTD_COM_IF_STRING;
+    case 0x49060007:
+      return SUS_POLLING_TASK_STRING;
     case 0x50000100:
       return CCSDS_PACKET_DISTRIBUTOR_STRING;
     case 0x50000200:
@@ -430,9 +442,15 @@ 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 0x73000008:
+      return IMTQ_ASSY_STRING;
+    case 0x73000009:
+      return STR_ASSY_STRING;
     case 0x73000100:
       return TM_FUNNEL_STRING;
     case 0x73000101:

bsp_q7s/OBSWConfig.h.in

@@ -67,7 +67,7 @@
 #define OBSW_PRINT_MISSED_DEADLINES                 1
 
 #define OBSW_MPSOC_JTAG_BOOT                        0
-#define OBSW_STAR_TRACKER_GROUND_CONFIG             1
+#define OBSW_STAR_TRACKER_GROUND_CONFIG             @OBSW_STAR_TRACKER_GROUND_CONFIG@
 #define OBSW_SYRLINKS_SIMULATED                     @OBSW_SYRLINKS_SIMULATED@
 #define OBSW_ADD_TEST_CODE                          0
 #define OBSW_ADD_TEST_TASK                          0

bsp_q7s/boardconfig/q7sConfig.h.in

@@ -17,7 +17,7 @@
 /*******************************************************************/
 
 // Probably better if this is disabled for mission code. Convenient for development
-#define Q7S_CHECK_FOR_ALREADY_RUNNING_IMG           1
+#define Q7S_CHECK_FOR_ALREADY_RUNNING_IMG           @Q7S_CHECK_FOR_ALREADY_RUNNING_IMG@
 
 #define Q7S_SIMPLE_ADD_FILE_SYSTEM_TEST             0
 

bsp_q7s/core/CMakeLists.txt

@@ -1,4 +1,4 @@
 target_sources(${OBSW_NAME} PRIVATE CoreController.cpp scheduling.cpp
-                                    ObjectFactory.cpp)
+                                    ObjectFactory.cpp WatchdogHandler.cpp)
 
 target_sources(${SIMPLE_OBSW_NAME} PRIVATE scheduling.cpp)

bsp_q7s/core/CoreController.cpp

@@ -33,12 +33,7 @@ xsc::Copy CoreController::CURRENT_COPY = xsc::Copy::NO_COPY;
 
 CoreController::CoreController(object_id_t objectId)
     : ExtendedControllerBase(objectId, 5), opDivider5(5), opDivider10(10), hkSet(this) {
-  ReturnValue_t result = returnvalue::OK;
   try {
-    result = initWatchdogFifo();
-    if (result != returnvalue::OK) {
-      sif::warning << "CoreController::CoreController: Watchdog FIFO init failed" << std::endl;
-    }
     sdcMan = SdCardManager::instance();
     if (sdcMan == nullptr) {
       sif::error << "CoreController::CoreController: SD card manager invalid!" << std::endl;
@@ -47,6 +42,16 @@ CoreController::CoreController(object_id_t objectId)
     if (not BLOCKING_SD_INIT) {
       sdcMan->setBlocking(false);
     }
+    auto sdCard = sdcMan->getPreferredSdCard();
+    if (not sdCard.has_value()) {
+      sif::error << "CoreController::initializeAfterTaskCreation: "
+                    "Issues getting preferred SD card, setting to 0"
+                 << std::endl;
+      sdCard = sd::SdCard::SLOT_0;
+    }
+    sdInfo.active = sdCard.value();
+    sdcMan->setActiveSdCard(sdInfo.active);
+    currMntPrefix = sdcMan->getCurrentMountPrefix();
 
     getCurrentBootCopy(CURRENT_CHIP, CURRENT_COPY);
 
@@ -54,6 +59,10 @@ CoreController::CoreController(object_id_t objectId)
   } catch (const std::filesystem::filesystem_error &e) {
     sif::error << "CoreController::CoreController: Failed with exception " << e.what() << std::endl;
   }
+  // Add script folder to path
+  char *currentEnvPath = getenv("PATH");
+  std::string updatedEnvPath = std::string(currentEnvPath) + ":/home/root/scripts:/usr/local/bin";
+  setenv("PATH", updatedEnvPath.c_str(), true);
   sdCardCheckCd.timeOut();
   eventQueue = QueueFactory::instance()->createMessageQueue(5, EventMessage::MAX_MESSAGE_SIZE);
 }
@@ -78,7 +87,6 @@ void CoreController::performControlOperation() {
       }
     }
   }
-  performWatchdogControlOperation();
   sdStateMachine();
   performMountedSdCardOperations();
   if (sdCardCheckCd.hasTimedOut()) {
@@ -148,22 +156,6 @@ ReturnValue_t CoreController::initialize() {
 
 ReturnValue_t CoreController::initializeAfterTaskCreation() {
   ReturnValue_t result = returnvalue::OK;
-  auto sdCard = sdcMan->getPreferredSdCard();
-  if (not sdCard) {
-    return returnvalue::FAILED;
-  }
-  sdInfo.active = sdCard.value();
-  if (sdInfo.active == sd::SdCard::NONE) {
-    sif::error << "CoreController::initializeAfterTaskCreation: "
-                  "Issues getting preferred SD card, setting to 0"
-               << std::endl;
-    sdInfo.active = sd::SdCard::SLOT_0;
-  }
-  sdcMan->setActiveSdCard(sdInfo.active);
-  currMntPrefix = sdcMan->getCurrentMountPrefix();
-  if (currMntPrefix == "") {
-    return ObjectManagerIF::CHILD_INIT_FAILED;
-  }
   if (BLOCKING_SD_INIT) {
     result = initSdCardBlocking();
     if (result != returnvalue::OK and result != SdCardManager::ALREADY_MOUNTED) {
@@ -175,12 +167,7 @@ ReturnValue_t CoreController::initializeAfterTaskCreation() {
   if (result != returnvalue::OK) {
     sif::warning << "CoreController::initialize: Version initialization failed" << std::endl;
   }
-  // Add script folder to path
-  char *currentEnvPath = getenv("PATH");
-  std::string updatedEnvPath = std::string(currentEnvPath) + ":/home/root/scripts:/usr/local/bin";
-  setenv("PATH", updatedEnvPath.c_str(), true);
   updateProtInfo();
-  initPrint();
   return ExtendedControllerBase::initializeAfterTaskCreation();
 }
 
@@ -203,6 +190,10 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_
       triggerEvent(VERSION_INFO, p1, p2);
       return HasActionsIF::EXECUTION_FINISHED;
     }
+    case (ANNOUNCE_BOOT_COUNTS): {
+      announceBootCounts();
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
     case (ANNOUNCE_CURRENT_IMAGE): {
       triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY);
       return HasActionsIF::EXECUTION_FINISHED;
@@ -394,7 +385,7 @@ ReturnValue_t CoreController::sdStateMachine() {
       sdInfo.cycleCount = 0;
       return true;
     } else if (sdInfo.cycleCount > 4) {
-      sif::warning << "CoreController::sdInitStateMachine: " << opPrintout << " takes too long"
+      sif::warning << "CoreController::sdStateMachine: " << opPrintout << " takes too long"
                    << std::endl;
       return false;
     }
@@ -406,7 +397,7 @@ ReturnValue_t CoreController::sdStateMachine() {
       // Create updated status file
       result = sdcMan->updateSdCardStateFile();
       if (result != returnvalue::OK) {
-        sif::warning << "CoreController::initialize: Updating SD card state file failed"
+        sif::warning << "CoreController::sdStateMachine: Updating SD card state file failed"
                      << std::endl;
       }
       sdInfo.commandExecuted = true;
@@ -614,7 +605,8 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS
       sif::info << "Unmounting SD card " << sdChar << std::endl;
       return sdcMan->unmountSdCard(sdCard);
     } else {
-      if (std::filesystem::exists(mountString)) {
+      std::error_code e;
+      if (std::filesystem::exists(mountString, e)) {
         sif::info << "SD card " << sdChar << " already on and mounted at " << mountString
                   << std::endl;
         return SdCardManager::ALREADY_MOUNTED;
@@ -711,7 +703,8 @@ ReturnValue_t CoreController::initVersionFile() {
   std::string versionFilePath = currMntPrefix + VERSION_FILE;
   std::fstream versionFile;
 
-  if (not std::filesystem::exists(versionFilePath)) {
+  std::error_code e;
+  if (not std::filesystem::exists(versionFilePath, e)) {
     sif::info << "Writing version file " << versionFilePath << ".." << std::endl;
     versionFile.open(versionFilePath, std::ios_base::out);
     versionFile << fullObswVersionString << std::endl;
@@ -823,7 +816,8 @@ ReturnValue_t CoreController::actionListDirectoryIntoFile(ActionId_t actionId,
 }
 
 ReturnValue_t CoreController::initBootCopyFile() {
-  if (not std::filesystem::exists(CURR_COPY_FILE)) {
+  std::error_code e;
+  if (not std::filesystem::exists(CURR_COPY_FILE, e)) {
     // This file is created by the systemd service eive-early-config so this should
     // not happen normally
     std::string cmd = "xsc_boot_copy > " + std::string(CURR_COPY_FILE);
@@ -844,36 +838,6 @@ void CoreController::getCurrentBootCopy(xsc::Chip &chip, xsc::Copy &copy) {
   copy = static_cast<xsc::Copy>(xscCopy);
 }
 
-ReturnValue_t CoreController::initWatchdogFifo() {
-  if (not std::filesystem::exists(watchdog::FIFO_NAME)) {
-    // Still return returnvalue::OK for now
-    sif::info << "Watchdog FIFO " << watchdog::FIFO_NAME << " does not exist, can't initiate"
-              << " watchdog" << std::endl;
-    return returnvalue::OK;
-  }
-  // Open FIFO write only and non-blocking to prevent SW from killing itself.
-  watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK);
-  if (watchdogFifoFd < 0) {
-    if (errno == ENXIO) {
-      watchdogFifoFd = RETRY_FIFO_OPEN;
-      sif::info << "eive-watchdog not running. FIFO can not be opened" << std::endl;
-    } else {
-      sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " << errno
-                 << ": " << strerror(errno) << std::endl;
-      return returnvalue::FAILED;
-    }
-  }
-  return returnvalue::OK;
-}
-
-void CoreController::initPrint() {
-#if OBSW_VERBOSE_LEVEL >= 1
-  if (watchdogFifoFd > 0) {
-    sif::info << "Opened watchdog FIFO successfully.." << std::endl;
-  }
-#endif
-}
-
 ReturnValue_t CoreController::actionXscReboot(const uint8_t *data, size_t size) {
   if (size < 1) {
     return HasActionsIF::INVALID_PARAMETERS;
@@ -1157,7 +1121,8 @@ ReturnValue_t CoreController::updateProtInfo(bool regenerateChipStateFile) {
       return result;
     }
   }
-  if (not filesystem::exists(CHIP_STATE_FILE)) {
+  std::error_code e;
+  if (not filesystem::exists(CHIP_STATE_FILE, e)) {
     return returnvalue::FAILED;
   }
   ifstream chipStateFile(CHIP_STATE_FILE);
@@ -1224,38 +1189,11 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
       }
     }
     wordIdx++;
-  }
-  return returnvalue::OK;
-}
-
-void CoreController::performWatchdogControlOperation() {
-  // Only perform each fifth iteration
-  if (watchdogFifoFd != 0 and opDivider5.check()) {
-    if (watchdogFifoFd == RETRY_FIFO_OPEN) {
-      // Open FIFO write only and non-blocking
-      watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK);
-      if (watchdogFifoFd < 0) {
-        if (errno == ENXIO) {
-          watchdogFifoFd = RETRY_FIFO_OPEN;
-          // No printout for now, would be spam
-          return;
-        } else {
-          sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with "
-                     << errno << ": " << strerror(errno) << std::endl;
-          return;
-        }
-      }
-      sif::info << "Opened " << watchdog::FIFO_NAME << " successfully" << std::endl;
-    } else if (watchdogFifoFd > 0) {
-      // Write to OBSW watchdog FIFO here
-      const char writeChar = 'a';
-      ssize_t writtenBytes = write(watchdogFifoFd, &writeChar, 1);
-      if (writtenBytes < 0) {
-        sif::error << "Errors writing to watchdog FIFO, code " << errno << ": " << strerror(errno)
-                   << std::endl;
-      }
+    if (wordIdx >= 10) {
+      break;
     }
   }
+  return returnvalue::OK;
 }
 
 void CoreController::performMountedSdCardOperations() {
@@ -1263,8 +1201,14 @@ void CoreController::performMountedSdCardOperations() {
     if (not performOneShotSdCardOpsSwitch) {
       std::ostringstream path;
       path << mntPoint << "/" << CONF_FOLDER;
-      if (not std::filesystem::exists(path.str())) {
-        std::filesystem::create_directory(path.str());
+      std::error_code e;
+      if (not std::filesystem::exists(path.str()), e) {
+        bool created = std::filesystem::create_directory(path.str(), e);
+        if (not created) {
+          sif::error << "Could not create CONF folder at " << path.str() << ": " << e.message()
+                     << std::endl;
+          return;
+        }
       }
       initVersionFile();
       ReturnValue_t result = initBootCopyFile();
@@ -1349,7 +1293,8 @@ ReturnValue_t CoreController::performSdCardCheck() {
 void CoreController::performRebootFileHandling(bool recreateFile) {
   using namespace std;
   std::string path = currMntPrefix + REBOOT_FILE;
-  if (not std::filesystem::exists(path) or recreateFile) {
+  std::error_code e;
+  if (not std::filesystem::exists(path, e) or recreateFile) {
 #if OBSW_VERBOSE_LEVEL >= 1
     sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl;
 #endif
@@ -1391,13 +1336,13 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
   if (rebootFile.bootFlag) {
     // Trigger event to inform ground that a reboot was triggered
     uint32_t p1 = rebootFile.lastChip << 16 | rebootFile.lastCopy;
-    uint32_t p2 = rebootFile.img00Cnt << 24 | rebootFile.img01Cnt << 16 | rebootFile.img10Cnt << 8 |
-                  rebootFile.img11Cnt;
-    triggerEvent(REBOOT_MECHANISM_TRIGGERED, p1, p2);
+    triggerEvent(REBOOT_MECHANISM_TRIGGERED, p1, 0);
     // Clear the boot flag
     rebootFile.bootFlag = false;
   }
 
+  announceBootCounts();
+
   if (rebootFile.mechanismNextChip != xsc::NO_CHIP and
       rebootFile.mechanismNextCopy != xsc::NO_COPY) {
     if (CURRENT_CHIP != rebootFile.mechanismNextChip or
@@ -1816,7 +1761,8 @@ ReturnValue_t CoreController::initClockFromTimeFile() {
   using namespace GpsHyperion;
   using namespace std;
   std::string fileName = currMntPrefix + BACKUP_TIME_FILE;
-  if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName) and
+  std::error_code e;
+  if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName, e) and
       ((gpsFix == FixMode::UNKNOWN or gpsFix == FixMode::NOT_SEEN) or
        not utility::timeSanityCheck())) {
     ifstream timeFile(fileName);
@@ -1941,7 +1887,8 @@ ReturnValue_t CoreController::executeSwUpdate(SwUpdateSources sourceDir, const u
     prefixPath = path("/tmp");
   }
   path archivePath = prefixPath / path(config::OBSW_UPDATE_ARCHIVE_FILE_NAME);
-  if (not exists(archivePath)) {
+  std::error_code e;
+  if (not exists(archivePath, e)) {
     return HasFileSystemIF::FILE_DOES_NOT_EXIST;
   }
   ostringstream cmd("tar -xJf", ios::app);
@@ -1951,12 +1898,12 @@ ReturnValue_t CoreController::executeSwUpdate(SwUpdateSources sourceDir, const u
     utility::handleSystemError(result, "CoreController::executeAction: SW Update Decompression");
   }
   path strippedImagePath = prefixPath / path(config::STRIPPED_OBSW_BINARY_FILE_NAME);
-  if (!exists(strippedImagePath)) {
+  if (!exists(strippedImagePath, e)) {
     // TODO: Custom returnvalue?
     return returnvalue::FAILED;
   }
   path obswVersionFilePath = prefixPath / path(config::OBSW_VERSION_FILE_NAME);
-  if (!exists(obswVersionFilePath)) {
+  if (!exists(obswVersionFilePath, e)) {
     // TODO: Custom returnvalue?
     return returnvalue::FAILED;
   }
@@ -2053,6 +2000,15 @@ bool CoreController::startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mo
   return true;
 }
 
+void CoreController::announceBootCounts() {
+  uint64_t totalBootCount =
+      rebootFile.img00Cnt + rebootFile.img01Cnt + rebootFile.img10Cnt + rebootFile.img11Cnt;
+  uint32_t individualBootCountsP1 = (rebootFile.img00Cnt << 16) | rebootFile.img01Cnt;
+  uint32_t individualBootCountsP2 = (rebootFile.img10Cnt << 16) | rebootFile.img11Cnt;
+  triggerEvent(INDIVIDUAL_BOOT_COUNTS, individualBootCountsP1, individualBootCountsP2);
+  triggerEvent(REBOOT_COUNTER, (totalBootCount >> 32) & 0xffffffff, totalBootCount & 0xffffffff);
+}
+
 bool CoreController::isNumber(const std::string &s) {
   return !s.empty() && std::find_if(s.begin(), s.end(),
                                     [](unsigned char c) { return !std::isdigit(c); }) == s.end();

bsp_q7s/core/CoreController.h

@@ -78,6 +78,7 @@ class CoreController : public ExtendedControllerBase {
   static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0;
   static constexpr ActionId_t ANNOUNCE_VERSION = 1;
   static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2;
+  static constexpr ActionId_t ANNOUNCE_BOOT_COUNTS = 3;
   static constexpr ActionId_t SWITCH_REBOOT_FILE_HANDLING = 5;
   static constexpr ActionId_t RESET_REBOOT_COUNTERS = 6;
   static constexpr ActionId_t SWITCH_IMG_LOCK = 7;
@@ -102,7 +103,7 @@ class CoreController : public ExtendedControllerBase {
   static constexpr Event ALLOC_FAILURE = event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM);
   //! [EXPORT] : [COMMENT]  Software reboot occurred. Can also be a systemd reboot.
   //! P1: Current Chip, P2: Current Copy
-  static constexpr Event REBOOT_SW = event::makeEvent(SUBSYSTEM_ID, 1, severity::MEDIUM);
+  static constexpr Event REBOOT_SW = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW);
   //! [EXPORT] : [COMMENT] The reboot mechanism was triggered.
   //! P1: First 16 bits: Last Chip, Last 16 bits: Last Copy,
   //! P2: Each byte is the respective reboot count for the slots
@@ -119,6 +120,13 @@ class CoreController : public ExtendedControllerBase {
   static constexpr Event VERSION_INFO = event::makeEvent(SUBSYSTEM_ID, 5, severity::INFO);
   //! [EXPORT] : [COMMENT] P1: Current Chip, P2: Current Copy
   static constexpr Event CURRENT_IMAGE_INFO = event::makeEvent(SUBSYSTEM_ID, 6, severity::INFO);
+  //! [EXPORT] : [COMMENT] Total reboot counter, which is the sum of the boot count of all
+  //! individual images.
+  static constexpr Event REBOOT_COUNTER = event::makeEvent(SUBSYSTEM_ID, 7, severity::INFO);
+  //! [EXPORT] : [COMMENT] Get the boot count of the individual images.
+  //! P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1.
+  //! P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1.
+  static constexpr Event INDIVIDUAL_BOOT_COUNTS = event::makeEvent(SUBSYSTEM_ID, 8, severity::INFO);
 
   CoreController(object_id_t objectId);
   virtual ~CoreController();
@@ -164,9 +172,6 @@ class CoreController : public ExtendedControllerBase {
   static constexpr uint32_t BOOT_OFFSET_SECONDS = 15;
   static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
   static constexpr uint32_t MUTEX_TIMEOUT = 20;
-  // Designated value for rechecking FIFO open
-  static constexpr int RETRY_FIFO_OPEN = -2;
-  int watchdogFifoFd = 0;
   GpsHyperion::FixMode gpsFix = GpsHyperion::FixMode::UNKNOWN;
 
   // States for SD state machine, which is used in non-blocking mode
@@ -263,7 +268,6 @@ class CoreController : public ExtendedControllerBase {
   ReturnValue_t performSdCardCheck();
   ReturnValue_t backupTimeFileHandler();
   ReturnValue_t initBootCopyFile();
-  ReturnValue_t initWatchdogFifo();
   ReturnValue_t initSdCardBlocking();
   bool startSdStateMachine(sd::SdCard targetActiveSd, SdCfgMode mode, MessageQueueId_t commander,
                            DeviceCommandId_t actionId);
@@ -288,8 +292,6 @@ class CoreController : public ExtendedControllerBase {
 
   ReturnValue_t gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, bool& protOpPerformed);
 
-  void performWatchdogControlOperation();
-
   ReturnValue_t handleProtInfoUpdateLine(std::string nextLine);
   int handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect,
                                 bool& protOperationPerformed, bool selfChip, bool selfCopy,
@@ -300,6 +302,7 @@ class CoreController : public ExtendedControllerBase {
   void setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::Copy tgtCopy);
   bool parseRebootFile(std::string path, RebootFile& file);
   void rewriteRebootFile(RebootFile file);
+  void announceBootCounts();
   void readHkData();
   bool isNumber(const std::string& s);
 };

bsp_q7s/core/ObjectFactory.cpp

@@ -1,9 +1,17 @@
 #include "ObjectFactory.h"
 
 #include <fsfw/subsystem/Subsystem.h>
+#include <linux/devices/AcsBoardPolling.h>
 #include <linux/devices/ImtqPollingTask.h>
 #include <linux/devices/RwPollingTask.h>
+#include <mission/devices/GyrL3gCustomHandler.h>
+#include <mission/devices/MgmLis3CustomHandler.h>
+#include <mission/devices/MgmRm3100CustomHandler.h>
+#include <mission/system/fdir/StrFdir.h>
 #include <mission/system/objects/CamSwitcher.h>
+#include <mission/system/objects/ImtqAssembly.h>
+#include <mission/system/objects/StrAssembly.h>
+#include <mission/system/objects/SyrlinksAssembly.h>
 
 #include "OBSWConfig.h"
 #include "bsp_q7s/boardtest/Q7STestTask.h"
@@ -40,6 +48,7 @@
 #include "linux/ipcore/PdecHandler.h"
 #include "linux/ipcore/Ptme.h"
 #include "linux/ipcore/PtmeConfig.h"
+#include "mission/config/configfile.h"
 #include "mission/csp/CspCookie.h"
 #include "mission/system/fdir/AcsBoardFdir.h"
 #include "mission/system/fdir/GomspacePowerFdir.h"
@@ -53,10 +62,12 @@
 #include "mission/system/tree/comModeTree.h"
 #include "mission/system/tree/payloadModeTree.h"
 #include "mission/system/tree/tcsModeTree.h"
+#include "mission/utility/GlobalConfigHandler.h"
 #include "tmtc/pusIds.h"
 #if OBSW_TEST_LIBGPIOD == 1
 #include "linux/boardtest/LibgpiodTest.h"
 #endif
+#include <mission/devices/GyrAdis1650XHandler.h>
 #include <mission/devices/ImtqHandler.h>
 #include <mission/devices/PcduHandler.h>
 #include <mission/devices/SyrlinksHandler.h>
@@ -83,7 +94,6 @@
 #include "mission/core/GenericFactory.h"
 #include "mission/devices/ACUHandler.h"
 #include "mission/devices/BpxBatteryHandler.h"
-#include "mission/devices/GyroADIS1650XHandler.h"
 #include "mission/devices/HeaterHandler.h"
 #include "mission/devices/Max31865PT1000Handler.h"
 #include "mission/devices/P60DockHandler.h"
@@ -239,8 +249,8 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF,
   return returnvalue::OK;
 }
 
-void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
-                                             PowerSwitchIF& pwrSwitcher) {
+void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF,
+                                             SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher) {
   using namespace gpio;
   GpioCookie* gpioCookieAcsBoard = new GpioCookie();
 
@@ -343,14 +353,16 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   static_cast<void>(fdir);
 
 #if OBSW_ADD_ACS_BOARD == 1
+  new AcsBoardPolling(objects::ACS_BOARD_POLLING_TASK, spiComIF, *gpioComIF);
   std::string spiDev = q7s::SPI_DEFAULT_DEV;
   std::array<DeviceHandlerBase*, 8> assemblyChildren;
   SpiCookie* spiCookie =
-      new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE,
+      new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE,
                     spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  auto mgmLis3Handler0 = new MgmLIS3MDLHandler(
-      objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY);
+  auto mgmLis3Handler0 =
+      new MgmLis3CustomHandler(objects::MGM_0_LIS3_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                               spiCookie, spi::LIS3_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER);
   mgmLis3Handler0->setCustomFdir(fdir);
   assemblyChildren[0] = mgmLis3Handler0;
@@ -362,12 +374,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   mgmLis3Handler->enablePeriodicPrintouts(true, 10);
 #endif
   spiCookie =
-      new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, RM3100::MAX_BUFFER_SIZE,
+      new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, mgmRm3100::MAX_BUFFER_SIZE,
                     spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto mgmRm3100Handler1 =
-      new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
-                           spi::RM3100_TRANSITION_DELAY);
+      new MgmRm3100CustomHandler(objects::MGM_1_RM3100_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                                 spiCookie, spi::RM3100_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER);
   mgmRm3100Handler1->setCustomFdir(fdir);
   assemblyChildren[1] = mgmRm3100Handler1;
@@ -378,12 +390,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
 #if OBSW_DEBUG_ACS == 1
   mgmRm3100Handler->enablePeriodicPrintouts(true, 10);
 #endif
-  spiCookie =
-      new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE,
-                    spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
+  spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE,
+                            spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  auto* mgmLis3Handler2 = new MgmLIS3MDLHandler(
-      objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY);
+  auto* mgmLis3Handler2 =
+      new MgmLis3CustomHandler(objects::MGM_2_LIS3_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                               spiCookie, spi::LIS3_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER);
   mgmLis3Handler2->setCustomFdir(fdir);
   assemblyChildren[2] = mgmLis3Handler2;
@@ -395,12 +407,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   mgmLis3Handler->enablePeriodicPrintouts(true, 10);
 #endif
   spiCookie =
-      new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, RM3100::MAX_BUFFER_SIZE,
+      new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, mgmRm3100::MAX_BUFFER_SIZE,
                     spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto* mgmRm3100Handler3 =
-      new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
-                           spi::RM3100_TRANSITION_DELAY);
+      new MgmRm3100CustomHandler(objects::MGM_3_RM3100_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                                 spiCookie, spi::RM3100_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER);
   mgmRm3100Handler3->setCustomFdir(fdir);
   assemblyChildren[3] = mgmRm3100Handler3;
@@ -414,12 +426,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   // Commented until ACS board V2 in in clean room again
   // Gyro 0 Side A
   spiCookie =
-      new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE,
+      new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE,
                     spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto adisHandler =
-      new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
-                               ADIS1650X::Type::ADIS16505);
+      new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                              spiCookie, adis1650x::Type::ADIS16505);
   fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER);
   adisHandler->setCustomFdir(fdir);
   assemblyChildren[4] = adisHandler;
@@ -431,11 +443,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   adisHandler->enablePeriodicPrintouts(true, 10);
 #endif
   // Gyro 1 Side A
-  spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, L3GD20H::MAX_BUFFER_SIZE,
+  spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, l3gd20h::MAX_BUFFER_SIZE,
                             spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  auto gyroL3gHandler1 = new GyroHandlerL3GD20H(
-      objects::GYRO_1_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY);
+  auto gyroL3gHandler1 =
+      new GyrL3gCustomHandler(objects::GYRO_1_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                              spiCookie, spi::L3G_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER);
   gyroL3gHandler1->setCustomFdir(fdir);
   assemblyChildren[5] = gyroL3gHandler1;
@@ -448,11 +461,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
 #endif
   // Gyro 2 Side B
   spiCookie =
-      new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE,
+      new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE,
                     spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_MAIN_COM_IF,
-                                         spiCookie, ADIS1650X::Type::ADIS16505);
+  adisHandler =
+      new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                              spiCookie, adis1650x::Type::ADIS16505);
   fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER);
   adisHandler->setCustomFdir(fdir);
   assemblyChildren[6] = adisHandler;
@@ -461,11 +475,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   adisHandler->setToGoToNormalModeImmediately();
 #endif
   // Gyro 3 Side B
-  spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, L3GD20H::MAX_BUFFER_SIZE,
+  spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, l3gd20h::MAX_BUFFER_SIZE,
                             spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  auto gyroL3gHandler3 = new GyroHandlerL3GD20H(
-      objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY);
+  auto gyroL3gHandler3 =
+      new GyrL3gCustomHandler(objects::GYRO_3_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                              spiCookie, spi::L3G_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER);
   gyroL3gHandler3->setCustomFdir(fdir);
   assemblyChildren[7] = gyroL3gHandler3;
@@ -568,12 +583,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
@@ -888,26 +905,47 @@ void ObjectFactory::createTestComponents(LinuxLibgpioIF* gpioComIF) {
 }
 
 void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) {
+  auto* strAssy = new StrAssembly(objects::STR_ASSY);
+  strAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
   auto* starTrackerCookie =
       new SerialCookie(objects::STAR_TRACKER, q7s::UART_STAR_TRACKER_DEV, uart::STAR_TRACKER_BAUD,
                        startracker::MAX_FRAME_SIZE * 2 + 2, UartModes::NON_CANONICAL);
   starTrackerCookie->setNoFixedSizeReply();
   StrHelper* strHelper = new StrHelper(objects::STR_HELPER);
+
+  const char* paramJsonFile = nullptr;
+#ifdef EGSE
+  paramJsonFile = "/home/pi/arcsec/json/flight-config.json";
+#else
+#if OBSW_STAR_TRACKER_GROUND_CONFIG == 1
+  paramJsonFile = "/mnt/sd0/startracker/ground-config.json";
+#else
+  paramJsonFile = "/mnt/sd0/startracker/flight-config.json";
+#endif
+#endif
+  if (paramJsonFile == nullptr) {
+    sif::error << "No valid Star Tracker parameter JSON file" << std::endl;
+  }
+  auto strFdir = new StrFdir(objects::STAR_TRACKER);
   auto starTracker =
       new StarTrackerHandler(objects::STAR_TRACKER, objects::UART_COM_IF, starTrackerCookie,
-                             strHelper, pcdu::PDU1_CH2_STAR_TRACKER_5V);
+                             paramJsonFile, strHelper, pcdu::PDU1_CH2_STAR_TRACKER_5V);
   starTracker->setPowerSwitcher(pwrSwitcher);
-  starTracker->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
+  starTracker->connectModeTreeParent(*strAssy);
+  starTracker->setCustomFdir(strFdir);
 }
 
 void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) {
+  auto* imtqAssy = new ImtqAssembly(objects::IMTQ_ASSY);
+  imtqAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
+
   new ImtqPollingTask(objects::IMTQ_POLLING);
   I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE);
   auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::IMTQ_POLLING, imtqI2cCookie,
                                      pcdu::Switches::PDU1_CH3_MGT_5V);
   imtqHandler->enableThermalModule(ThermalStateCfg());
   imtqHandler->setPowerSwitcher(pwrSwitcher);
-  imtqHandler->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
+  imtqHandler->connectModeTreeParent(*imtqAssy);
   static_cast<void>(imtqHandler);
 #if OBSW_TEST_IMTQ == 1
   imtqHandler->setStartUpImmediately();

bsp_q7s/core/ObjectFactory.h

@@ -2,6 +2,7 @@
 #define BSP_Q7S_OBJECTFACTORY_H_
 
 #include <fsfw/returnvalues/returnvalue.h>
+#include <fsfw_hal/linux/spi/SpiComIF.h>
 #include <mission/devices/HeaterHandler.h>
 #include <mission/system/objects/Stack5VHandler.h>
 #include <mission/tmtc/CcsdsIpCoreHandler.h>
@@ -31,7 +32,7 @@ void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF,
                             PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler);
 void createTmpComponents();
 ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler);
-void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
+void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
                               PowerSwitchIF& pwrSwitcher);
 void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable,
                             HeaterHandler*& heaterHandler);

bsp_q7s/core/WatchdogHandler.cpp

@@ -0,0 +1,87 @@
+#include "WatchdogHandler.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <cerrno>
+#include <cstring>
+#include <filesystem>
+
+#include "fsfw/serviceinterface.h"
+#include "watchdog/definitions.h"
+
+WatchdogHandler::WatchdogHandler() {}
+
+void WatchdogHandler::periodicOperation() {
+  if (watchdogFifoFd != 0) {
+    if (watchdogFifoFd == RETRY_FIFO_OPEN) {
+      // Open FIFO write only and non-blocking
+      watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK);
+      if (watchdogFifoFd < 0) {
+        if (errno == ENXIO) {
+          watchdogFifoFd = RETRY_FIFO_OPEN;
+          // No printout for now, would be spam
+          return;
+        } else {
+          sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with "
+                     << errno << ": " << strerror(errno) << std::endl;
+          return;
+        }
+      }
+      sif::info << "Opened " << watchdog::FIFO_NAME << " successfully" << std::endl;
+      performStartHandling();
+    } else if (watchdogFifoFd > 0) {
+      // Write to OBSW watchdog FIFO here
+      const char writeChar = watchdog::first::IDLE_CHAR;
+      ssize_t writtenBytes = write(watchdogFifoFd, &writeChar, 1);
+      if (writtenBytes < 0) {
+        sif::error << "Errors writing to watchdog FIFO, code " << errno << ": " << strerror(errno)
+                   << std::endl;
+      }
+    }
+  }
+}
+
+ReturnValue_t WatchdogHandler::initialize(bool enableWatchdogFunction) {
+  using namespace std::filesystem;
+  this->enableWatchFunction = enableWatchdogFunction;
+  std::error_code e;
+  if (not std::filesystem::exists(watchdog::FIFO_NAME, e)) {
+    // Still return returnvalue::OK for now
+    sif::info << "Watchdog FIFO " << watchdog::FIFO_NAME << " does not exist, can't initiate"
+              << " watchdog" << std::endl;
+    return returnvalue::OK;
+  }
+  // Open FIFO write only and non-blocking to prevent SW from killing itself.
+  watchdogFifoFd = open(watchdog::FIFO_NAME.c_str(), O_WRONLY | O_NONBLOCK);
+  if (watchdogFifoFd < 0) {
+    if (errno == ENXIO) {
+      watchdogFifoFd = RETRY_FIFO_OPEN;
+      sif::info << "eive-watchdog not running. FIFO can not be opened" << std::endl;
+    } else {
+      sif::error << "Opening pipe " << watchdog::FIFO_NAME << " write-only failed with " << errno
+                 << ": " << strerror(errno) << std::endl;
+      return returnvalue::FAILED;
+    }
+  }
+  return performStartHandling();
+}
+
+ReturnValue_t WatchdogHandler::performStartHandling() {
+  char startBuf[2];
+  ssize_t writeLen = 1;
+  startBuf[0] = watchdog::first::START_CHAR;
+  if (enableWatchFunction) {
+    writeLen += 1;
+    startBuf[1] = watchdog::second::WATCH_FLAG;
+  }
+  ssize_t writtenBytes = write(watchdogFifoFd, &startBuf, writeLen);
+  if (writtenBytes < 0) {
+    sif::error << "WatchdogHandler: Errors writing to watchdog FIFO, code " << errno << ": "
+               << strerror(errno) << std::endl;
+    return returnvalue::FAILED;
+  } else if (writtenBytes != writeLen) {
+    sif::warning << "WatchdogHandler: Not all bytes were written, possible error" << std::endl;
+  }
+  return returnvalue::OK;
+}

bsp_q7s/core/WatchdogHandler.h

@@ -0,0 +1,23 @@
+#ifndef BSP_Q7S_CORE_WATCHDOGHANDLER_H_
+#define BSP_Q7S_CORE_WATCHDOGHANDLER_H_
+
+#include "fsfw/returnvalues/returnvalue.h"
+
+class WatchdogHandler {
+ public:
+  WatchdogHandler();
+
+  ReturnValue_t initialize(bool enableWatchFunction);
+  void periodicOperation();
+
+ private:
+  // Designated value for rechecking FIFO open
+  static constexpr int RETRY_FIFO_OPEN = -2;
+
+  int watchdogFifoFd = 0;
+  bool enableWatchFunction = false;
+
+  ReturnValue_t performStartHandling();
+};
+
+#endif /* BSP_Q7S_CORE_WATCHDOGHANDLER_H_ */

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);
@@ -194,6 +198,15 @@ void scheduling::initTasks() {
     scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER);
   }
 
+#if OBSW_ADD_ACS_BOARD == 1
+  PeriodicTaskIF* acsBrdPolling = factory->createPeriodicTask(
+      "ACS_BOARD_POLLING", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+  result = acsBrdPolling->addComponent(objects::ACS_BOARD_POLLING_TASK);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("ACS_BOARD_POLLING", objects::ACS_BOARD_POLLING_TASK);
+  }
+#endif
+
 #if OBSW_ADD_RW == 1
   PeriodicTaskIF* rwPolling = factory->createPeriodicTask(
       "RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
@@ -211,6 +224,15 @@ void scheduling::initTasks() {
   }
 #endif
 
+#if OBSW_ADD_SUN_SENSORS == 1
+  PeriodicTaskIF* susPolling = factory->createPeriodicTask(
+      "SUS_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+  result = susPolling->addComponent(objects::SUS_POLLING_TASK);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("SUS_POLLING_TASK", objects::SUS_POLLING_TASK);
+  }
+#endif
+
   PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
       "ACS_SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
   static_cast<void>(acsSysTask);
@@ -218,24 +240,26 @@ void scheduling::initTasks() {
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM);
   }
-#if OBSW_ADD_ACS_BOARD == 1
+  result = acsSysTask->addComponent(objects::IMTQ_ASSY);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("IMTQ_ASSY", objects::IMTQ_ASSY);
+  }
   result = acsSysTask->addComponent(objects::ACS_BOARD_ASS);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("ACS_BOARD_ASS", objects::ACS_BOARD_ASS);
   }
-#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
   result = acsSysTask->addComponent(objects::SUS_BOARD_ASS);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("SUS_BOARD_ASS", objects::SUS_BOARD_ASS);
   }
-#endif
+  result = acsSysTask->addComponent(objects::STR_ASSY);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("STR_ASSY", objects::STR_ASSY);
+  }
 
   PeriodicTaskIF* tcsSystemTask = factory->createPeriodicTask(
       "TCS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.5, missedDeadlineFunc);
@@ -351,9 +375,15 @@ void scheduling::initTasks() {
 #if OBSW_ADD_SA_DEPL == 1
   solarArrayDeplTask->startTask();
 #endif
+#if OBSW_ADD_ACS_BOARD == 1
+  acsBrdPolling->startTask();
+#endif
 #if OBSW_ADD_MGT == 1
   imtqPolling->startTask();
 #endif
+#if OBSW_ADD_SUN_SENSORS == 1
+  susPolling->startTask();
+#endif
 
   taskStarter(pstTasks, "PST task vector");
   taskStarter(pusTasks, "PUS task vector");
@@ -493,6 +523,10 @@ void scheduling::createPusTasks(TaskFactory& factory, TaskDeadlineMissedFunction
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS_8", objects::PUS_SERVICE_8_FUNCTION_MGMT);
   }
+  result = pusMedPrio->addComponent(objects::PUS_SERVICE_15_TM_STORAGE);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("PUS_15", objects::PUS_SERVICE_15_TM_STORAGE);
+  }
   result = pusMedPrio->addComponent(objects::PUS_SERVICE_11_TC_SCHEDULER);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS_11", objects::PUS_SERVICE_11_TC_SCHEDULER);

bsp_q7s/fmObjectFactory.cpp

@@ -43,7 +43,7 @@ void ObjectFactory::produce(void* args) {
 #endif
 
 #if OBSW_ADD_ACS_BOARD == 1
-  createAcsBoardComponents(gpioComIF, uartComIF, *pwrSwitcher);
+  createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher);
 #endif
   HeaterHandler* heaterHandler;
   createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler);

bsp_q7s/fs/FilesystemHelper.cpp

@@ -22,7 +22,7 @@ ReturnValue_t FilesystemHelper::checkPath(std::string path) {
     }
   } else if (path.substr(0, sizeof(config::SD_1_MOUNT_POINT)) ==
              std::string(config::SD_1_MOUNT_POINT)) {
-    if (!sdcMan->isSdCardUsable(sd::SLOT_0)) {
+    if (!sdcMan->isSdCardUsable(sd::SLOT_1)) {
       sif::warning << "FilesystemHelper::checkPath: SD card 1 not mounted" << std::endl;
       return SD_NOT_MOUNTED;
     }

bsp_q7s/fs/SdCardManager.cpp

@@ -20,16 +20,16 @@
 SdCardManager* SdCardManager::INSTANCE = nullptr;
 
 SdCardManager::SdCardManager() : SystemObject(objects::SDC_MANAGER), cmdExecutor(256) {
-  mutex = MutexFactory::instance()->createMutex();
-  ReturnValue_t result = mutex->lockMutex();
-  if (result != returnvalue::OK) {
+  sdLock = MutexFactory::instance()->createMutex();
+  prefLock = MutexFactory::instance()->createMutex();
+  defaultLock = MutexFactory::instance()->createMutex();
+
+  MutexGuard mg(prefLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX);
+  if (mg.getLockResult() != 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) {
-    sif::error << "SdCardManager::SdCardManager: Mutex unlock failed" << std::endl;
-  }
+  ReturnValue_t result = scratch::readNumber(scratch::PREFERED_SDC_KEY, prefSdRaw);
 
   if (result != returnvalue::OK) {
     if (result == scratch::KEY_NOT_FOUND) {
@@ -37,14 +37,16 @@ SdCardManager::SdCardManager() : SystemObject(objects::SDC_MANAGER), cmdExecutor
                       "Preferred SD card not set. Setting to 0"
                    << std::endl;
       setPreferredSdCard(sd::SdCard::SLOT_0);
-      sdInfo.pref = sd::SdCard::SLOT_0;
+      scratch::writeNumber(scratch::PREFERED_SDC_KEY, static_cast<uint8_t>(sd::SdCard::SLOT_0));
+      prefSdRaw = sd::SdCard::SLOT_0;
+
     } else {
       // Should not happen.
       // TODO: Maybe trigger event?
       sif::error << "SdCardManager::SdCardManager: Reading preferred SD card from scratch"
                     "buffer failed"
                  << std::endl;
-      sdInfo.pref = sd::SdCard::SLOT_0;
+      prefSdRaw = sd::SdCard::SLOT_0;
     }
   }
   sdInfo.pref = static_cast<sd::SdCard>(prefSdRaw);
@@ -195,8 +197,9 @@ ReturnValue_t SdCardManager::setSdCardState(sd::SdCard sdCard, bool on) {
 
 ReturnValue_t SdCardManager::getSdCardsStatus(SdStatePair& active) {
   using namespace std;
-  MutexGuard mg(mutex);
-  if (not filesystem::exists(SD_STATE_FILE)) {
+  MutexGuard mg(sdLock, LOCK_TYPE, SD_LOCK_TIMEOUT, LOCK_CTX);
+  std::error_code e;
+  if (not filesystem::exists(SD_STATE_FILE, e)) {
     return STATUS_FILE_NEXISTS;
   }
 
@@ -237,7 +240,8 @@ ReturnValue_t SdCardManager::mountSdCard(sd::SdCard sdCard) {
     mountDev = SD_1_DEV_NAME;
     mountPoint = config::SD_1_MOUNT_POINT;
   }
-  if (not filesystem::exists(mountDev)) {
+  std::error_code e;
+  if (not filesystem::exists(mountDev, e)) {
     sif::warning << "SdCardManager::mountSdCard: Device file does not exists. Make sure to"
                     " turn on the SD card"
                  << std::endl;
@@ -272,7 +276,8 @@ ReturnValue_t SdCardManager::unmountSdCard(sd::SdCard sdCard) {
   } else if (sdCard == sd::SdCard::SLOT_1) {
     mountPoint = config::SD_1_MOUNT_POINT;
   }
-  if (not filesystem::exists(mountPoint)) {
+  std::error_code e;
+  if (not filesystem::exists(mountPoint, e)) {
     sif::error << "SdCardManager::unmountSdCard: Default mount point " << mountPoint
                << "does not exist" << std::endl;
     return UNMOUNT_ERROR;
@@ -378,7 +383,7 @@ void SdCardManager::processSdStatusLine(std::pair<sd::SdState, sd::SdState>& act
 }
 
 std::optional<sd::SdCard> SdCardManager::getPreferredSdCard() const {
-  MutexGuard mg(mutex);
+  MutexGuard mg(prefLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX);
   auto res = mg.getLockResult();
   if (res != returnvalue::OK) {
     sif::error << "SdCardManager::getPreferredSdCard: Lock error" << std::endl;
@@ -387,7 +392,7 @@ std::optional<sd::SdCard> SdCardManager::getPreferredSdCard() const {
 }
 
 ReturnValue_t SdCardManager::setPreferredSdCard(sd::SdCard sdCard) {
-  MutexGuard mg(mutex);
+  MutexGuard mg(prefLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX);
   if (sdCard == sd::SdCard::BOTH) {
     return returnvalue::FAILED;
   }
@@ -399,7 +404,7 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() {
   if (cmdExecutor.getCurrentState() == CommandExecutor::States::PENDING) {
     return CommandExecutor::COMMAND_PENDING;
   }
-  MutexGuard mg(mutex);
+  MutexGuard mg(sdLock, LOCK_TYPE, SD_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 +416,7 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() {
 }
 
 const char* SdCardManager::getCurrentMountPrefix() const {
-  MutexGuard mg(mutex);
+  MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX);
   if (currentPrefix.has_value()) {
     return currentPrefix.value().c_str();
   }
@@ -464,7 +469,7 @@ void SdCardManager::setPrintCommandOutput(bool print) { this->printCmdOutput = p
 
 bool SdCardManager::isSdCardUsable(std::optional<sd::SdCard> sdCard) {
   {
-    MutexGuard mg(mutex);
+    MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX);
     if (markedUnusable) {
       return false;
     }
@@ -508,9 +513,9 @@ bool SdCardManager::isSdCardUsable(std::optional<sd::SdCard> sdCard) {
 ReturnValue_t SdCardManager::isSdCardMountedReadOnly(sd::SdCard sdcard, bool& readOnly) {
   std::ostringstream command;
   if (sdcard == sd::SdCard::SLOT_0) {
-    command << "grep -q '" << config::SD_0_MOUNT_POINT << " ext4 ro,' /proc/mounts";
+    command << "grep -q '" << config::SD_0_MOUNT_POINT << " ext4 rw,' /proc/mounts";
   } else if (sdcard == sd::SdCard::SLOT_1) {
-    command << "grep -q '" << config::SD_1_MOUNT_POINT << " ext4 ro,' /proc/mounts";
+    command << "grep -q '" << config::SD_1_MOUNT_POINT << " ext4 rw,' /proc/mounts";
   } else {
     return returnvalue::FAILED;
   }
@@ -519,18 +524,9 @@ ReturnValue_t SdCardManager::isSdCardMountedReadOnly(sd::SdCard sdcard, bool& re
     return result;
   }
   result = cmdExecutor.execute();
-  if (result != returnvalue::OK) {
-    int exitStatus = cmdExecutor.getLastError();
-    if (exitStatus == 1) {
-      readOnly = false;
-      return returnvalue::OK;
-    }
-    return result;
-  }
-  auto& readVec = cmdExecutor.getReadVector();
-  size_t readLen = strnlen(readVec.data(), readVec.size());
-  if (readLen == 0) {
+  if (result == returnvalue::OK) {
     readOnly = false;
+    return result;
   }
   readOnly = true;
   return returnvalue::OK;
@@ -569,7 +565,7 @@ ReturnValue_t SdCardManager::performFsck(sd::SdCard sdcard, bool printOutput, in
 }
 
 void SdCardManager::setActiveSdCard(sd::SdCard sdCard) {
-  MutexGuard mg(mutex);
+  MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX);
   sdInfo.active = sdCard;
   if (sdInfo.active == sd::SdCard::SLOT_0) {
     currentPrefix = config::SD_0_MOUNT_POINT;
@@ -579,7 +575,7 @@ void SdCardManager::setActiveSdCard(sd::SdCard sdCard) {
 }
 
 std::optional<sd::SdCard> SdCardManager::getActiveSdCard() const {
-  MutexGuard mg(mutex);
+  MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX);
   if (markedUnusable) {
     return std::nullopt;
   }
@@ -587,6 +583,6 @@ std::optional<sd::SdCard> SdCardManager::getActiveSdCard() const {
 }
 
 void SdCardManager::markUnusable() {
-  MutexGuard mg(mutex);
+  MutexGuard mg(defaultLock, LOCK_TYPE, OTHER_TIMEOUT, LOCK_CTX);
   markedUnusable = true;
 }

bsp_q7s/fs/SdCardManager.h

@@ -223,7 +223,13 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
   bool sdCardActive = true;
   bool printCmdOutput = true;
   bool markedUnusable = false;
-  MutexIF* mutex = nullptr;
+  MutexIF* sdLock = nullptr;
+  MutexIF* prefLock = nullptr;
+  MutexIF* defaultLock = nullptr;
+  static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
+  static constexpr uint32_t SD_LOCK_TIMEOUT = 250;
+  static constexpr uint32_t OTHER_TIMEOUT = 20;
+  static constexpr char LOCK_CTX[] = "SdCardManager";
 
   SdCardManager();
 

bsp_q7s/main.cpp

@@ -12,10 +12,10 @@
  * @brief 	This is the main program for the target hardware.
  * @return
  */
-int main(void) {
+int main(int argc, char* argv[]) {
   using namespace std;
 #if Q7S_SIMPLE_MODE == 0
-  return obsw::obsw();
+  return obsw::obsw(argc, argv);
 #else
   return simple::simple();
 #endif

bsp_q7s/memory/CMakeLists.txt

@@ -1 +1 @@
-target_sources(${OBSW_NAME} PRIVATE scratchApi.cpp)
+target_sources(${OBSW_NAME} PRIVATE scratchApi.cpp LocalParameterHandler.cpp)

bsp_q7s/memory/LocalParameterHandler.cpp

@@ -0,0 +1,41 @@
+#include "LocalParameterHandler.h"
+
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+LocalParameterHandler::LocalParameterHandler(std::string sdRelativeName, SdCardMountedIF* sdcMan)
+    : NVMParameterBase(), sdRelativeName(sdRelativeName), sdcMan(sdcMan) {}
+
+LocalParameterHandler::~LocalParameterHandler() {}
+
+ReturnValue_t LocalParameterHandler::initialize() {
+  ReturnValue_t result = updateFullName();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = readJsonFile();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t LocalParameterHandler::writeJsonFile() {
+  ReturnValue_t result = updateFullName();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return NVMParameterBase::writeJsonFile();
+}
+
+ReturnValue_t LocalParameterHandler::updateFullName() {
+  std::string mountPrefix;
+  auto activeSd = sdcMan->getActiveSdCard();
+  if (activeSd and sdcMan->isSdCardUsable(activeSd.value())) {
+    mountPrefix = sdcMan->getCurrentMountPrefix();
+  } else {
+    return SD_NOT_READY;
+  }
+  std::string fullname = mountPrefix + "/" + sdRelativeName;
+  NVMParameterBase::setFullName(fullname);
+  return returnvalue::OK;
+}

bsp_q7s/memory/LocalParameterHandler.h

@@ -0,0 +1,106 @@
+#ifndef BSP_Q7S_MEMORY_LOCALPARAMETERHANDLER_H_
+#define BSP_Q7S_MEMORY_LOCALPARAMETERHANDLER_H_
+
+#include <mission/memory/NvmParameterBase.h>
+#include <mission/memory/SdCardMountedIF.h>
+
+#include <string>
+
+/**
+ * @brief	Class to handle persistent parameters
+ *
+ */
+class LocalParameterHandler : public NVMParameterBase {
+ public:
+  static constexpr uint8_t INTERFACE_ID = CLASS_ID::LOCAL_PARAM_HANDLER;
+
+  static constexpr ReturnValue_t SD_NOT_READY = returnvalue::makeCode(INTERFACE_ID, 0);
+  /**
+   * @brief	Constructor
+   *
+   * @param sdRelativeName	Absolute name of json file relative to mount
+   * 												directory
+   * of SD card. E.g. conf/example.json
+   * @param sdcMan	Pointer to SD card manager
+   */
+  LocalParameterHandler(std::string sdRelativeName, SdCardMountedIF* sdcMan);
+  virtual ~LocalParameterHandler();
+
+  /**
+   * @brief Will initialize the local parameter handler
+   *
+   * @return OK if successful, otherwise error return value
+   */
+  ReturnValue_t initialize();
+
+  /**
+   * @brief Function to add parameter to json file. If the json file does
+   * 				not yet exist it will be created here.
+   *
+   * @param key	The string to identify the parameter
+   * @param value	The value to set for this parameter
+   *
+   * @return OK if successful, otherwise error return value
+   *
+   * @details The function will add the parameter only if it is not already
+   * 					present in the json file
+   */
+  template <typename T>
+  ReturnValue_t addParameter(std::string key, T value);
+
+  /**
+   * @brief	Function will update a parameter which already exists in the json
+   * 				file
+   *
+   * @param key 	The unique string to identify the parameter to update
+   * @param value The new new value to set
+   *
+   * @return OK if successful, otherwise error return value
+   */
+  template <typename T>
+  ReturnValue_t updateParameter(std::string key, T value);
+
+ private:
+  // Name relative to mount point of SD card where parameters will be stored
+  std::string sdRelativeName;
+
+  SdCardMountedIF* sdcMan;
+
+  virtual ReturnValue_t writeJsonFile();
+
+  /**
+   * @brief This function sets the name of the json file dependent on the
+   * 				currently active SD card
+   *
+   * @return OK if successful, otherwise error return value
+   */
+  ReturnValue_t updateFullName();
+};
+
+template <typename T>
+inline ReturnValue_t LocalParameterHandler::addParameter(std::string key, T value) {
+  ReturnValue_t result = insertValue(key, value);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = writeJsonFile();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return returnvalue::OK;
+}
+
+template <typename T>
+inline ReturnValue_t LocalParameterHandler::updateParameter(std::string key, T value) {
+  ReturnValue_t result = setValue(key, value);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = writeJsonFile();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return returnvalue::OK;
+}
+
+#endif /* BSP_Q7S_MEMORY_LOCALPARAMETERHANDLER_H_ */

bsp_q7s/obsw.cpp

@@ -9,6 +9,7 @@
 #include <iostream>
 
 #include "OBSWConfig.h"
+#include "bsp_q7s/core/WatchdogHandler.h"
 #include "commonConfig.h"
 #include "core/scheduling.h"
 #include "fsfw/tasks/TaskFactory.h"
@@ -18,13 +19,16 @@
 #include "q7sConfig.h"
 #include "watchdog/definitions.h"
 
-static int OBSW_ALREADY_RUNNING = -2;
+static constexpr int OBSW_ALREADY_RUNNING = -2;
 #if OBSW_Q7S_EM == 0
 static const char* DEV_STRING = "Xiphos Q7S FM";
 #else
 static const char* DEV_STRING = "Xiphos Q7S EM";
 #endif
-int obsw::obsw() {
+
+WatchdogHandler WATCHDOG_HANDLER;
+
+int obsw::obsw(int argc, char* argv[]) {
   using namespace fsfw;
   std::cout << "-- EIVE OBSW --" << std::endl;
   std::cout << "-- Compiled for Linux (" << DEV_STRING << ") --" << std::endl;
@@ -33,9 +37,10 @@ int obsw::obsw() {
   std::cout << "-- " << __DATE__ << " " << __TIME__ << " --" << std::endl;
 
 #if Q7S_CHECK_FOR_ALREADY_RUNNING_IMG == 1
+  std::error_code e;
   // Check special file here. This file is created or deleted by the eive-watchdog application
   // or systemd service!
-  if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME)) {
+  if (std::filesystem::exists(watchdog::RUNNING_FILE_NAME, e)) {
     sif::warning << "File " << watchdog::RUNNING_FILE_NAME
                  << " exists so the software might "
                     "already be running. Check if obsw systemd service has been stopped."
@@ -44,14 +49,45 @@ int obsw::obsw() {
   }
 #endif
 
+  // Delay the boot if applicable.
+  bootDelayHandling();
+
+  bool initWatchFunction = false;
+  std::string fullExecPath = argv[0];
+  if (fullExecPath.find("/usr/bin") != std::string::npos) {
+    initWatchFunction = true;
+  }
+  ReturnValue_t result = WATCHDOG_HANDLER.initialize(initWatchFunction);
+  if (result != returnvalue::OK) {
+    std::cerr << "Initiating EIVE watchdog handler failed" << std::endl;
+  }
+
+  scheduling::initMission();
+
+  // Command the EIVE system to safe mode
+#if OBSW_COMMAND_SAFE_MODE_AT_STARTUP == 1
+  commandEiveSystemToSafe();
+#else
+  announceAllModes();
+#endif
+
+  for (;;) {
+    WATCHDOG_HANDLER.periodicOperation();
+    TaskFactory::delayTask(2000);
+  }
+  return 0;
+}
+
+void obsw::bootDelayHandling() {
   const char* homedir = nullptr;
   homedir = getenv("HOME");
   if (homedir == nullptr) {
     homedir = getpwuid(getuid())->pw_dir;
   }
   std::filesystem::path bootDelayFile = std::filesystem::path(homedir) / "boot_delay_secs.txt";
+  std::error_code e;
   // Init delay handling.
-  if (std::filesystem::exists(bootDelayFile)) {
+  if (std::filesystem::exists(bootDelayFile, e)) {
     std::ifstream ifile(bootDelayFile);
     std::string lineStr;
     unsigned int bootDelaySecs = 0;
@@ -71,31 +107,26 @@ int obsw::obsw() {
     std::cout << "Delaying OBSW start for " << bootDelaySecs << " seconds" << std::endl;
     TaskFactory::delayTask(bootDelaySecs * 1000);
   }
+}
 
-  scheduling::initMission();
-
-  // Command the EIVE system to safe mode
+void obsw::commandEiveSystemToSafe() {
   auto sysQueueId = satsystem::EIVE_SYSTEM.getCommandQueue();
   CommandMessage msg;
-#if OBSW_COMMAND_SAFE_MODE_AT_STARTUP == 1
   ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
   ReturnValue_t result =
       MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
   if (result != returnvalue::OK) {
     sif::error << "Sending safe mode command to EIVE system failed" << std::endl;
   }
-#else
+}
+
+void obsw::announceAllModes() {
+  auto sysQueueId = satsystem::EIVE_SYSTEM.getCommandQueue();
+  CommandMessage msg;
   ModeMessage::setModeAnnounceMessage(msg, true);
   ReturnValue_t result =
       MessageQueueSenderIF::sendMessage(sysQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
   if (result != returnvalue::OK) {
     sif::error << "Sending safe mode command to EIVE system failed" << std::endl;
   }
-#endif
-
-  for (;;) {
-    /* Suspend main thread by sleeping it. */
-    TaskFactory::delayTask(5000);
-  }
-  return 0;
 }

bsp_q7s/obsw.h

@@ -3,8 +3,12 @@
 
 namespace obsw {
 
-int obsw();
+int obsw(int argc, char* argv[]);
 
-};
+void bootDelayHandling();
+void commandEiveSystemToSafe();
+void announceAllModes();
+
+};  // namespace obsw
 
 #endif /* BSP_Q7S_CORE_OBSW_H_ */

common/config/eive/definitions.h

@@ -55,16 +55,18 @@ static constexpr uint32_t MAX_PUS_FUNNEL_QUEUE_DEPTH = 100;
 static constexpr uint32_t MAX_STORED_CMDS_UDP = 120;
 static constexpr uint32_t MAX_STORED_CMDS_TCP = 150;
 
-namespace acs {
+namespace spiSched {
 
 static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15;
 static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30;
-static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42;
+static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 43;
 static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45;
 static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55;
-static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 95;
+static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 105;
 static constexpr uint32_t SCHED_BLOCK_RTD = 150;
 static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300;
+static constexpr uint32_t SCHED_BLOCK_8_PLPCDU_MS = 320;
+static constexpr uint32_t SCHED_BLOCK_9_RAD_SENS_MS = 340;
 
 // 15 ms for FM
 static constexpr float SCHED_BLOCK_1_PERIOD = static_cast<float>(SCHED_BLOCK_1_SUS_READ_MS) / 400.0;
@@ -76,10 +78,12 @@ static constexpr float SCHED_BLOCK_4_PERIOD = static_cast<float>(SCHED_BLOCK_4_A
 static constexpr float SCHED_BLOCK_5_PERIOD = static_cast<float>(SCHED_BLOCK_5_ACTUATOR_MS) / 400.0;
 static constexpr float SCHED_BLOCK_6_PERIOD =
     static_cast<float>(SCHED_BLOCK_6_IMTQ_BLOCK_2_MS) / 400.0;
-static constexpr float SCHED_BLOCK_7_PERIOD = static_cast<float>(SCHED_BLOCK_7_RW_READ_MS) / 400.0;
 static constexpr float SCHED_BLOCK_RTD_PERIOD = static_cast<float>(SCHED_BLOCK_RTD) / 400.0;
+static constexpr float SCHED_BLOCK_7_PERIOD = static_cast<float>(SCHED_BLOCK_7_RW_READ_MS) / 400.0;
+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
 

common/config/eive/eventSubsystemIds.h

@@ -36,8 +36,9 @@ enum : uint8_t {
   SCEX_HANDLER = 138,
   CONFIGHANDLER = 139,
   CORE = 140,
-  PERSISTENT_TM_STORE = 141,
-  TCS_CONTROLLER = 142,
+  TCS_CONTROLLER = 141,
+  COM_SUBSYSTEM = 142,
+  PERSISTENT_TM_STORE = 143,
   COMMON_SUBSYSTEM_ID_END
 
 };

common/config/eive/objects.h

@@ -123,7 +123,10 @@ enum commonObjects : uint32_t {
   // CCSDS_IP_CORE_BRIDGE = 0x73500000,
 
   /* 0x49 ('I') for Communication Interfaces */
-  SPI_RTD_COM_IF = 0x49020006,
+  ACS_BOARD_POLLING_TASK = 0x49060004,
+  RW_POLLING_TASK = 0x49060005,
+  SPI_RTD_COM_IF = 0x49060006,
+  SUS_POLLING_TASK = 0x49060007,
 
   // 0x60 for other stuff
   HEATER_0_PLOC_PROC_BRD = 0x60000000,
@@ -139,8 +142,11 @@ 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,
+  IMTQ_ASSY = 0x73000008,
+  STR_ASSY = 0x73000009,
   EIVE_SYSTEM = 0x73010000,
   ACS_SUBSYSTEM = 0x73010001,
   PL_SUBSYSTEM = 0x73010002,

common/config/eive/resultClassIds.h

@@ -40,6 +40,8 @@ enum commonClassIds : uint8_t {
   ACS_SAFE,                 // ACSSAF
   ACS_PTG,                  // ACSPTG
   ACS_DETUMBLE,             // ACSDTB
+  SD_CARD_MANAGER,          // SDMA
+  LOCAL_PARAM_HANDLER,      // LPH
   COMMON_CLASS_ID_END       // [EXPORT] : [END]
 };
 }

dummies/CoreControllerDummy.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <fsfw/controller/ExtendedControllerBase.h>
-#include <mission/devices/devicedefinitions/SusDefinitions.h>
+#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
 
 class CoreControllerDummy : public ExtendedControllerBase {
  public:

dummies/GyroAdisDummy.cpp

@@ -1,6 +1,6 @@
 #include "GyroAdisDummy.h"
 
-#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 
 GyroAdisDummy::GyroAdisDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {}
@@ -40,13 +40,13 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r
 
 ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({-0.5}, true));
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.2}, true));
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({-1.2}, true));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry<double>({-0.5}, true));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry<double>({0.2}, true));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry<double>({-1.2}, true));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry<float>({0.0}));
 
   return returnvalue::OK;
 }

dummies/GyroAdisDummy.h

@@ -2,8 +2,7 @@
 #define DUMMIES_GYROADISDUMMY_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-
-#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 
 class GyroAdisDummy : public DeviceHandlerBase {
  public:

dummies/GyroL3GD20Dummy.cpp

@@ -1,6 +1,6 @@
 #include "GyroL3GD20Dummy.h"
 
-#include "fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h"
+#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
 
 GyroL3GD20Dummy::GyroL3GD20Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie) {}
@@ -40,9 +40,9 @@ uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
 
 ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                        LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry<float>({-0.1}, true));
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry<float>({0.7}, true));
-  localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({-0.1}, true));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.7}, true));
+  localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
   return returnvalue::OK;
 }

dummies/MgmLIS3MDLDummy.cpp

@@ -1,6 +1,6 @@
 #include "MgmLIS3MDLDummy.h"
 
-#include "fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h"
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
 
 MgmLIS3MDLDummy::MgmLIS3MDLDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {}
@@ -40,8 +40,8 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
 
 ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                        LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS,
+  localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS,
                            new PoolEntry<float>({1.02, 0.56, -0.78}, true));
   return returnvalue::OK;
 }

dummies/MgmLIS3MDLDummy.h

@@ -2,8 +2,7 @@
 #define DUMMIES_MGMLIS3MDLDUMMY_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-
-#include "fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h"
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
 
 class MgmLIS3MDLDummy : public DeviceHandlerBase {
  public:
@@ -17,7 +16,7 @@ class MgmLIS3MDLDummy : public DeviceHandlerBase {
   virtual ~MgmLIS3MDLDummy();
 
  protected:
-  MGMLIS3MDL::MgmPrimaryDataset dataset;
+  mgmLis3::MgmPrimaryDataset dataset;
   void doStartUp() override;
   void doShutDown() override;
   ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;

dummies/MgmRm3100Dummy.cpp

@@ -36,7 +36,7 @@ uint32_t MgmRm3100Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
 
 ReturnValue_t MgmRm3100Dummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                       LocalDataPoolManager& poolManager) {
-  localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS,
+  localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS,
                            new PoolEntry<float>({0.87, -0.95, 0.11}, true));
   return returnvalue::OK;
 }

dummies/MgmRm3100Dummy.h

@@ -1,8 +1,9 @@
 #ifndef DUMMIES_MGMRM3100DUMMY_H_
 #define DUMMIES_MGMRM3100DUMMY_H_
 
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h>
+
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
-#include "fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h"
 
 class MgmRm3100Dummy : public DeviceHandlerBase {
  public:
@@ -10,7 +11,7 @@ class MgmRm3100Dummy : public DeviceHandlerBase {
   virtual ~MgmRm3100Dummy();
 
  protected:
-  RM3100::Rm3100PrimaryDataset dataset;
+  mgmRm3100::Rm3100PrimaryDataset dataset;
   void doStartUp() override;
   void doShutDown() override;
   ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;

dummies/SusDummy.cpp

@@ -35,8 +35,9 @@ uint32_t SusDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return
 
 ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                 LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
-  localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
+  localDataPoolMap.emplace(susMax1227::SusPoolIds::TEMPERATURE_C,
+                           new PoolEntry<float>({0}, 1, true));
+  localDataPoolMap.emplace(susMax1227::SusPoolIds::CHANNEL_VEC,
                            new PoolEntry<uint16_t>({2603, 781, 2760, 2048, 4056, 0}, true));
 
   return returnvalue::OK;

dummies/SusDummy.h

@@ -2,8 +2,7 @@
 #define DUMMIES_SUSDUMMY_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-
-#include "mission/devices/devicedefinitions/SusDefinitions.h"
+#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
 
 class SusDummy : public DeviceHandlerBase {
  public:
@@ -17,7 +16,7 @@ class SusDummy : public DeviceHandlerBase {
   virtual ~SusDummy();
 
  protected:
-  SUS::SusDataset susSet;
+  susMax1227::SusDataset susSet;
   void doStartUp() override;
   void doShutDown() override;
   ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;

generators/bsp_hosted_events.csv

@@ -151,7 +151,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 12406;0x3076;LOST_BIT_LOCK_PDEC;INFO;Lost bit lock;linux/ipcore/PdecHandler.h
 12407;0x3077;TOO_MANY_IRQS;MEDIUM;Too many IRQs over the time window of one second. P1: Allowed TCs;linux/ipcore/PdecHandler.h
 12408;0x3078;POLL_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h
-12409;0x3079;WRITE_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h
+12409;0x3079;WRITE_SYSCALL_ERROR_PDEC;HIGH;No description;linux/ipcore/PdecHandler.h
+12410;0x307a;PDEC_RESET_FAILED;HIGH;Failed to pull PDEC reset to low;linux/ipcore/PdecHandler.h
+12411;0x307b;OPEN_IRQ_FILE_FAILED;HIGH;Failed to open the IRQ uio file;linux/ipcore/PdecHandler.h
 12500;0x30d4;IMAGE_UPLOAD_FAILED;LOW;Image upload failed;linux/devices/startracker/StrHelper.h
 12501;0x30d5;IMAGE_DOWNLOAD_FAILED;LOW;Image download failed;linux/devices/startracker/StrHelper.h
 12502;0x30d6;IMAGE_UPLOAD_SUCCESSFUL;LOW;Uploading image to star tracker was successfulop;linux/devices/startracker/StrHelper.h
@@ -248,17 +250,21 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 13904;0x3650;WRITE_CONFIGFILE_FAILED;MEDIUM;No description;mission/utility/GlobalConfigHandler.h
 13905;0x3651;READ_CONFIGFILE_FAILED;MEDIUM;No description;mission/utility/GlobalConfigHandler.h
 14000;0x36b0;ALLOC_FAILURE;MEDIUM;No description;bsp_q7s/core/CoreController.h
-14001;0x36b1;REBOOT_SW;MEDIUM; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h
+14001;0x36b1;REBOOT_SW;LOW; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h
 14002;0x36b2;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h
 14003;0x36b3;REBOOT_HW;MEDIUM;No description;bsp_q7s/core/CoreController.h
 14004;0x36b4;NO_SD_CARD_ACTIVE;HIGH;No SD card was active. Core controller will attempt to re-initialize a SD card.;bsp_q7s/core/CoreController.h
 14005;0x36b5;VERSION_INFO;INFO;P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;bsp_q7s/core/CoreController.h
 14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h
-14100;0x3714;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/tmtc/PersistentTmStore.h
-14200;0x3778;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/ThermalController.h
-14201;0x3779;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/ThermalController.h
-14202;0x377a;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
-14203;0x377b;PLOC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
-14204;0x377c;OBC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
-14205;0x377d;HPA_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
-14206;0x377e;PLPCDU_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14007;0x36b7;REBOOT_COUNTER;INFO;Total reboot counter, which is the sum of the boot count of all individual images.;bsp_q7s/core/CoreController.h
+14008;0x36b8;INDIVIDUAL_BOOT_COUNTS;INFO;Get the boot count of the individual images. P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1. P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1.;bsp_q7s/core/CoreController.h
+14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/ThermalController.h
+14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/ThermalController.h
+14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14103;0x3717;PLOC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14104;0x3718;OBC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14105;0x3719;HPA_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14106;0x371a;PLPCDU_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/objects/ComSubsystem.h
+14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/objects/ComSubsystem.h
+14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/tmtc/PersistentTmStore.h

generators/bsp_hosted_objects.csv

@@ -77,8 +77,11 @@
 0x49000001;ARDUINO_COM_IF
 0x49000002;DUMMY_COM_IF
 0x49010006;SCEX_UART_READER
-0x49020006;SPI_RTD_COM_IF
 0x49030003;UART_COM_IF
+0x49060004;ACS_BOARD_POLLING_TASK
+0x49060005;RW_POLLING_TASK
+0x49060006;SPI_RTD_COM_IF
+0x49060007;SUS_POLLING_TASK
 0x50000100;CCSDS_PACKET_DISTRIBUTOR
 0x50000200;PUS_PACKET_DISTRIBUTOR
 0x50000300;TCP_TMTC_SERVER
@@ -131,8 +134,11 @@
 0x73000001;ACS_BOARD_ASS
 0x73000002;SUS_BOARD_ASS
 0x73000003;TCS_BOARD_ASS
-0x73000004;RW_ASS
+0x73000004;RW_ASSY
 0x73000006;CAM_SWITCHER
+0x73000007;SYRLINKS_ASSY
+0x73000008;IMTQ_ASSY
+0x73000009;STR_ASSY
 0x73000100;TM_FUNNEL
 0x73000101;PUS_TM_FUNNEL
 0x73000102;CFDP_TM_FUNNEL

generators/bsp_hosted_returnvalues.csv

@@ -1,7 +1,7 @@
 Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
-0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h
+0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NvmParameterBase.h
 0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
 0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
 0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
@@ -20,8 +20,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x52b5;RWHA_NoReply;Reaction wheel only responds with empty frames.;181;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b6;RWHA_NoStartMarker;Expected a start marker as first byte;182;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b7;RWHA_SpiReadTimeout;Timeout when reading reply;183;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
-0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/SusHandler.h
-0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/SusHandler.h
+0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/LegacySusHandler.h
+0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/LegacySusHandler.h
 0x66a0;SADPL_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SA_DEPL_HANDLER;mission/devices/RwHandler.h
 0x66a1;SADPL_InvalidRampTime;Action Message with invalid ramp time was received.;161;SA_DEPL_HANDLER;mission/devices/RwHandler.h
 0x66a2;SADPL_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SA_DEPL_HANDLER;mission/devices/RwHandler.h

generators/bsp_q7s_events.csv

@@ -151,7 +151,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 12406;0x3076;LOST_BIT_LOCK_PDEC;INFO;Lost bit lock;linux/ipcore/PdecHandler.h
 12407;0x3077;TOO_MANY_IRQS;MEDIUM;Too many IRQs over the time window of one second. P1: Allowed TCs;linux/ipcore/PdecHandler.h
 12408;0x3078;POLL_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h
-12409;0x3079;WRITE_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h
+12409;0x3079;WRITE_SYSCALL_ERROR_PDEC;HIGH;No description;linux/ipcore/PdecHandler.h
+12410;0x307a;PDEC_RESET_FAILED;HIGH;Failed to pull PDEC reset to low;linux/ipcore/PdecHandler.h
+12411;0x307b;OPEN_IRQ_FILE_FAILED;HIGH;Failed to open the IRQ uio file;linux/ipcore/PdecHandler.h
 12500;0x30d4;IMAGE_UPLOAD_FAILED;LOW;Image upload failed;linux/devices/startracker/StrHelper.h
 12501;0x30d5;IMAGE_DOWNLOAD_FAILED;LOW;Image download failed;linux/devices/startracker/StrHelper.h
 12502;0x30d6;IMAGE_UPLOAD_SUCCESSFUL;LOW;Uploading image to star tracker was successfulop;linux/devices/startracker/StrHelper.h
@@ -248,17 +250,21 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 13904;0x3650;WRITE_CONFIGFILE_FAILED;MEDIUM;No description;mission/utility/GlobalConfigHandler.h
 13905;0x3651;READ_CONFIGFILE_FAILED;MEDIUM;No description;mission/utility/GlobalConfigHandler.h
 14000;0x36b0;ALLOC_FAILURE;MEDIUM;No description;bsp_q7s/core/CoreController.h
-14001;0x36b1;REBOOT_SW;MEDIUM; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h
+14001;0x36b1;REBOOT_SW;LOW; Software reboot occurred. Can also be a systemd reboot. P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h
 14002;0x36b2;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h
 14003;0x36b3;REBOOT_HW;MEDIUM;No description;bsp_q7s/core/CoreController.h
 14004;0x36b4;NO_SD_CARD_ACTIVE;HIGH;No SD card was active. Core controller will attempt to re-initialize a SD card.;bsp_q7s/core/CoreController.h
 14005;0x36b5;VERSION_INFO;INFO;P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;bsp_q7s/core/CoreController.h
 14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h
-14100;0x3714;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/tmtc/PersistentTmStore.h
-14200;0x3778;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/ThermalController.h
-14201;0x3779;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/ThermalController.h
-14202;0x377a;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
-14203;0x377b;PLOC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
-14204;0x377c;OBC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
-14205;0x377d;HPA_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
-14206;0x377e;PLPCDU_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14007;0x36b7;REBOOT_COUNTER;INFO;Total reboot counter, which is the sum of the boot count of all individual images.;bsp_q7s/core/CoreController.h
+14008;0x36b8;INDIVIDUAL_BOOT_COUNTS;INFO;Get the boot count of the individual images. P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1. P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1.;bsp_q7s/core/CoreController.h
+14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/ThermalController.h
+14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/ThermalController.h
+14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14103;0x3717;PLOC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14104;0x3718;OBC_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14105;0x3719;HPA_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14106;0x371a;PLPCDU_OVERHEATING;HIGH;No description;mission/controller/ThermalController.h
+14201;0x3779;TX_TIMER_EXPIRED;INFO;The transmit timer to protect the Syrlinks expired P1: The current timer value;mission/system/objects/ComSubsystem.h
+14202;0x377a;BIT_LOCK_TX_ON;INFO;Transmitter will be turned on due to detection of bitlock;mission/system/objects/ComSubsystem.h
+14300;0x37dc;POSSIBLE_FILE_CORRUPTION;LOW;P1: Result code of TM packet parser. P2: Timestamp of possibly corrupt file as a unix timestamp.;mission/tmtc/PersistentTmStore.h

generators/bsp_q7s_objects.csv

@@ -77,11 +77,13 @@
 0x49010005;GPIO_IF
 0x49010006;SCEX_UART_READER
 0x49020004;SPI_MAIN_COM_IF
-0x49020005;RW_POLLING_TASK
-0x49020006;SPI_RTD_COM_IF
 0x49030003;UART_COM_IF
 0x49040002;I2C_COM_IF
 0x49050001;CSP_COM_IF
+0x49060004;ACS_BOARD_POLLING_TASK
+0x49060005;RW_POLLING_TASK
+0x49060006;SPI_RTD_COM_IF
+0x49060007;SUS_POLLING_TASK
 0x50000100;CCSDS_PACKET_DISTRIBUTOR
 0x50000200;PUS_PACKET_DISTRIBUTOR
 0x50000300;TCP_TMTC_SERVER
@@ -137,8 +139,11 @@
 0x73000001;ACS_BOARD_ASS
 0x73000002;SUS_BOARD_ASS
 0x73000003;TCS_BOARD_ASS
-0x73000004;RW_ASS
+0x73000004;RW_ASSY
 0x73000006;CAM_SWITCHER
+0x73000007;SYRLINKS_ASSY
+0x73000008;IMTQ_ASSY
+0x73000009;STR_ASSY
 0x73000100;TM_FUNNEL
 0x73000101;PUS_TM_FUNNEL
 0x73000102;CFDP_TM_FUNNEL

generators/bsp_q7s_returnvalues.csv

@@ -1,7 +1,7 @@
 Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
-0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h
+0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NvmParameterBase.h
 0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
 0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
 0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
@@ -20,8 +20,8 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x52b5;RWHA_NoReply;Reaction wheel only responds with empty frames.;181;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b6;RWHA_NoStartMarker;Expected a start marker as first byte;182;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b7;RWHA_SpiReadTimeout;Timeout when reading reply;183;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
-0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/SusHandler.h
-0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/SusHandler.h
+0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/LegacySusHandler.h
+0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/LegacySusHandler.h
 0x66a0;SADPL_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SA_DEPL_HANDLER;mission/devices/RwHandler.h
 0x66a1;SADPL_InvalidRampTime;Action Message with invalid ramp time was received.;161;SA_DEPL_HANDLER;mission/devices/RwHandler.h
 0x66a2;SADPL_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SA_DEPL_HANDLER;mission/devices/RwHandler.h
@@ -474,19 +474,20 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x1d03;ATC_IllegalApplicationData;No description;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
 0x1d04;ATC_SendTmFailed;No description;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
 0x1d05;ATC_Timeout;No description;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
-0x7000;SCBU_KeyNotFound;No description;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h
+0x7100;SCBU_KeyNotFound;No description;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h
+0x6f00;LPH_SdNotReady;No description;0;LOCAL_PARAM_HANDLER;bsp_q7s/memory/LocalParameterHandler.h
 0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h
 0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h
-0x6f00;SDMA_OpOngoing;No description;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f01;SDMA_AlreadyOn;No description;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f02;SDMA_AlreadyMounted;No description;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f03;SDMA_AlreadyOff;No description;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f0a;SDMA_StatusFileNexists;No description;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f0b;SDMA_StatusFileFormatInvalid;No description;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f0c;SDMA_MountError;No description;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f0d;SDMA_UnmountError;No description;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f0e;SDMA_SystemCallError;No description;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6f0f;SDMA_PopenCallError;No description;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e00;SDMA_OpOngoing;No description;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e01;SDMA_AlreadyOn;No description;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e02;SDMA_AlreadyMounted;No description;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e03;SDMA_AlreadyOff;No description;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e0a;SDMA_StatusFileNexists;No description;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e0b;SDMA_StatusFileFormatInvalid;No description;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e0c;SDMA_MountError;No description;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e0d;SDMA_UnmountError;No description;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e0e;SDMA_SystemCallError;No description;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6e0f;SDMA_PopenCallError;No description;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
 0x65a0;PLMPHLP_FileClosedAccidentally;File accidentally close;160;PLOC_MPSOC_HELPER;linux/devices/ploc/PlocMPSoCHelper.h
 0x5ea0;PLMEMDUMP_MramAddressTooHigh;The capacity of the MRAM amounts to 512 kB. Thus the maximum address must not be higher than 0x7d000.;160;PLOC_MEMORY_DUMPER;linux/devices/ploc/PlocMemoryDumper.h
 0x5ea1;PLMEMDUMP_MramInvalidAddressCombination;The specified end address is lower than the start address;161;PLOC_MEMORY_DUMPER;linux/devices/ploc/PlocMemoryDumper.h

generators/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 263 translations.
+ * @brief    Auto-generated event translation file. Contains 269 translations.
  * @details
- * Generated on: 2023-02-24 16:57:00
+ * Generated on: 2023-03-08 16:44:32
  */
 #include "translateEvents.h"
 
@@ -158,6 +158,8 @@ const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC";
 const char *TOO_MANY_IRQS_STRING = "TOO_MANY_IRQS";
 const char *POLL_SYSCALL_ERROR_PDEC_STRING = "POLL_SYSCALL_ERROR_PDEC";
 const char *WRITE_SYSCALL_ERROR_PDEC_STRING = "WRITE_SYSCALL_ERROR_PDEC";
+const char *PDEC_RESET_FAILED_STRING = "PDEC_RESET_FAILED";
+const char *OPEN_IRQ_FILE_FAILED_STRING = "OPEN_IRQ_FILE_FAILED";
 const char *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED";
 const char *IMAGE_DOWNLOAD_FAILED_STRING = "IMAGE_DOWNLOAD_FAILED";
 const char *IMAGE_UPLOAD_SUCCESSFUL_STRING = "IMAGE_UPLOAD_SUCCESSFUL";
@@ -255,7 +257,8 @@ const char *REBOOT_HW_STRING = "REBOOT_HW";
 const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE";
 const char *VERSION_INFO_STRING = "VERSION_INFO";
 const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO";
-const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION";
+const char *REBOOT_COUNTER_STRING = "REBOOT_COUNTER";
+const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS";
 const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE";
 const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE";
 const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING";
@@ -263,6 +266,9 @@ const char *PLOC_OVERHEATING_STRING = "PLOC_OVERHEATING";
 const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING";
 const char *HPA_OVERHEATING_STRING = "HPA_OVERHEATING";
 const char *PLPCDU_OVERHEATING_STRING = "PLPCDU_OVERHEATING";
+const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED";
+const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON";
+const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION";
 
 const char *translateEvents(Event event) {
   switch ((event & 0xFFFF)) {
@@ -572,6 +578,10 @@ const char *translateEvents(Event event) {
       return POLL_SYSCALL_ERROR_PDEC_STRING;
     case (12409):
       return WRITE_SYSCALL_ERROR_PDEC_STRING;
+    case (12410):
+      return PDEC_RESET_FAILED_STRING;
+    case (12411):
+      return OPEN_IRQ_FILE_FAILED_STRING;
     case (12500):
       return IMAGE_UPLOAD_FAILED_STRING;
     case (12501):
@@ -766,22 +776,30 @@ const char *translateEvents(Event event) {
       return VERSION_INFO_STRING;
     case (14006):
       return CURRENT_IMAGE_INFO_STRING;
+    case (14007):
+      return REBOOT_COUNTER_STRING;
+    case (14008):
+      return INDIVIDUAL_BOOT_COUNTS_STRING;
     case (14100):
-      return POSSIBLE_FILE_CORRUPTION_STRING;
-    case (14200):
       return NO_VALID_SENSOR_TEMPERATURE_STRING;
-    case (14201):
+    case (14101):
       return NO_HEALTHY_HEATER_AVAILABLE_STRING;
-    case (14202):
+    case (14102):
       return SYRLINKS_OVERHEATING_STRING;
-    case (14203):
+    case (14103):
       return PLOC_OVERHEATING_STRING;
-    case (14204):
+    case (14104):
       return OBC_OVERHEATING_STRING;
-    case (14205):
+    case (14105):
       return HPA_OVERHEATING_STRING;
-    case (14206):
+    case (14106):
       return PLPCDU_OVERHEATING_STRING;
+    case (14201):
+      return TX_TIMER_EXPIRED_STRING;
+    case (14202):
+      return BIT_LOCK_TX_ON_STRING;
+    case (14300):
+      return POSSIBLE_FILE_CORRUPTION_STRING;
     default:
       return "UNKNOWN_EVENT";
   }

generators/objects/translateObjects.cpp

@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 159 translations.
- * Generated on: 2023-02-24 16:57:00
+ * Contains 164 translations.
+ * Generated on: 2023-03-08 16:44:32
  */
 #include "translateObjects.h"
 
@@ -85,11 +85,13 @@ const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF";
 const char *GPIO_IF_STRING = "GPIO_IF";
 const char *SCEX_UART_READER_STRING = "SCEX_UART_READER";
 const char *SPI_MAIN_COM_IF_STRING = "SPI_MAIN_COM_IF";
-const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK";
-const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF";
 const char *UART_COM_IF_STRING = "UART_COM_IF";
 const char *I2C_COM_IF_STRING = "I2C_COM_IF";
 const char *CSP_COM_IF_STRING = "CSP_COM_IF";
+const char *ACS_BOARD_POLLING_TASK_STRING = "ACS_BOARD_POLLING_TASK";
+const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK";
+const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF";
+const char *SUS_POLLING_TASK_STRING = "SUS_POLLING_TASK";
 const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR";
 const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR";
 const char *TCP_TMTC_SERVER_STRING = "TCP_TMTC_SERVER";
@@ -145,8 +147,11 @@ 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 *IMTQ_ASSY_STRING = "IMTQ_ASSY";
+const char *STR_ASSY_STRING = "STR_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";
@@ -326,16 +331,20 @@ const char *translateObject(object_id_t object) {
       return SCEX_UART_READER_STRING;
     case 0x49020004:
       return SPI_MAIN_COM_IF_STRING;
-    case 0x49020005:
-      return RW_POLLING_TASK_STRING;
-    case 0x49020006:
-      return SPI_RTD_COM_IF_STRING;
     case 0x49030003:
       return UART_COM_IF_STRING;
     case 0x49040002:
       return I2C_COM_IF_STRING;
     case 0x49050001:
       return CSP_COM_IF_STRING;
+    case 0x49060004:
+      return ACS_BOARD_POLLING_TASK_STRING;
+    case 0x49060005:
+      return RW_POLLING_TASK_STRING;
+    case 0x49060006:
+      return SPI_RTD_COM_IF_STRING;
+    case 0x49060007:
+      return SUS_POLLING_TASK_STRING;
     case 0x50000100:
       return CCSDS_PACKET_DISTRIBUTOR_STRING;
     case 0x50000200:
@@ -447,9 +456,15 @@ 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 0x73000008:
+      return IMTQ_ASSY_STRING;
+    case 0x73000009:
+      return STR_ASSY_STRING;
     case 0x73000100:
       return TM_FUNNEL_STRING;
     case 0x73000101:

linux/ObjectFactory.cpp

@@ -10,8 +10,10 @@
 #include <fsfw_hal/linux/spi/SpiCookie.h>
 #include <linux/callbacks/gpioCallbacks.h>
 #include <linux/devices/Max31865RtdPolling.h>
+#include <linux/devices/SusPolling.h>
 #include <mission/controller/AcsController.h>
 #include <mission/core/GenericFactory.h>
+#include <mission/devices/LegacySusHandler.h>
 #include <mission/devices/Max31865EiveHandler.h>
 #include <mission/devices/ScexDeviceHandler.h>
 #include <mission/devices/SusHandler.h>
@@ -33,6 +35,7 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo
                                               PowerSwitchIF& pwrSwitcher, std::string spiDev,
                                               bool swap0And6) {
   using namespace gpio;
+  new SusPolling(objects::SUS_POLLING_TASK, *spiComIF, *gpioComIF);
   GpioCookie* gpioCookieSus = new GpioCookie();
   GpioCallback* susgpio = nullptr;
 
@@ -78,99 +81,100 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo
 #if OBSW_ADD_SUN_SENSORS == 1
   SusFdir* fdir = nullptr;
   std::array<SusHandler*, 12> susHandlers = {};
-  SpiCookie* spiCookie = new SpiCookie(addresses::SUS_0, gpioIds::CS_SUS_0, SUS::MAX_CMD_SIZE,
-                                       spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
+  SpiCookie* spiCookie =
+      new SpiCookie(addresses::SUS_0, gpioIds::CS_SUS_0, susMax1227::MAX_CMD_SIZE,
+                    spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[0] =
-      new SusHandler(objects::SUS_0_N_LOC_XFYFZM_PT_XF, 0, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_0_N_LOC_XFYFZM_PT_XF, 0, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
   susHandlers[0]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_1, gpioIds::CS_SUS_1, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_1, gpioIds::CS_SUS_1, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[1] =
-      new SusHandler(objects::SUS_1_N_LOC_XBYFZM_PT_XB, 1, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_1_N_LOC_XBYFZM_PT_XB, 1, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
   susHandlers[1]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_2, gpioIds::CS_SUS_2, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_2, gpioIds::CS_SUS_2, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[2] =
-      new SusHandler(objects::SUS_2_N_LOC_XFYBZB_PT_YB, 2, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_2_N_LOC_XFYBZB_PT_YB, 2, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
   susHandlers[2]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_3, gpioIds::CS_SUS_3, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_3, gpioIds::CS_SUS_3, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[3] =
-      new SusHandler(objects::SUS_3_N_LOC_XFYBZF_PT_YF, 3, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_3_N_LOC_XFYBZF_PT_YF, 3, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
   susHandlers[3]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_4, gpioIds::CS_SUS_4, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_4, gpioIds::CS_SUS_4, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[4] =
-      new SusHandler(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, 4, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, 4, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
   susHandlers[4]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_5, gpioIds::CS_SUS_5, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_5, gpioIds::CS_SUS_5, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[5] =
-      new SusHandler(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, 5, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, 5, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
   susHandlers[5]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[6] =
-      new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
   susHandlers[6]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_7, gpioIds::CS_SUS_7, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_7, gpioIds::CS_SUS_7, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[7] =
-      new SusHandler(objects::SUS_7_R_LOC_XBYBZM_PT_XB, 7, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_7_R_LOC_XBYBZM_PT_XB, 7, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
   susHandlers[7]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_8, gpioIds::CS_SUS_8, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_8, gpioIds::CS_SUS_8, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[8] =
-      new SusHandler(objects::SUS_8_R_LOC_XBYBZB_PT_YB, 8, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_8_R_LOC_XBYBZB_PT_YB, 8, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
   susHandlers[8]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_9, gpioIds::CS_SUS_9, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_9, gpioIds::CS_SUS_9, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[9] =
-      new SusHandler(objects::SUS_9_R_LOC_XBYBZB_PT_YF, 9, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_9_R_LOC_XBYBZB_PT_YF, 9, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
   susHandlers[9]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_10, gpioIds::CS_SUS_10, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_10, gpioIds::CS_SUS_10, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[10] =
-      new SusHandler(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, 10, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, 10, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
   susHandlers[10]->setCustomFdir(fdir);
 
-  spiCookie = new SpiCookie(addresses::SUS_11, gpioIds::CS_SUS_11, SUS::MAX_CMD_SIZE,
+  spiCookie = new SpiCookie(addresses::SUS_11, gpioIds::CS_SUS_11, susMax1227::MAX_CMD_SIZE,
                             spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::SUS_CS_TIMEOUT);
   susHandlers[11] =
-      new SusHandler(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, 11, objects::SPI_MAIN_COM_IF, spiCookie);
+      new SusHandler(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, 11, objects::SUS_POLLING_TASK, spiCookie);
   fdir = new SusFdir(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
   susHandlers[11]->setCustomFdir(fdir);
 

linux/devices/AcsBoardPolling.cpp

@@ -0,0 +1,732 @@
+#include "AcsBoardPolling.h"
+
+#include <fcntl.h>
+#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>
+#include <fsfw_hal/linux/spi/SpiCookie.h>
+#include <fsfw_hal/linux/utility.h>
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
+#include <sys/ioctl.h>
+
+#include "devices/gpioIds.h"
+
+using namespace returnvalue;
+
+AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF)
+    : SystemObject(objectId), spiComIF(lowLevelComIF), gpioIF(gpioIF) {
+  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
+  semaphore->acquire();
+  ipcLock = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
+  while (true) {
+    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);
+      gyroL3gHandler(gyro3L3g);
+      mgmRm3100Handler(mgm1Rm3100);
+      mgmRm3100Handler(mgm3Rm3100);
+      mgmLis3Handler(mgm0Lis3);
+      mgmLis3Handler(mgm2Lis3);
+    }
+    // To prevent task being not reactivated by tardy tasks
+    TaskFactory::delayTask(20);
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t AcsBoardPolling::initialize() { return returnvalue::OK; }
+
+ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) {
+  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
+  switch (spiCookie->getChipSelectPin()) {
+    case (gpioIds::MGM_0_LIS3_CS): {
+      mgm0Lis3.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::MGM_1_RM3100_CS): {
+      mgm1Rm3100.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::MGM_2_LIS3_CS): {
+      mgm2Lis3.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::MGM_3_RM3100_CS): {
+      mgm3Rm3100.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::GYRO_0_ADIS_CS): {
+      gyro0Adis.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::GYRO_1_L3G_CS): {
+      gyro1L3g.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::GYRO_2_ADIS_CS): {
+      gyro2Adis.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::GYRO_3_L3G_CS): {
+      gyro3L3g.cookie = spiCookie;
+      break;
+    }
+    default: {
+      sif::error << "AcsBoardPollingTask: invalid spi cookie" << std::endl;
+    }
+  }
+  return spiComIF.initializeInterface(cookie);
+}
+
+ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
+                                           size_t sendLen) {
+  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
+  auto handleAdisRequest = [&](GyroAdis& adis) {
+    if (sendLen != sizeof(acs::Adis1650XRequest)) {
+      sif::error << "AcsBoardPolling: invalid adis request send length";
+      adis.replyResult = returnvalue::FAILED;
+      return returnvalue::FAILED;
+    }
+    auto* req = reinterpret_cast<const acs::Adis1650XRequest*>(sendData);
+    if (req->mode != adis.mode) {
+      if (req->mode == acs::SimpleSensorMode::NORMAL) {
+        adis.type = req->type;
+        adis.countdown.setTimeout(adis1650x::START_UP_TIME);
+        if (adis.type == adis1650x::Type::ADIS16507) {
+          adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
+        } else if (adis.type == adis1650x::Type::ADIS16505) {
+          adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16505;
+        } else {
+          sif::warning << "AcsBoardPolling: Unknown ADIS type" << std::endl;
+        }
+        adis.performStartup = true;
+      } else if (req->mode == acs::SimpleSensorMode::OFF) {
+        adis.performStartup = false;
+        adis.ownReply.cfgWasSet = false;
+        adis.ownReply.dataWasSet = false;
+      }
+      adis.mode = req->mode;
+    }
+    return returnvalue::OK;
+  };
+  auto handleL3gRequest = [&](GyroL3g& gyro) {
+    if (sendLen != sizeof(acs::GyroL3gRequest)) {
+      sif::error << "AcsBoardPolling: invalid l3g request send length";
+      gyro.replyResult = returnvalue::FAILED;
+      return returnvalue::FAILED;
+    }
+    auto* req = reinterpret_cast<const acs::GyroL3gRequest*>(sendData);
+    if (req->mode != gyro.mode) {
+      if (req->mode == acs::SimpleSensorMode::NORMAL) {
+        std::memcpy(gyro.sensorCfg, req->ctrlRegs, 5);
+        gyro.performStartup = true;
+      } else {
+        gyro.ownReply.cfgWasSet = false;
+      }
+      gyro.mode = req->mode;
+    }
+    return returnvalue::OK;
+  };
+  auto handleLis3Request = [&](MgmLis3& mgm) {
+    if (sendLen != sizeof(acs::MgmLis3Request)) {
+      sif::error << "AcsBoardPolling: invalid lis3 request send length";
+      mgm.replyResult = returnvalue::FAILED;
+      return returnvalue::FAILED;
+    }
+    auto* req = reinterpret_cast<const acs::MgmLis3Request*>(sendData);
+    if (req->mode != mgm.mode) {
+      if (req->mode == acs::SimpleSensorMode::NORMAL) {
+        mgm.performStartup = true;
+      } else {
+        mgm.ownReply.dataWasSet = false;
+        mgm.ownReply.temperatureWasSet = false;
+      }
+      mgm.mode = req->mode;
+    }
+    return returnvalue::OK;
+  };
+  auto handleRm3100Request = [&](MgmRm3100& mgm) {
+    if (sendLen != sizeof(acs::MgmRm3100Request)) {
+      sif::error << "AcsBoardPolling: invalid rm3100 request send length";
+      mgm.replyResult = returnvalue::FAILED;
+      return returnvalue::FAILED;
+    }
+    auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData);
+    if (req->mode != mgm.mode) {
+      if (req->mode == acs::SimpleSensorMode::NORMAL) {
+        mgm.performStartup = true;
+      } else {
+        mgm.ownReply.dataWasRead = false;
+      }
+      mgm.mode = req->mode;
+    }
+    return returnvalue::OK;
+  };
+  {
+    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;
+      }
+    }
+    if (state == InternalState::IDLE) {
+      state = InternalState::BUSY;
+    }
+  }
+  semaphore->release();
+  return returnvalue::OK;
+}
+
+ReturnValue_t AcsBoardPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
+
+ReturnValue_t AcsBoardPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
+  return returnvalue::OK;
+}
+
+ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
+                                                   size_t* size) {
+  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
+  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);
+    *size = sizeof(acs::Adis1650XReply);
+    return gyro.replyResult;
+  };
+  auto handleL3gReply = [&](GyroL3g& gyro) {
+    std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply));
+    *buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
+    *size = sizeof(acs::GyroL3gReply);
+    return gyro.replyResult;
+  };
+  auto handleRm3100Reply = [&](MgmRm3100& mgm) {
+    std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmRm3100Reply));
+    *buffer = reinterpret_cast<uint8_t*>(&mgm.readerReply);
+    *size = sizeof(acs::MgmRm3100Reply);
+    return mgm.replyResult;
+  };
+  auto handleLis3Reply = [&](MgmLis3& mgm) {
+    std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmLis3Reply));
+    *buffer = reinterpret_cast<uint8_t*>(&mgm.readerReply);
+    *size = sizeof(acs::MgmLis3Reply);
+    return mgm.replyResult;
+  };
+  switch (spiCookie->getChipSelectPin()) {
+    case (gpioIds::MGM_0_LIS3_CS): {
+      return handleLis3Reply(mgm0Lis3);
+    }
+    case (gpioIds::MGM_1_RM3100_CS): {
+      return handleRm3100Reply(mgm1Rm3100);
+    }
+    case (gpioIds::MGM_2_LIS3_CS): {
+      return handleLis3Reply(mgm2Lis3);
+    }
+    case (gpioIds::MGM_3_RM3100_CS): {
+      return handleRm3100Reply(mgm3Rm3100);
+    }
+    case (gpioIds::GYRO_0_ADIS_CS): {
+      return handleAdisReply(gyro0Adis);
+    }
+    case (gpioIds::GYRO_2_ADIS_CS): {
+      return handleAdisReply(gyro2Adis);
+    }
+    case (gpioIds::GYRO_1_L3G_CS): {
+      return handleL3gReply(gyro1L3g);
+    }
+    case (gpioIds::GYRO_3_L3G_CS): {
+      return handleL3gReply(gyro3L3g);
+    }
+  }
+  return returnvalue::OK;
+}
+
+void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
+  ReturnValue_t result;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+  bool gyroPerformStartup = false;
+  {
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    mode = l3g.mode;
+    gyroPerformStartup = l3g.performStartup;
+  }
+  if (mode == acs::SimpleSensorMode::NORMAL) {
+    if (gyroPerformStartup) {
+      cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK;
+      std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5);
+      result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
+      if (result != returnvalue::OK) {
+        l3g.replyResult = returnvalue::OK;
+      }
+      // Ignore useless reply and red config
+      cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
+      std::memset(cmdBuf.data() + 1, 0, 5);
+      result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
+      if (result != returnvalue::OK) {
+        l3g.replyResult = returnvalue::OK;
+      }
+      result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
+      if (result != returnvalue::OK) {
+        l3g.replyResult = returnvalue::OK;
+      }
+      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]) {
+          sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
+          l3g.replyResult = returnvalue::FAILED;
+          return;
+        }
+      }
+      l3g.performStartup = false;
+      l3g.ownReply.cfgWasSet = true;
+      l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]);
+    }
+    cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
+    std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN);
+    result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), l3gd20h::READ_LEN + 1);
+    if (result != returnvalue::OK) {
+      l3g.replyResult = returnvalue::FAILED;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
+    if (result != returnvalue::OK) {
+      l3g.replyResult = returnvalue::FAILED;
+      return;
+    }
+    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++) {
+      if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
+        sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
+        l3g.replyResult = returnvalue::FAILED;
+        return;
+      }
+    }
+    l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX];
+    l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L];
+    l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L];
+    l3g.ownReply.angVelocities[2] = (rawReply[l3gd20h::OUT_Z_H] << 8) | rawReply[l3gd20h::OUT_Z_L];
+    l3g.ownReply.tempOffsetRaw = rawReply[l3gd20h::TEMPERATURE_IDX];
+  }
+}
+
+ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) {
+  ReturnValue_t result = returnvalue::OK;
+  int retval = 0;
+  // Prepare transfer
+  int fileDescriptor = 0;
+  std::string device = spiComIF.getSpiDev();
+  UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
+  if (fileHelper.getOpenResult() != returnvalue::OK) {
+    return SpiComIF::OPENING_FILE_FAILED;
+  }
+  spi::SpiModes spiMode = spi::SpiModes::MODE_0;
+  uint32_t spiSpeed = 0;
+  cookie.getSpiParameters(spiMode, spiSpeed, nullptr);
+  spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
+  cookie.assignWriteBuffer(cmdBuf.data());
+  cookie.setTransferSize(2);
+
+  gpioId_t gpioId = cookie.getChipSelectPin();
+  MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+  uint32_t timeoutMs = 0;
+  MutexIF* mutex = spiComIF.getCsMutex();
+  cookie.getMutexParams(timeoutType, timeoutMs);
+  if (mutex == nullptr) {
+    sif::warning << "GyroADIS16507Handler::spiSendCallback: "
+                    "Mutex or GPIO interface invalid"
+                 << std::endl;
+    return returnvalue::FAILED;
+  }
+
+  size_t idx = 0;
+  spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle();
+  uint64_t origTx = transferStruct->tx_buf;
+  uint64_t origRx = transferStruct->rx_buf;
+  while (idx < transferLen) {
+    result = mutex->lockMutex(timeoutType, timeoutMs);
+    if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl;
+#endif
+      return result;
+    }
+    // Pull SPI CS low. For now, no support for active high given
+    if (gpioId != gpio::NO_GPIO) {
+      gpioIF.pullLow(gpioId);
+    }
+
+    // Execute transfer
+    // Initiate a full duplex SPI transfer.
+    retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle());
+    if (retval < 0) {
+      utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
+      result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
+    }
+#if FSFW_HAL_SPI_WIRETAPPING == 1
+    comIf->performSpiWiretapping(cookie);
+#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
+
+    if (gpioId != gpio::NO_GPIO) {
+      gpioIF.pullHigh(gpioId);
+    }
+    mutex->unlockMutex();
+
+    idx += 2;
+    transferStruct->tx_buf += 2;
+    transferStruct->rx_buf += 2;
+    if (idx < transferLen) {
+      usleep(adis1650x::STALL_TIME_MICROSECONDS);
+    }
+  }
+  transferStruct->tx_buf = origTx;
+  transferStruct->rx_buf = origRx;
+  cookie.setTransferSize(transferLen);
+  return returnvalue::OK;
+}
+
+void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
+  ReturnValue_t result;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+  bool cdHasTimedOut = false;
+  bool mustPerformStartup = false;
+  {
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    mode = gyro.mode;
+    cdHasTimedOut = gyro.countdown.hasTimedOut();
+    mustPerformStartup = gyro.performStartup;
+  }
+  if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) {
+    if (mustPerformStartup) {
+      uint8_t regList[6];
+      // Read configuration
+      regList[0] = adis1650x::DIAG_STAT_REG;
+      regList[1] = adis1650x::FILTER_CTRL_REG;
+      regList[2] = adis1650x::RANG_MDL_REG;
+      regList[3] = adis1650x::MSC_CTRL_REG;
+      regList[4] = adis1650x::DEC_RATE_REG;
+      regList[5] = adis1650x::PROD_ID_REG;
+      size_t transferLen =
+          adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size());
+      result = readAdisCfg(*gyro.cookie, transferLen);
+      if (result != returnvalue::OK) {
+        gyro.replyResult = result;
+        return;
+      }
+      result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
+      if (result != returnvalue::OK or rawReply == nullptr) {
+        gyro.replyResult = result;
+        return;
+      }
+      uint16_t prodId = (rawReply[12] << 8) | rawReply[13];
+      if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or
+          ((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) {
+        sif::warning << "AcsPollingTask: Invalid  ADIS product ID " << prodId << std::endl;
+        gyro.replyResult = returnvalue::FAILED;
+        return;
+      }
+      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];
+      gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7];
+      gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9];
+      gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11];
+      gyro.ownReply.cfg.prodId = prodId;
+      gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
+      gyro.performStartup = false;
+    }
+    // Read regular registers
+    std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
+                adis1650x::BURST_READ_ENABLE.size());
+    std::memset(cmdBuf.data() + 2, 0, 10 * 2);
+    result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE);
+    if (result != returnvalue::OK) {
+      gyro.replyResult = returnvalue::FAILED;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
+    if (result != returnvalue::OK or rawReply == nullptr) {
+      gyro.replyResult = returnvalue::FAILED;
+      return;
+    }
+    uint16_t checksum = (rawReply[20] << 8) | rawReply[21];
+
+    // Now verify the read checksum with the expected checksum according to datasheet p. 20
+    uint16_t calcChecksum = 0;
+    for (size_t idx = 2; idx < 20; idx++) {
+      calcChecksum += rawReply[idx];
+    }
+    if (checksum != calcChecksum) {
+      sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl;
+      gyro.replyResult = returnvalue::FAILED;
+      return;
+    }
+
+    auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg);
+    if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) {
+      sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode"
+                    " not implemented!"
+                 << std::endl;
+      gyro.replyResult = returnvalue::FAILED;
+      return;
+    }
+
+    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];
+    gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7];
+    gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9];
+
+    gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11];
+    gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13];
+    gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15];
+
+    gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17];
+  }
+}
+
+void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
+  ReturnValue_t result;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+  bool mustPerformStartup = false;
+  {
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    mode = mgm.mode;
+    mustPerformStartup = mgm.performStartup;
+  }
+  if (mode == acs::SimpleSensorMode::NORMAL) {
+    if (mustPerformStartup) {
+      // To check valid communication, read back identification
+      // register which should always be the same value.
+      cmdBuf[0] = mgmLis3::readCommand(mgmLis3::IDENTIFY_DEVICE_REG_ADDR);
+      cmdBuf[1] = 0x00;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      if (rawReply[1] != mgmLis3::DEVICE_ID) {
+        sif::error << "AcsPollingTask: invalid MGM lis3 device ID" << std::endl;
+        mgm.replyResult = result;
+        return;
+      }
+      mgm.cfg[0] = mgmLis3::CTRL_REG1_DEFAULT;
+      mgm.cfg[1] = mgmLis3::CTRL_REG2_DEFAULT;
+      mgm.cfg[2] = mgmLis3::CTRL_REG3_DEFAULT;
+      mgm.cfg[3] = mgmLis3::CTRL_REG4_DEFAULT;
+      mgm.cfg[4] = mgmLis3::CTRL_REG5_DEFAULT;
+      cmdBuf[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1, true);
+      std::memcpy(cmdBuf.data() + 1, mgm.cfg, 5);
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 6);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      // Done here. We can always read back config and data during periodic handling
+      mgm.performStartup = false;
+    }
+    cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true);
+    std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS);
+    result =
+        spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1);
+    if (result != returnvalue::OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+    if (result != returnvalue::OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    // Verify communication by re-checking config
+    if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or
+        rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) {
+      mgm.replyResult = result;
+      return;
+    }
+    {
+      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] =
+          (rawReply[mgmLis3::X_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::X_LOWBYTE_IDX];
+      mgm.ownReply.mgmValuesRaw[1] =
+          (rawReply[mgmLis3::Y_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Y_LOWBYTE_IDX];
+      mgm.ownReply.mgmValuesRaw[2] =
+          (rawReply[mgmLis3::Z_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Z_LOWBYTE_IDX];
+    }
+    // Read tempetature
+    cmdBuf[0] = mgmLis3::readCommand(mgmLis3::TEMP_LOWBYTE, true);
+    result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 3);
+    if (result != returnvalue::OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+    if (result != returnvalue::OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    mgm.ownReply.temperatureWasSet = true;
+    mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1];
+  }
+}
+
+void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
+  ReturnValue_t result;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+  bool mustPerformStartup = false;
+  {
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    mode = mgm.mode;
+    mustPerformStartup = mgm.performStartup;
+  }
+  if (mode == acs::SimpleSensorMode::NORMAL) {
+    if (mustPerformStartup) {
+      // Configure CMM first
+      cmdBuf[0] = mgmRm3100::CMM_REGISTER;
+      cmdBuf[1] = mgmRm3100::CMM_VALUE;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      // Read back register
+      cmdBuf[0] = mgmRm3100::CMM_REGISTER | mgmRm3100::READ_MASK;
+      cmdBuf[1] = 0;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      if (rawReply[1] != mgmRm3100::CMM_VALUE) {
+        sif::error << "AcsBoardPolling: MGM RM3100 read back CMM invalid" << std::endl;
+        mgm.replyResult = result;
+        return;
+      }
+      // Configure TMRC register
+      cmdBuf[0] = mgmRm3100::TMRC_REGISTER;
+      // hardcoded for now
+      cmdBuf[1] = mgm.tmrcValue;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      // Read back and verify value
+      cmdBuf[0] = mgmRm3100::TMRC_REGISTER | mgmRm3100::READ_MASK;
+      cmdBuf[1] = 0;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      if (rawReply[1] != mgm.tmrcValue) {
+        sif::error << "AcsBoardPolling: MGM RM3100 read back TMRC invalid" << std::endl;
+        mgm.replyResult = result;
+        return;
+      }
+      mgm.performStartup = false;
+    }
+    // Regular read operation
+    cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK;
+    std::memset(cmdBuf.data() + 1, 0, 9);
+    result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 10);
+    if (result != OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+    if (result != OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    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!
+      mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN;
+    }
+    mgm.ownReply.dataWasRead = true;
+    // Bitshift trickery to account for 24 bit signed value.
+    mgm.ownReply.mgmValuesRaw[0] =
+        ((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8;
+    mgm.ownReply.mgmValuesRaw[1] =
+        ((rawReply[4] << 24) | (rawReply[5] << 16) | (rawReply[6] << 8)) >> 8;
+    mgm.ownReply.mgmValuesRaw[2] =
+        ((rawReply[7] << 24) | (rawReply[8] << 16) | (rawReply[9] << 8)) >> 8;
+  }
+}

linux/devices/AcsBoardPolling.h

@@ -0,0 +1,91 @@
+#ifndef LINUX_DEVICES_ACSBOARDPOLLING_H_
+#define LINUX_DEVICES_ACSBOARDPOLLING_H_
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/tasks/SemaphoreIF.h>
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h>
+#include <fsfw_hal/linux/spi/SpiComIF.h>
+#include <mission/devices/devicedefinitions/acsPolling.h>
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
+
+class AcsBoardPolling : public SystemObject,
+                        public ExecutableObjectIF,
+                        public DeviceCommunicationIF {
+ public:
+  AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF);
+
+  ReturnValue_t performOperation(uint8_t operationCode) override;
+  ReturnValue_t initialize() override;
+
+ 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;
+
+  struct DevBase {
+    SpiCookie* cookie = nullptr;
+    bool performStartup = false;
+    acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+    ReturnValue_t replyResult = returnvalue::OK;
+  };
+
+  struct GyroAdis : public DevBase {
+    adis1650x::Type type;
+    Countdown countdown;
+    acs::Adis1650XReply ownReply;
+    acs::Adis1650XReply readerReply;
+  };
+  GyroAdis gyro0Adis{};
+  GyroAdis gyro2Adis{};
+
+  struct GyroL3g : public DevBase {
+    uint8_t sensorCfg[5];
+    acs::GyroL3gReply ownReply;
+    acs::GyroL3gReply readerReply;
+  };
+  GyroL3g gyro1L3g{};
+  GyroL3g gyro3L3g{};
+
+  struct MgmRm3100 : public DevBase {
+    uint8_t tmrcValue = mgmRm3100::TMRC_DEFAULT_37HZ_VALUE;
+    acs::MgmRm3100Reply ownReply;
+    acs::MgmRm3100Reply readerReply;
+  };
+  MgmRm3100 mgm1Rm3100;
+  MgmRm3100 mgm3Rm3100;
+
+  struct MgmLis3 : public DevBase {
+    uint8_t cfg[5]{};
+    acs::MgmLis3Reply ownReply;
+    acs::MgmLis3Reply readerReply;
+  };
+  MgmLis3 mgm0Lis3;
+  MgmLis3 mgm2Lis3;
+
+  uint8_t* rawReply = nullptr;
+  size_t dummy = 0;
+
+  SpiComIF& spiComIF;
+  GpioIF& gpioIF;
+
+  ReturnValue_t initializeInterface(CookieIF* cookie) override;
+  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
+  ReturnValue_t getSendSuccess(CookieIF* cookie) override;
+  ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
+  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
+
+  void gyroL3gHandler(GyroL3g& l3g);
+  void gyroAdisHandler(GyroAdis& gyro);
+  void mgmLis3Handler(MgmLis3& mgm);
+  void mgmRm3100Handler(MgmRm3100& mgm);
+  // Special readout: 16us stall time between small 2 byte transfers.
+  ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen);
+};
+
+#endif /* LINUX_DEVICES_ACSBOARDPOLLING_H_ */

linux/devices/CMakeLists.txt

@@ -4,8 +4,14 @@ endif()
 
 target_sources(
   ${OBSW_NAME}
-  PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp
-          ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)
+  PRIVATE Max31865RtdPolling.cpp
+          ScexUartReader.cpp
+          ImtqPollingTask.cpp
+          SusPolling.cpp
+          ScexDleParser.cpp
+          ScexHelper.cpp
+          RwPollingTask.cpp
+          AcsBoardPolling.cpp)
 
 add_subdirectory(ploc)
 

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);
@@ -172,20 +170,10 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
         return false;
       }
       oneShotSwitches.gpsReadFailedSwitch = true;
-      // did not event get mode, nothing to see.
-      if (MODE_SET != (MODE_SET & gps.set)) {
-        if (mode != MODE_OFF) {
-          if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
-            sif::warning
-                << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed "
-                << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
-            triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
-            oneShotSwitches.cantGetFixSwitch = false;
-          }
-          // Mode is on, so do next read immediately
-          return true;
-        }
-        // GPS device is off anyway, so do other handling
+      ReturnValue_t result = handleGpsReadData();
+      if (result == returnvalue::OK) {
+        return true;
+      } else {
         return false;
       }
       noModeSetCntr = 0;
@@ -197,11 +185,26 @@ bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
                   "SHM read not implemented"
                << std::endl;
   }
-  handleGpsReadData();
   return true;
 }
 
 ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
+  bool modeIsSet = true;
+  if (MODE_SET != (MODE_SET & gps.set)) {
+    if (mode != MODE_OFF) {
+      if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
+        sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed "
+                     << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
+        triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
+        oneShotSwitches.cantGetFixSwitch = false;
+      }
+      modeIsSet = false;
+    } else {
+      // GPS device is off anyway, so do other handling
+      return returnvalue::FAILED;
+    }
+  }
+
   PoolReadGuard pg(&gpsSet);
   if (pg.getReadResult() != returnvalue::OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
@@ -211,66 +214,95 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
   }
 
   bool validFix = false;
-  // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
-  if (gps.fix.mode == 2 or gps.fix.mode == 3) {
-    validFix = true;
-  }
-  if (gpsSet.fixMode.value != gps.fix.mode) {
-    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode);
-  }
-  gpsSet.fixMode.value = gps.fix.mode;
-  if (gps.fix.mode == 0 or gps.fix.mode == 1) {
-    if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
-      // We are supposed to be on and functioning, but no fix was found
-      if (mode == MODE_ON or mode == MODE_NORMAL) {
-        mode = MODE_OFF;
-      }
-      modeCommanded = false;
+  uint8_t newFix = 0;
+  if (modeIsSet) {
+    // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
+    if (gps.fix.mode == 2 or gps.fix.mode == 3) {
+      validFix = true;
+    }
+    newFix = gps.fix.mode;
+    if (newFix == 0 or newFix == 1) {
+      if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
+        // We are supposed to be on and functioning, but no fix was found
+        if (mode == MODE_ON or mode == MODE_NORMAL) {
+          mode = MODE_OFF;
+        }
+        modeCommanded = false;
+      }
+    }
+  }
+  if (gpsSet.fixMode.value != newFix) {
+    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFix);
+  }
+  gpsSet.fixMode = newFix;
+  gpsSet.fixMode.setValid(modeIsSet);
+
+  // Only set on specific messages, so only set a valid flag to invalid
+  // if not set for more than a full message set (10 messages here)
+  if (SATELLITE_SET == (SATELLITE_SET & gps.set)) {
+    gpsSet.satInUse.value = gps.satellites_used;
+    gpsSet.satInView.value = gps.satellites_visible;
+    if (not gpsSet.satInUse.isValid()) {
+      gpsSet.satInUse.setValid(true);
+      gpsSet.satInView.setValid(true);
+    }
+    satNotSetCounter = 0;
+  } else {
+    satNotSetCounter++;
+    if (gpsSet.satInUse.isValid() and satNotSetCounter >= 10) {
+      gpsSet.satInUse.setValid(false);
+      gpsSet.satInView.setValid(false);
     }
-    gpsSet.setValidity(false, true);
-  } else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) {
-    gpsSet.setValidity(true, true);
   }
 
-  gpsSet.satInUse.value = gps.satellites_used;
-  gpsSet.satInView.value = gps.satellites_visible;
-
+  // LATLON is set for every message, no need for a counter
   bool latValid = false;
-  if (std::isfinite(gps.fix.latitude)) {
-    // Negative latitude -> South direction
-    gpsSet.latitude.value = gps.fix.latitude;
-    if (gps.fix.mode >= 2) {
-      latValid = true;
+  bool longValid = false;
+  if (LATLON_SET == (LATLON_SET & gps.set)) {
+    if (std::isfinite(gps.fix.latitude)) {
+      // Negative latitude -> South direction
+      gpsSet.latitude.value = gps.fix.latitude;
+      // As specified in gps.h: Only valid if mode >= 2
+      if (gps.fix.mode >= 2) {
+        latValid = true;
+      }
+    }
+
+    if (std::isfinite(gps.fix.longitude)) {
+      // Negative longitude -> West direction
+      gpsSet.longitude.value = gps.fix.longitude;
+      // As specified in gps.h: Only valid if mode >= 2
+      if (gps.fix.mode >= 2) {
+        longValid = true;
+      }
     }
   }
   gpsSet.latitude.setValid(latValid);
+  gpsSet.longitude.setValid(longValid);
 
-  bool longValid = false;
-  if (std::isfinite(gps.fix.longitude)) {
-    // Negative longitude -> West direction
-    gpsSet.longitude.value = gps.fix.longitude;
-    if (gps.fix.mode >= 2) {
-      longValid = true;
-    }
-  }
-  gpsSet.latitude.setValid(longValid);
-
+  // ALTITUDE is set for every message, no need for a counter
   bool altitudeValid = false;
-  if (std::isfinite(gps.fix.altitude)) {
+  if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) {
     gpsSet.altitude.value = gps.fix.altitude;
+    // As specified in gps.h: Only valid if mode == 3
     if (gps.fix.mode == 3) {
       altitudeValid = true;
     }
   }
   gpsSet.altitude.setValid(altitudeValid);
 
-  if (std::isfinite(gps.fix.speed)) {
+  // SPEED is set for every message, no need for a counter
+  bool speedValid = false;
+  if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
     gpsSet.speed.value = gps.fix.speed;
-  } else {
-    gpsSet.speed.setValid(false);
+    speedValid = true;
   }
+  gpsSet.speed.setValid(speedValid);
 
+  // TIME is set for every message, no need for a counter
+  bool timeValid = false;
   if (TIME_SET == (TIME_SET & gps.set)) {
+    timeValid = true;
     timeval time = {};
 #if LIBGPS_VERSION_MINOR <= 17
     gpsSet.unixSeconds.value = std::floor(gps.fix.time);
@@ -294,15 +326,14 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
     gpsSet.hours = timeOfDay.hour;
     gpsSet.minutes = timeOfDay.minute;
     gpsSet.seconds = timeOfDay.second;
-  } else {
-    gpsSet.unixSeconds.setValid(false);
-    gpsSet.year.setValid(false);
-    gpsSet.month.setValid(false);
-    gpsSet.day.setValid(false);
-    gpsSet.hours.setValid(false);
-    gpsSet.minutes.setValid(false);
-    gpsSet.seconds.setValid(false);
   }
+  gpsSet.unixSeconds.setValid(timeValid);
+  gpsSet.year.setValid(timeValid);
+  gpsSet.month.setValid(timeValid);
+  gpsSet.day.setValid(timeValid);
+  gpsSet.hours.setValid(timeValid);
+  gpsSet.minutes.setValid(timeValid);
+  gpsSet.seconds.setValid(timeValid);
 
   if (debugHyperionGps) {
     sif::info << "-- Hyperion GPS Data --" << std::endl;

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 };
 
@@ -61,6 +62,8 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
   Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);
   bool modeCommanded = false;
   bool timeInit = false;
+  uint8_t satNotSetCounter = 0;
+
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;
 #endif
@@ -77,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.

linux/devices/ImtqPollingTask.cpp

@@ -28,6 +28,8 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
     // Stopwatch watch;
     switch (currentRequest) {
       case imtq::RequestType::MEASURE_NO_ACTUATION: {
+        // Measured to take 24 ms for debug and release builds.
+        // Stopwatch watch;
         handleMeasureStep();
         break;
       }
@@ -35,6 +37,9 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
         handleActuateStep();
         break;
       }
+      default: {
+        break;
+      }
     };
   }
   return returnvalue::OK;
@@ -44,6 +49,9 @@ void ImtqPollingTask::handleMeasureStep() {
   size_t replyLen = 0;
   uint8_t* replyPtr;
   ImtqRepliesDefault replies(replyBuf.data());
+  // If some startup handling is added later, set configured after it was done once.
+  replies.setConfigured();
+
   // Can be used later to verify correct timing (e.g. all data has been read)
   clearReadFlagsDefault(replies);
   auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
@@ -118,12 +126,19 @@ void ImtqPollingTask::handleMeasureStep() {
   }
   // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
   // commands.
-  TaskFactory::delayTask(currentIntegrationTimeMs);
+  TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);
 
   cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
   if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
     return;
   }
+  bool mgmMeasurementTooOld = false;
+  // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
+  // have old data. Which can be really bad for ACS. And everything.
+  if ((replyPtr[2] >> 7) == 0) {
+    replyPtr[0] = false;
+    mgmMeasurementTooOld = true;
+  }
 
   cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
   if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
@@ -134,7 +149,9 @@ void ImtqPollingTask::handleMeasureStep() {
   if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
     return;
   }
-  // sif::debug << "measure done" << std::endl;
+  if (mgmMeasurementTooOld) {
+    sif::error << "IMTQ: MGM measurement too old" << std::endl;
+  }
   return;
 }
 
@@ -157,23 +174,36 @@ void ImtqPollingTask::handleActuateStep() {
     return;
   }
 
+  TaskFactory::delayTask(10);
+
   cmdLen = 1;
   cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
   if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
     return;
   }
 
-  TaskFactory::delayTask(currentIntegrationTimeMs);
+  TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);
 
   cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
   if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
     return;
   }
+  bool measurementWasTooOld = false;
+  // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
+  // have old data. Which can be really bad for ACS. And everything.
+  if ((replyPtr[2] >> 7) == 0) {
+    measurementWasTooOld = true;
+    replyPtr[0] = false;
+  }
+
   cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
   if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
     return;
   }
-  // sif::debug << "measure with torque done" << std::endl;
+
+  if (measurementWasTooOld) {
+    sif::error << "IMTQ: MGM measurement too old" << std::endl;
+  }
   return;
 }
 
@@ -192,15 +222,15 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
 
 ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                            size_t sendLen) {
-  ImtqRequest request(sendData, sendLen);
+  const auto* imtqReq = reinterpret_cast<const imtq::Request*>(sendData);
   {
     MutexGuard mg(ipcLock);
-    currentRequest = request.getRequestType();
-    if (currentRequest == imtq::RequestType::ACTUATE) {
-      std::memcpy(dipoles, request.getDipoles(), 6);
-      torqueDuration = request.getTorqueDuration();
+    if (imtqReq->request == imtq::RequestType::ACTUATE) {
+      std::memcpy(dipoles, imtqReq->dipoles, sizeof(dipoles));
+      torqueDuration = imtqReq->torqueDuration;
     }
-    specialRequest = request.getSpecialRequest();
+    currentRequest = imtqReq->request;
+    specialRequest = imtqReq->specialRequest;
     if (state != InternalState::IDLE) {
       return returnvalue::FAILED;
     }
@@ -309,6 +339,8 @@ void ImtqPollingTask::buildDipoleCommand() {
   }
   SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(),
                               SerializeIF::Endianness::LITTLE);
+  // sif::debug << "Dipole X: " << dipoles[0] << std::endl;
+  // sif::debug << "Torqeu Dur: " << torqueDuration << std::endl;
   cmdLen = 1 + serLen;
 }
 
@@ -325,9 +357,11 @@ ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** b
   if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) {
     replyLen = getExchangeBufLen(specialRequest);
     memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
-  } else {
+  } else if (currentRequest == imtq::RequestType::ACTUATE) {
     replyLen = ImtqRepliesWithTorque::BASE_LEN;
     memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen);
+  } else {
+    *size = 0;
   }
   *buffer = exchangeBuf.data();
   *size = replyLen;

linux/devices/ImtqPollingTask.h

@@ -32,12 +32,13 @@ class ImtqPollingTask : public SystemObject,
   const char* i2cDev = nullptr;
   address_t i2cAddr = 0;
   uint32_t currentIntegrationTimeMs = 10;
+  // Required in addition to integration time, otherwise old data might be read.
+  static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 6;
   bool ignoreNextActuateRequest = false;
 
   imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
   int16_t dipoles[3] = {};
   uint16_t torqueDuration = 0;
-  // uint8_t startActuateRawBuf[3] = {};
 
   std::array<uint8_t, 32> cmdBuf;
   std::array<uint8_t, 524> replyBuf;

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()) {
-#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;
@@ -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);
-    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 +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;

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

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

linux/devices/SusPolling.cpp

@@ -0,0 +1,220 @@
+#include "SusPolling.h"
+
+#include <fsfw/tasks/SemaphoreFactory.h>
+#include <fsfw/tasks/TaskFactory.h>
+#include <fsfw/timemanager/Stopwatch.h>
+#include <fsfw_hal/linux/spi/SpiCookie.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/devices/max1227.h>
+#include <unistd.h>
+
+#include "mission/devices/devicedefinitions/susMax1227Helpers.h"
+
+using namespace returnvalue;
+
+SusPolling::SusPolling(object_id_t objectId, SpiComIF& spiComIF, GpioIF& gpioIF)
+    : SystemObject(objectId), spiComIF(spiComIF), gpioIF(gpioIF) {
+  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
+  semaphore->acquire();
+  ipcLock = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t SusPolling::performOperation(uint8_t operationCode) {
+  while (true) {
+    ipcLock->lockMutex();
+    state = InternalState::IDLE;
+    ipcLock->unlockMutex();
+    semaphore->acquire();
+    // Give SUS handlers a chance to submit all requests.
+    TaskFactory::delayTask(2);
+    {
+      // Takes 4-5 ms in debug mode.
+      // Stopwatch watch;
+      handleSusPolling();
+    }
+    // Protection against tardy tasks unlocking the thread again immediately.
+    TaskFactory::delayTask(20);
+  }
+  return OK;
+}
+
+ReturnValue_t SusPolling::initialize() { return OK; }
+
+ReturnValue_t SusPolling::initializeInterface(CookieIF* cookie) {
+  auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return FAILED;
+  }
+  int susIdx = addressToIndex(spiCookie->getSpiAddress());
+  if (susIdx < 0) {
+    return FAILED;
+  }
+  susDevs[susIdx].cookie = spiCookie;
+  return spiComIF.initializeInterface(cookie);
+}
+
+ReturnValue_t SusPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) {
+  auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return FAILED;
+  }
+  int susIdx = addressToIndex(spiCookie->getSpiAddress());
+  if (susIdx < 0) {
+    return FAILED;
+  }
+  if (sendLen != sizeof(acs::SusRequest)) {
+    return FAILED;
+  }
+  const auto* susReq = reinterpret_cast<const acs::SusRequest*>(sendData);
+  MutexGuard mg(ipcLock);
+  if (susDevs[susIdx].mode != susReq->mode) {
+    if (susReq->mode == acs::SimpleSensorMode::NORMAL) {
+      susDevs[susIdx].performStartup = true;
+    } else {
+      susDevs[susIdx].ownReply.cfgWasSet = false;
+      susDevs[susIdx].ownReply.dataWasSet = false;
+    }
+    susDevs[susIdx].mode = susReq->mode;
+  }
+  if (state == InternalState::IDLE) {
+    state = InternalState::BUSY;
+    semaphore->release();
+  }
+  return OK;
+}
+
+ReturnValue_t SusPolling::getSendSuccess(CookieIF* cookie) { return OK; }
+
+ReturnValue_t SusPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { return OK; }
+
+ReturnValue_t SusPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
+  auto* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return FAILED;
+  }
+  int susIdx = addressToIndex(spiCookie->getSpiAddress());
+  if (susIdx < 0) {
+    return FAILED;
+  }
+  MutexGuard mg(ipcLock);
+  std::memcpy(&susDevs[susIdx].readerReply, &susDevs[susIdx].ownReply, sizeof(acs::SusReply));
+  *buffer = reinterpret_cast<uint8_t*>(&susDevs[susIdx].readerReply);
+  *size = sizeof(acs::SusReply);
+  return OK;
+}
+
+ReturnValue_t SusPolling::handleSusPolling() {
+  ReturnValue_t result;
+  acs::SimpleSensorMode modes[12];
+  bool performStartups[12]{};
+  bool cfgsWereSet[12]{};
+  uint8_t idx = 0;
+  {
+    MutexGuard mg(ipcLock);
+    for (idx = 0; idx < 12; idx++) {
+      modes[idx] = susDevs[idx].mode;
+      performStartups[idx] = susDevs[idx].performStartup;
+    }
+  }
+  for (idx = 0; idx < 12; idx++) {
+    if (modes[idx] == acs::SimpleSensorMode::NORMAL) {
+      if (performStartups[idx]) {
+        // Startup handling.
+        cmdBuf[0] = susMax1227::SETUP_INT_CLOKED;
+        result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 1);
+        if (result != OK) {
+          susDevs[idx].replyResult = result;
+          continue;
+        }
+        MutexGuard mg(ipcLock);
+        susDevs[idx].ownReply.cfgWasSet = true;
+        cfgsWereSet[idx] = true;
+        susDevs[idx].performStartup = true;
+      }
+    }
+  }
+  for (idx = 0; idx < 12; idx++) {
+    if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) {
+      // Regular sensor polling.
+      cmdBuf[0] = max1227::buildResetByte(true);
+      cmdBuf[1] = susMax1227::CONVERSION;
+      result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        susDevs[idx].replyResult = result;
+        continue;
+      }
+    }
+  }
+
+  // Internal conversion time is 3.5 us
+  usleep(4);
+
+  for (idx = 0; idx < 12; idx++) {
+    if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) {
+      std::memset(cmdBuf.data(), 0, susMax1227::SIZE_READ_INT_CONVERSIONS);
+      result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(),
+                                    susMax1227::SIZE_READ_INT_CONVERSIONS);
+      if (result != OK) {
+        susDevs[idx].replyResult = result;
+        continue;
+      }
+      result = spiComIF.readReceivedMessage(susDevs[idx].cookie, &rawReply, &dummy);
+      if (result != OK) {
+        susDevs[idx].replyResult = result;
+        continue;
+      }
+      MutexGuard mg(ipcLock);
+      susDevs[idx].ownReply.tempRaw = ((rawReply[0] & 0x0f) << 8) | rawReply[1];
+      for (unsigned chIdx = 0; chIdx < 6; chIdx++) {
+        susDevs[idx].ownReply.channelsRaw[chIdx] =
+            (rawReply[chIdx * 2 + 2] << 8) | rawReply[chIdx * 2 + 3];
+      }
+      susDevs[idx].ownReply.dataWasSet = true;
+    }
+  }
+  return OK;
+}
+
+int SusPolling::addressToIndex(address_t addr) {
+  switch (addr) {
+    case (addresses::SUS_0):
+      return 0;
+      break;
+    case (addresses::SUS_1):
+      return 1;
+      break;
+    case (addresses::SUS_2):
+      return 2;
+      break;
+    case (addresses::SUS_3):
+      return 3;
+      break;
+    case (addresses::SUS_4):
+      return 4;
+      break;
+    case (addresses::SUS_5):
+      return 5;
+      break;
+    case (addresses::SUS_6):
+      return 6;
+      break;
+    case (addresses::SUS_7):
+      return 7;
+      break;
+    case (addresses::SUS_8):
+      return 8;
+      break;
+    case (addresses::SUS_9):
+      return 9;
+      break;
+    case (addresses::SUS_10):
+      return 10;
+      break;
+    case (addresses::SUS_11):
+      return 11;
+      break;
+    default: {
+      return -1;
+    }
+  }
+}

linux/devices/SusPolling.h

@@ -0,0 +1,52 @@
+#ifndef LINUX_DEVICES_SUSPOLLING_H_
+#define LINUX_DEVICES_SUSPOLLING_H_
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/tasks/SemaphoreIF.h>
+#include <fsfw_hal/linux/spi/SpiComIF.h>
+
+#include "devices/addresses.h"
+#include "mission/devices/devicedefinitions/acsPolling.h"
+
+class SusPolling : public SystemObject, public ExecutableObjectIF, public DeviceCommunicationIF {
+ public:
+  SusPolling(object_id_t objectId, SpiComIF& spiComIF, GpioIF& gpioIF);
+
+  ReturnValue_t performOperation(uint8_t operationCode) override;
+  ReturnValue_t initialize() override;
+
+ private:
+  enum class InternalState { IDLE, BUSY } state = InternalState::IDLE;
+
+  struct SusDev {
+    SpiCookie* cookie = nullptr;
+    bool performStartup = false;
+    acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+    ReturnValue_t replyResult = returnvalue::OK;
+    acs::SusReply ownReply{};
+    acs::SusReply readerReply{};
+  };
+
+  MutexIF* ipcLock;
+  SemaphoreIF* semaphore;
+  uint8_t* rawReply = nullptr;
+  size_t dummy = 0;
+  SpiComIF& spiComIF;
+  GpioIF& gpioIF;
+
+  std::array<SusDev, 12> susDevs;
+  std::array<uint8_t, 32> cmdBuf;
+
+  ReturnValue_t initializeInterface(CookieIF* cookie) override;
+  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
+  ReturnValue_t getSendSuccess(CookieIF* cookie) override;
+  ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
+  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
+
+  ReturnValue_t handleSusPolling();
+  static int addressToIndex(address_t addr);
+};
+
+#endif /* LINUX_DEVICES_SUSPOLLING_H_ */

linux/devices/startracker/ArcsecJsonParamBase.cpp

@@ -5,14 +5,14 @@
 ArcsecJsonParamBase::ArcsecJsonParamBase(std::string setName) : setName(setName) {}
 
 ReturnValue_t ArcsecJsonParamBase::create(std::string fullname, uint8_t* buffer) {
-  ReturnValue_t result = returnvalue::OK;
-  result = init(fullname);
-  if (result != returnvalue::OK) {
-    sif::warning << "ArcsecJsonParamBase::create: Failed to init parameter command for set "
-                 << setName << std::endl;
-    return result;
-  }
-  result = createCommand(buffer);
+  // ReturnValue_t result = returnvalue::OK;
+  //  result = init(fullname);
+  //  if (result != returnvalue::OK) {
+  //    sif::warning << "ArcsecJsonParamBase::create: Failed to init parameter command for set "
+  //                 << setName << std::endl;
+  //    return result;
+  //  }
+  ReturnValue_t result = createCommand(buffer);
   if (result != returnvalue::OK) {
     sif::warning << "ArcsecJsonParamBase::create: Failed to create parameter command for set "
                  << setName << std::endl;
@@ -74,12 +74,17 @@ ReturnValue_t ArcsecJsonParamBase::init(const std::string filename) {
                  << std::endl;
     return JSON_FILE_NOT_EXISTS;
   }
-  createJsonObject(filename);
-  result = initSet();
-  if (result != returnvalue::OK) {
-    return result;
+  try {
+    createJsonObject(filename);
+    result = initSet();
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    return returnvalue::OK;
+  } catch (json::exception& e) {
+    // TODO: Re-create json file from backup here.
+    return returnvalue::FAILED;
   }
-  return returnvalue::OK;
 }
 
 void ArcsecJsonParamBase::createJsonObject(const std::string fullname) {

linux/devices/startracker/ArcsecJsonParamBase.h

@@ -41,6 +41,17 @@ class ArcsecJsonParamBase {
    */
   ArcsecJsonParamBase(std::string setName);
 
+  /**
+   * @brief   Initializes the properties json object and the set json object
+   *
+   * @param fullname  Name including absolute path to json file
+   * @param setName   The name of the set to work on
+   *
+   * @param return    JSON_FILE_NOT_EXISTS if specified file does not exist, otherwise
+   *                  returnvalue::OK
+   */
+  ReturnValue_t init(const std::string filename);
+
   /**
    * @brief   Fills a buffer with a parameter set
    *
@@ -124,17 +135,6 @@ class ArcsecJsonParamBase {
    */
   virtual ReturnValue_t createCommand(uint8_t* buffer) = 0;
 
-  /**
-   * @brief   Initializes the properties json object and the set json object
-   *
-   * @param fullname  Name including absolute path to json file
-   * @param setName   The name of the set to work on
-   *
-   * @param return    JSON_FILE_NOT_EXISTS if specified file does not exist, otherwise
-   *                  returnvalue::OK
-   */
-  ReturnValue_t init(const std::string filename);
-
   void createJsonObject(const std::string fullname);
 
   /**

linux/devices/startracker/StarTrackerHandler.cpp

@@ -1,8 +1,11 @@
 #include "StarTrackerHandler.h"
 
 #include <fsfw/ipc/QueueFactory.h>
+#include <fsfw/timemanager/Stopwatch.h>
 
+#include <atomic>
 #include <fstream>
+#include <thread>
 
 #include "OBSWConfig.h"
 #include "StarTrackerJsonCommands.h"
@@ -14,8 +17,11 @@ extern "C" {
 #include "common/misc.h"
 }
 
+std::atomic_bool JCFG_DONE(false);
+
 StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
-                                       StrHelper* strHelper, power::Switch_t powerSwitch)
+                                       const char* jsonFileStr, StrHelper* strHelper,
+                                       power::Switch_t powerSwitch)
     : DeviceHandlerBase(objectId, comIF, comCookie),
       temperatureSet(this),
       versionSet(this),
@@ -40,6 +46,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
       logSubscriptionSet(this),
       debugCameraSet(this),
       strHelper(strHelper),
+      paramJsonFile(jsonFileStr),
       powerSwitch(powerSwitch) {
   if (comCookie == nullptr) {
     sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
@@ -59,6 +66,11 @@ ReturnValue_t StarTrackerHandler::initialize() {
     return result;
   }
 
+  // Spin up a thread to do the JSON initialization, takes 200-250 ms which would
+  // delay whole satellite boot process.
+  jcfgCountdown.resetTimer();
+  jsonCfgTask = std::thread{setUpJsonCfgs, std::ref(jcfgs), paramJsonFile.c_str()};
+
   EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
   if (manager == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -240,8 +252,19 @@ void StarTrackerHandler::doStartUp() {
       // the device handler's submode to the star tracker's mode
       return;
     case StartupState::DONE:
+      if (jcfgCountdown.isBusy()) {
+        startupState = StartupState::WAIT_JCFG;
+        return;
+      }
       startupState = StartupState::IDLE;
       break;
+    case StartupState::WAIT_JCFG: {
+      if (jcfgCountdown.hasTimedOut()) {
+        startupState = StartupState::IDLE;
+        break;
+      }
+      return;
+    }
     default:
       return;
   }
@@ -419,8 +442,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       return returnvalue::OK;
     }
     case (startracker::SUBSCRIPTION): {
-      Subscription subscription;
-      result = prepareParamCommand(commandData, commandDataLen, subscription);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.subscription);
       return returnvalue::OK;
     }
     case (startracker::REQ_SOLUTION): {
@@ -436,68 +458,55 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       return returnvalue::OK;
     }
     case (startracker::LIMITS): {
-      Limits limits;
-      result = prepareParamCommand(commandData, commandDataLen, limits);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.limits);
       return result;
     }
     case (startracker::MOUNTING): {
-      Mounting mounting;
-      result = prepareParamCommand(commandData, commandDataLen, mounting);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.mounting);
       return result;
     }
     case (startracker::IMAGE_PROCESSOR): {
-      ImageProcessor imageProcessor;
-      result = prepareParamCommand(commandData, commandDataLen, imageProcessor);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.imageProcessor);
       return result;
     }
     case (startracker::CAMERA): {
-      Camera camera;
-      result = prepareParamCommand(commandData, commandDataLen, camera);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.camera);
       return result;
     }
     case (startracker::CENTROIDING): {
-      Centroiding centroiding;
-      result = prepareParamCommand(commandData, commandDataLen, centroiding);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.centroiding);
       return result;
     }
     case (startracker::LISA): {
-      Lisa lisa;
-      result = prepareParamCommand(commandData, commandDataLen, lisa);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.lisa);
       return result;
     }
     case (startracker::MATCHING): {
-      Matching matching;
-      result = prepareParamCommand(commandData, commandDataLen, matching);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.matching);
       return result;
     }
     case (startracker::VALIDATION): {
-      Validation validation;
-      result = prepareParamCommand(commandData, commandDataLen, validation);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.validation);
       return result;
     }
     case (startracker::ALGO): {
-      Algo algo;
-      result = prepareParamCommand(commandData, commandDataLen, algo);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.algo);
       return result;
     }
     case (startracker::TRACKING): {
-      Tracking tracking;
-      result = prepareParamCommand(commandData, commandDataLen, tracking);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.tracking);
       return result;
     }
     case (startracker::LOGLEVEL): {
-      LogLevel logLevel;
-      result = prepareParamCommand(commandData, commandDataLen, logLevel);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.logLevel);
       return result;
     }
     case (startracker::LOGSUBSCRIPTION): {
-      LogSubscription logSubscription;
-      result = prepareParamCommand(commandData, commandDataLen, logSubscription);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.logSubscription);
       return result;
     }
     case (startracker::DEBUG_CAMERA): {
-      DebugCamera debugCamera;
-      result = prepareParamCommand(commandData, commandDataLen, debugCamera);
+      result = prepareParamCommand(commandData, commandDataLen, jcfgs.debugCamera);
       return result;
     }
     case (startracker::CHECKSUM): {
@@ -746,6 +755,24 @@ void StarTrackerHandler::bootFirmware(Mode_t toMode) {
   }
 }
 
+void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonFile) {
+  cfgs.tracking.init(paramJsonFile);
+  cfgs.logLevel.init(paramJsonFile);
+  cfgs.logSubscription.init(paramJsonFile);
+  cfgs.debugCamera.init(paramJsonFile);
+  cfgs.algo.init(paramJsonFile);
+  cfgs.validation.init(paramJsonFile);
+  cfgs.matching.init(paramJsonFile);
+  cfgs.lisa.init(paramJsonFile);
+  cfgs.centroiding.init(paramJsonFile);
+  cfgs.camera.init(paramJsonFile);
+  cfgs.imageProcessor.init(paramJsonFile);
+  cfgs.mounting.init(paramJsonFile);
+  cfgs.limits.init(paramJsonFile);
+  cfgs.subscription.init(paramJsonFile);
+  JCFG_DONE = true;
+}
+
 void StarTrackerHandler::bootBootloader() {
   if (internalState == InternalState::IDLE) {
     internalState = InternalState::BOOT_BOOTLOADER;
@@ -1650,6 +1677,7 @@ void StarTrackerHandler::prepareHistogramRequest() {
 ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
                                                       size_t commandDataLen,
                                                       ArcsecJsonParamBase& paramSet) {
+  // Stopwatch watch;
   ReturnValue_t result = returnvalue::OK;
   if (commandDataLen > MAX_PATH_SIZE) {
     return FILE_PATH_TOO_LONG;

linux/devices/startracker/StarTrackerHandler.h

@@ -2,6 +2,9 @@
 #define MISSION_DEVICES_STARTRACKERHANDLER_H_
 
 #include <fsfw/datapool/PoolReadGuard.h>
+#include <linux/devices/startracker/StarTrackerJsonCommands.h>
+
+#include <thread>
 
 #include "ArcsecDatalinkLayer.h"
 #include "ArcsecJsonParamBase.h"
@@ -35,7 +38,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
    * 						to high to enable the device.
    */
   StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
-                     StrHelper* strHelper, power::Switch_t powerSwitch);
+                     const char* jsonFileStr, StrHelper* strHelper, power::Switch_t powerSwitch);
   virtual ~StarTrackerHandler();
 
   ReturnValue_t initialize() override;
@@ -216,15 +219,29 @@ class StarTrackerHandler : public DeviceHandlerBase {
   // Loading firmware requires some time and the command will not trigger a reply when executed
   Countdown bootCountdown;
 
-#ifdef EGSE
-  std::string paramJsonFile = "/home/pi/arcsec/json/flight-config.json";
-#else
-#if OBSW_STAR_TRACKER_GROUND_CONFIG == 1
-  std::string paramJsonFile = "/mnt/sd0/startracker/ground-config.json";
-#else
-  std::string paramJsonFile = "/mnt/sd0/startracker/flight-config.json";
-#endif
-#endif
+  struct JsonConfigs {
+    Tracking tracking;
+    LogLevel logLevel;
+    LogSubscription logSubscription;
+    DebugCamera debugCamera;
+    Algo algo;
+    Validation validation;
+    Matching matching;
+    Lisa lisa;
+    Centroiding centroiding;
+    Camera camera;
+    ImageProcessor imageProcessor;
+    Mounting mounting;
+    Limits limits;
+    Subscription subscription;
+  };
+  JsonConfigs jcfgs;
+  Countdown jcfgCountdown = Countdown(250);
+  bool commandExecuted = false;
+  std::thread jsonCfgTask;
+  static void setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonFile);
+
+  std::string paramJsonFile;
 
   enum class NormalState { TEMPERATURE_REQUEST, SOLUTION_REQUEST };
 
@@ -262,7 +279,14 @@ class StarTrackerHandler : public DeviceHandlerBase {
 
   InternalState internalState = InternalState::IDLE;
 
-  enum class StartupState { IDLE, CHECK_PROGRAM, WAIT_CHECK_PROGRAM, BOOT_BOOTLOADER, DONE };
+  enum class StartupState {
+    IDLE,
+    CHECK_PROGRAM,
+    WAIT_CHECK_PROGRAM,
+    BOOT_BOOTLOADER,
+    WAIT_JCFG,
+    DONE
+  };
 
   StartupState startupState = StartupState::IDLE;
 

linux/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 263 translations.
+ * @brief    Auto-generated event translation file. Contains 269 translations.
  * @details
- * Generated on: 2023-02-24 16:57:00
+ * Generated on: 2023-03-08 16:44:32
  */
 #include "translateEvents.h"
 
@@ -158,6 +158,8 @@ const char *LOST_BIT_LOCK_PDEC_STRING = "LOST_BIT_LOCK_PDEC";
 const char *TOO_MANY_IRQS_STRING = "TOO_MANY_IRQS";
 const char *POLL_SYSCALL_ERROR_PDEC_STRING = "POLL_SYSCALL_ERROR_PDEC";
 const char *WRITE_SYSCALL_ERROR_PDEC_STRING = "WRITE_SYSCALL_ERROR_PDEC";
+const char *PDEC_RESET_FAILED_STRING = "PDEC_RESET_FAILED";
+const char *OPEN_IRQ_FILE_FAILED_STRING = "OPEN_IRQ_FILE_FAILED";
 const char *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED";
 const char *IMAGE_DOWNLOAD_FAILED_STRING = "IMAGE_DOWNLOAD_FAILED";
 const char *IMAGE_UPLOAD_SUCCESSFUL_STRING = "IMAGE_UPLOAD_SUCCESSFUL";
@@ -255,7 +257,8 @@ const char *REBOOT_HW_STRING = "REBOOT_HW";
 const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE";
 const char *VERSION_INFO_STRING = "VERSION_INFO";
 const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO";
-const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION";
+const char *REBOOT_COUNTER_STRING = "REBOOT_COUNTER";
+const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS";
 const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE";
 const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE";
 const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING";
@@ -263,6 +266,9 @@ const char *PLOC_OVERHEATING_STRING = "PLOC_OVERHEATING";
 const char *OBC_OVERHEATING_STRING = "OBC_OVERHEATING";
 const char *HPA_OVERHEATING_STRING = "HPA_OVERHEATING";
 const char *PLPCDU_OVERHEATING_STRING = "PLPCDU_OVERHEATING";
+const char *TX_TIMER_EXPIRED_STRING = "TX_TIMER_EXPIRED";
+const char *BIT_LOCK_TX_ON_STRING = "BIT_LOCK_TX_ON";
+const char *POSSIBLE_FILE_CORRUPTION_STRING = "POSSIBLE_FILE_CORRUPTION";
 
 const char *translateEvents(Event event) {
   switch ((event & 0xFFFF)) {
@@ -572,6 +578,10 @@ const char *translateEvents(Event event) {
       return POLL_SYSCALL_ERROR_PDEC_STRING;
     case (12409):
       return WRITE_SYSCALL_ERROR_PDEC_STRING;
+    case (12410):
+      return PDEC_RESET_FAILED_STRING;
+    case (12411):
+      return OPEN_IRQ_FILE_FAILED_STRING;
     case (12500):
       return IMAGE_UPLOAD_FAILED_STRING;
     case (12501):
@@ -766,22 +776,30 @@ const char *translateEvents(Event event) {
       return VERSION_INFO_STRING;
     case (14006):
       return CURRENT_IMAGE_INFO_STRING;
+    case (14007):
+      return REBOOT_COUNTER_STRING;
+    case (14008):
+      return INDIVIDUAL_BOOT_COUNTS_STRING;
     case (14100):
-      return POSSIBLE_FILE_CORRUPTION_STRING;
-    case (14200):
       return NO_VALID_SENSOR_TEMPERATURE_STRING;
-    case (14201):
+    case (14101):
       return NO_HEALTHY_HEATER_AVAILABLE_STRING;
-    case (14202):
+    case (14102):
       return SYRLINKS_OVERHEATING_STRING;
-    case (14203):
+    case (14103):
       return PLOC_OVERHEATING_STRING;
-    case (14204):
+    case (14104):
       return OBC_OVERHEATING_STRING;
-    case (14205):
+    case (14105):
       return HPA_OVERHEATING_STRING;
-    case (14206):
+    case (14106):
       return PLPCDU_OVERHEATING_STRING;
+    case (14201):
+      return TX_TIMER_EXPIRED_STRING;
+    case (14202):
+      return BIT_LOCK_TX_ON_STRING;
+    case (14300):
+      return POSSIBLE_FILE_CORRUPTION_STRING;
     default:
       return "UNKNOWN_EVENT";
   }

linux/fsfwconfig/objects/systemObjectList.h

@@ -47,7 +47,6 @@ enum sourceObjects : uint32_t {
   GPIO_IF = 0x49010005,
 
   /* Custom device handler */
-  RW_POLLING_TASK = 0x49020005,
 
   /* 0x54 ('T') for test handlers */
   TEST_TASK = 0x54694269,

linux/fsfwconfig/objects/translateObjects.cpp

@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 159 translations.
- * Generated on: 2023-02-24 16:57:00
+ * Contains 164 translations.
+ * Generated on: 2023-03-08 16:44:32
  */
 #include "translateObjects.h"
 
@@ -85,11 +85,13 @@ const char *ARDUINO_COM_IF_STRING = "ARDUINO_COM_IF";
 const char *GPIO_IF_STRING = "GPIO_IF";
 const char *SCEX_UART_READER_STRING = "SCEX_UART_READER";
 const char *SPI_MAIN_COM_IF_STRING = "SPI_MAIN_COM_IF";
-const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK";
-const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF";
 const char *UART_COM_IF_STRING = "UART_COM_IF";
 const char *I2C_COM_IF_STRING = "I2C_COM_IF";
 const char *CSP_COM_IF_STRING = "CSP_COM_IF";
+const char *ACS_BOARD_POLLING_TASK_STRING = "ACS_BOARD_POLLING_TASK";
+const char *RW_POLLING_TASK_STRING = "RW_POLLING_TASK";
+const char *SPI_RTD_COM_IF_STRING = "SPI_RTD_COM_IF";
+const char *SUS_POLLING_TASK_STRING = "SUS_POLLING_TASK";
 const char *CCSDS_PACKET_DISTRIBUTOR_STRING = "CCSDS_PACKET_DISTRIBUTOR";
 const char *PUS_PACKET_DISTRIBUTOR_STRING = "PUS_PACKET_DISTRIBUTOR";
 const char *TCP_TMTC_SERVER_STRING = "TCP_TMTC_SERVER";
@@ -145,8 +147,11 @@ 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 *IMTQ_ASSY_STRING = "IMTQ_ASSY";
+const char *STR_ASSY_STRING = "STR_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";
@@ -326,16 +331,20 @@ const char *translateObject(object_id_t object) {
       return SCEX_UART_READER_STRING;
     case 0x49020004:
       return SPI_MAIN_COM_IF_STRING;
-    case 0x49020005:
-      return RW_POLLING_TASK_STRING;
-    case 0x49020006:
-      return SPI_RTD_COM_IF_STRING;
     case 0x49030003:
       return UART_COM_IF_STRING;
     case 0x49040002:
       return I2C_COM_IF_STRING;
     case 0x49050001:
       return CSP_COM_IF_STRING;
+    case 0x49060004:
+      return ACS_BOARD_POLLING_TASK_STRING;
+    case 0x49060005:
+      return RW_POLLING_TASK_STRING;
+    case 0x49060006:
+      return SPI_RTD_COM_IF_STRING;
+    case 0x49060007:
+      return SUS_POLLING_TASK_STRING;
     case 0x50000100:
       return CCSDS_PACKET_DISTRIBUTOR_STRING;
     case 0x50000200:
@@ -447,9 +456,15 @@ 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 0x73000008:
+      return IMTQ_ASSY_STRING;
+    case 0x73000009:
+      return STR_ASSY_STRING;
     case 0x73000100:
       return TM_FUNNEL_STRING;
     case 0x73000101:

linux/fsfwconfig/returnvalues/classIds.h

@@ -13,9 +13,8 @@
 namespace CLASS_ID {
 enum {
   CLASS_ID_START = COMMON_CLASS_ID_END,
-  SD_CARD_MANAGER,  // SDMA
-  SCRATCH_BUFFER,   // SCBU
-  CLASS_ID_END      // [EXPORT] : [END]
+  SCRATCH_BUFFER,  // SCBU
+  CLASS_ID_END     // [EXPORT] : [END]
 };
 }
 

linux/ipcore/PdecConfig.cpp

@@ -1,12 +1,120 @@
 #include "PdecConfig.h"
 
+#include "fsfw/filesystem/HasFileSystemIF.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
+#include "pdecconfigdefs.h"
 
-PdecConfig::PdecConfig() { initialize(); }
+PdecConfig::PdecConfig()
+    : localParameterHandler("conf/pdecconfig.json", SdCardManager::instance()) {}
 
 PdecConfig::~PdecConfig() {}
 
-void PdecConfig::initialize() {
+void PdecConfig::setMemoryBaseAddress(uint32_t* memoryBaseAddress_) {
+  memoryBaseAddress = memoryBaseAddress_;
+}
+
+ReturnValue_t PdecConfig::write() {
+  if (memoryBaseAddress == nullptr) {
+    sif::error << "PdecConfig::write: Memory base address not set" << std::endl;
+    return returnvalue::FAILED;
+  }
+  ReturnValue_t result = initializePersistentParameters();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = writeFrameHeaderFirstOctet();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = writeFrameHeaderSecondOctet();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  writeMapConfig();
+  return returnvalue::OK;
+}
+
+ReturnValue_t PdecConfig::initializePersistentParameters() {
+  ReturnValue_t result = localParameterHandler.initialize();
+  if (result == HasFileSystemIF::FILE_DOES_NOT_EXIST) {
+    result = createPersistentConfig();
+    if (result != returnvalue::OK) {
+      return result;
+    }
+  }
+  return result;
+}
+
+ReturnValue_t PdecConfig::createPersistentConfig() {
+  ReturnValue_t result = localParameterHandler.addParameter(
+      pdecconfigdefs::paramkeys::POSITIVE_WINDOW, pdecconfigdefs::defaultvalue::positiveWindow);
+  if (result != returnvalue::OK) {
+    sif::error << "PdecConfig::createPersistentConfig: Failed to set positive window" << std::endl;
+    return result;
+  }
+  result = localParameterHandler.addParameter(pdecconfigdefs::paramkeys::NEGATIVE_WINDOW,
+                                              pdecconfigdefs::defaultvalue::negativeWindow);
+  if (result != returnvalue::OK) {
+    sif::error << "PdecConfig::createPersistentConfig: Failed to set negative window" << std::endl;
+    return result;
+  }
+  return returnvalue::OK;
+}
+
+uint32_t PdecConfig::getImrReg() {
+  return static_cast<uint32_t>(enableNewFarIrq << 2) |
+         static_cast<uint32_t>(enableTcAbortIrq << 1) | static_cast<uint32_t>(enableTcNewIrq);
+}
+
+ReturnValue_t PdecConfig::setPositiveWindow(uint8_t pw) {
+  if (memoryBaseAddress == nullptr) {
+    sif::error << "PdecConfig::setPositiveWindow: Memory base address not set" << std::endl;
+    return returnvalue::FAILED;
+  }
+  ReturnValue_t result =
+      localParameterHandler.updateParameter(pdecconfigdefs::paramkeys::POSITIVE_WINDOW, pw);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  // Rewrite second config word which contains the positive window parameter
+  writeFrameHeaderSecondOctet();
+  return returnvalue::OK;
+}
+
+ReturnValue_t PdecConfig::setNegativeWindow(uint8_t nw) {
+  if (memoryBaseAddress == nullptr) {
+    sif::error << "PdecConfig::setPositiveWindow: Memory base address not set" << std::endl;
+    return returnvalue::FAILED;
+  }
+  ReturnValue_t result =
+      localParameterHandler.updateParameter(pdecconfigdefs::paramkeys::NEGATIVE_WINDOW, nw);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  // Rewrite second config word which contains the negative window parameter
+  writeFrameHeaderSecondOctet();
+  return returnvalue::OK;
+}
+
+ReturnValue_t PdecConfig::getPositiveWindow(uint8_t& positiveWindow) {
+  ReturnValue_t result =
+      localParameterHandler.getValue(pdecconfigdefs::paramkeys::POSITIVE_WINDOW, positiveWindow);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t PdecConfig::getNegativeWindow(uint8_t& negativeWindow) {
+  ReturnValue_t result =
+      localParameterHandler.getValue(pdecconfigdefs::paramkeys::NEGATIVE_WINDOW, negativeWindow);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t PdecConfig::writeFrameHeaderFirstOctet() {
   uint32_t word = 0;
   word |= (VERSION_ID << 30);
 
@@ -19,24 +127,65 @@ void PdecConfig::initialize() {
   word |= (SPACECRAFT_ID << 16);
   word |= (VIRTUAL_CHANNEL << 10);
   word |= (DUMMY_BITS << 8);
-  word |= POSITIVE_WINDOW;
-  configWords[0] = word;
+  uint8_t positiveWindow = 0;
+  ReturnValue_t result =
+      localParameterHandler.getValue(pdecconfigdefs::paramkeys::POSITIVE_WINDOW, positiveWindow);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  word |= static_cast<uint32_t>(positiveWindow);
+  *(memoryBaseAddress + FRAME_HEADER_OFFSET) = word;
+  return returnvalue::OK;
+}
+
+ReturnValue_t PdecConfig::writeFrameHeaderSecondOctet() {
+  uint8_t negativeWindow = 0;
+  ReturnValue_t result =
+      localParameterHandler.getValue(pdecconfigdefs::paramkeys::NEGATIVE_WINDOW, negativeWindow);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  uint32_t word = 0;
   word = 0;
-  word |= (static_cast<uint32_t>(NEGATIVE_WINDOW) << 24);
+  word |= (static_cast<uint32_t>(negativeWindow) << 24);
   word |= (HIGH_AU_MAP_ID << 16);
   word |= (ENABLE_DERANDOMIZER << 8);
-  configWords[1] = word;
+  *(memoryBaseAddress + FRAME_HEADER_OFFSET + 1) = word;
+  return returnvalue::OK;
 }
 
-uint32_t PdecConfig::getConfigWord(uint8_t wordNo) {
-  if (wordNo >= CONFIG_WORDS_NUM) {
-    sif::error << "PdecConfig::getConfigWord: Invalid word number" << std::endl;
-    return 0;
+void PdecConfig::writeMapConfig() {
+  // Configure all MAP IDs as invalid
+  for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) {
+    *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + idx / 4) =
+        NO_DESTINATION << 24 | NO_DESTINATION << 16 | NO_DESTINATION << 8 | NO_DESTINATION;
+  }
+
+  // All TCs with MAP ID 7 will be routed to the PM module (can then be read from memory)
+  uint8_t routeToPm = calcMapAddrEntry(PM_BUFFER);
+  *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + 1) =
+      (NO_DESTINATION << 24) | (NO_DESTINATION << 16) | (NO_DESTINATION << 8) | routeToPm;
+
+  // Write map id clock frequencies
+  for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) {
+    *(memoryBaseAddress + MAP_CLK_FREQ_OFFSET + idx / 4) =
+        MAP_CLK_FREQ << 24 | MAP_CLK_FREQ << 16 | MAP_CLK_FREQ << 8 | MAP_CLK_FREQ;
   }
-  return configWords[wordNo];
 }
 
-uint32_t PdecConfig::getImrReg() {
-  return static_cast<uint32_t>(enableNewFarIrq << 2) |
-         static_cast<uint32_t>(enableTcAbortIrq << 1) | static_cast<uint32_t>(enableTcNewIrq);
+uint8_t PdecConfig::calcMapAddrEntry(uint8_t moduleId) {
+  uint8_t lutEntry = 0;
+  uint8_t parity = getOddParity(moduleId | (1 << VALID_POSITION));
+  lutEntry = (parity << PARITY_POSITION) | (1 << VALID_POSITION) | moduleId;
+  return lutEntry;
+}
+
+uint8_t PdecConfig::getOddParity(uint8_t number) {
+  uint8_t parityBit = 0;
+  uint8_t countBits = 0;
+  for (unsigned int idx = 0; idx < sizeof(number) * 8; idx++) {
+    countBits += (number >> idx) & 0x1;
+  }
+  parityBit = ~(countBits & 0x1) & 0x1;
+  return parityBit;
 }

linux/ipcore/PdecConfig.h

@@ -1,30 +1,53 @@
 #ifndef LINUX_OBC_PDECCONFIG_H_
 #define LINUX_OBC_PDECCONFIG_H_
 
-#include <cstring>
+#include <string>
 
+#include "bsp_q7s/fs/SdCardManager.h"
+#include "bsp_q7s/memory/LocalParameterHandler.h"
 #include "fsfw/returnvalues/returnvalue.h"
+#include "pdec.h"
 
 /**
  * @brief   This class generates the configuration words for the configuration memory of the PDEC
  *          IP Cores.
  *
- * @details Fields are initialized according to pecification in PDEC datasheet section 6.11.3.1
+ * @details Fields are initialized according to specification in PDEC datasheet section 6.11.3.1
  *          PROM usage.
  *
  * @author  J. Meier
  */
 class PdecConfig {
  public:
+  /**
+   * @brief	Constructor
+   */
   PdecConfig();
   virtual ~PdecConfig();
 
   /**
-   * @brief   Returns the configuration word by specifying the position.
+   * @brief	Sets the memory base address pointer
+   */
+  void setMemoryBaseAddress(uint32_t* memoryBaseAddress_);
+
+  /**
+   * @brief	Will write the config to the PDEC configuration memory. New config
+   * 				becomes active after resetting PDEC.
+   */
+  ReturnValue_t write();
+
+  /**
+   * @brief	Returns the value to write to the interrupt mask register. This
+   * 				value defines which interrupts should be enabled/disabled.
    */
-  uint32_t getConfigWord(uint8_t wordNo);
   uint32_t getImrReg();
 
+  ReturnValue_t setPositiveWindow(uint8_t pw);
+  ReturnValue_t setNegativeWindow(uint8_t nw);
+
+  ReturnValue_t getPositiveWindow(uint8_t& positiveWindow);
+  ReturnValue_t getNegativeWindow(uint8_t& negativeWindow);
+
  private:
   // TC transfer frame configuration parameters
   static const uint8_t VERSION_ID = 0;
@@ -36,21 +59,73 @@ class PdecConfig {
   static const uint8_t RESERVED_FIELD_A = 0;
   static const uint16_t SPACECRAFT_ID = 0x3DC;
   static const uint16_t DUMMY_BITS = 0;
-  // Parameters to control the FARM for AD frames
-  // Set here for future use
-  static const uint8_t POSITIVE_WINDOW = 10;
-  static const uint8_t NEGATIVE_WINDOW = 151;
   static const uint8_t HIGH_AU_MAP_ID = 0xF;
   static const uint8_t ENABLE_DERANDOMIZER = 1;
 
   static const uint8_t CONFIG_WORDS_NUM = 2;
 
+  // 0x200 / 4 = 0x80
+  static const uint32_t FRAME_HEADER_OFFSET = 0x80;
+
+  static const uint32_t MAP_ADDR_LUT_OFFSET = 0xA0;
+  static const uint32_t MAP_CLK_FREQ_OFFSET = 0x90;
+  // MAP clock frequency. Must be a value between 1 and 13 otherwise the TC segment will be
+  // discarded
+  static const uint8_t MAP_CLK_FREQ = 2;
+
+  static const uint8_t MAX_MAP_ADDR = 63;
+  // Writing this to the map address in the look up table will invalidate a MAP ID.
+  static const uint8_t NO_DESTINATION = 0;
+  static const uint8_t VALID_POSITION = 6;
+  static const uint8_t PARITY_POSITION = 7;
+
+  /**
+   * TCs with map addresses (also know as Map IDs) assigned to this channel will be stored in
+   * the PDEC memory.
+   */
+  static const uint8_t PM_BUFFER = 7;
+
+  uint32_t* memoryBaseAddress = nullptr;
+
+  // Pointer to object providing access to persistent configuration parameters
+  LocalParameterHandler localParameterHandler;
+
   uint32_t configWords[CONFIG_WORDS_NUM];
   bool enableTcNewIrq = true;
   bool enableTcAbortIrq = true;
   bool enableNewFarIrq = true;
 
-  void initialize();
+  ReturnValue_t initializePersistentParameters();
+  /**
+   * @brief	If the json file containing the persistent config parameters does
+   * 				not exist it will be created here.
+   */
+  ReturnValue_t createPersistentConfig();
+
+  ReturnValue_t writeFrameHeaderFirstOctet();
+  ReturnValue_t writeFrameHeaderSecondOctet();
+  void writeMapConfig();
+
+  /**
+   * @brief   This function calculates the entry for the configuration of the MAP ID routing.
+   *
+   * @param mapAddr   The MAP ID to configure
+   * @param moduleId  The destination module where all TCs with the map id mapAddr will be routed
+   *                  to.
+   *
+   * @details The PDEC has different modules where the TCs can be routed to. A lookup table is
+   *          used which links the MAP ID field to the destination module. The entry for this
+   *          lookup table is created by this function and must be stored in the configuration
+   *          memory region of the PDEC. The entry has a specific format
+   */
+  uint8_t calcMapAddrEntry(uint8_t moduleId);
+
+  /**
+   * @brief   This functions calculates the odd parity of the bits in number.
+   *
+   * @param number    The number from which to calculate the odd parity.
+   */
+  uint8_t getOddParity(uint8_t number);
 };
 
 #endif /* LINUX_OBC_PDECCONFIG_H_ */

linux/ipcore/PdecHandler.cpp

@@ -29,7 +29,8 @@ PdecHandler::PdecHandler(object_id_t objectId, object_id_t tcDestinationId,
       gpioComIF(gpioComIF),
       pdecReset(pdecReset),
       actionHelper(this, nullptr),
-      uioNames(names) {
+      uioNames(names),
+      paramHelper(this) {
   auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
   commandQueue = QueueFactory::instance()->createMessageQueue(
       QUEUE_SIZE, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
@@ -62,6 +63,8 @@ ReturnValue_t PdecHandler::initialize() {
   result = configMemMapper.getMappedAdress(&memoryBaseAddress, UioMapper::Permissions::READ_WRITE);
   if (result != returnvalue::OK) {
     return ObjectManagerIF::CHILD_INIT_FAILED;
+  } else {
+    pdecConfig.setMemoryBaseAddress(memoryBaseAddress);
   }
   UioMapper ramMapper(uioNames.ramMemory);
   result = ramMapper.getMappedAdress(&ramBaseAddress, UioMapper::Permissions::READ_WRITE);
@@ -73,12 +76,34 @@ ReturnValue_t PdecHandler::initialize() {
     sif::error << "Can not use IRQ mode if IRQ UIO name is invalid" << std::endl;
     return returnvalue::FAILED;
   }
-  PdecConfig pdecConfig;
-  writePdecConfigDuringReset(pdecConfig);
+
+  result = actionHelper.initialize(commandQueue);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+
+  result = paramHelper.initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+
+  return returnvalue::OK;
+}
+
+ReturnValue_t PdecHandler::firstLoop() {
+  ReturnValue_t result = pdecConfig.write();
+  if (result != returnvalue::OK) {
+    if (result == LocalParameterHandler::SD_NOT_READY) {
+      return result;
+    } else {
+      sif::error << "PdecHandler::firstLoop: Failed to write PDEC config" << std::endl;
+    }
+    return returnvalue::FAILED;
+  }
 
   result = releasePdec();
   if (result != returnvalue::OK) {
-    return ObjectManagerIF::CHILD_INIT_FAILED;
+    return returnvalue::FAILED;
   }
 
   // This configuration must be done while the PDEC is not held in reset.
@@ -86,11 +111,12 @@ ReturnValue_t PdecHandler::initialize() {
     // Configure interrupt mask register to enable interrupts
     *(registerBaseAddress + PDEC_IMR_OFFSET) = pdecConfig.getImrReg();
   }
-  result = actionHelper.initialize(commandQueue);
+  result = resetFarStatFlag();
   if (result != returnvalue::OK) {
+    // Requires reconfiguration and reinitialization of PDEC
+    triggerEvent(INVALID_FAR);
     return result;
   }
-
   return returnvalue::OK;
 }
 
@@ -104,26 +130,28 @@ ReturnValue_t PdecHandler::performOperation(uint8_t operationCode) {
 }
 
 ReturnValue_t PdecHandler::polledOperation() {
-  ReturnValue_t result = returnvalue::OK;
   readCommandQueue();
 
   switch (state) {
-    case State::INIT:
-      resetFarStatFlag();
-      if (result != returnvalue::OK) {
-        // Requires reconfiguration and reinitialization of PDEC
-        triggerEvent(INVALID_FAR);
-        state = State::WAIT_FOR_RECOVERY;
-        break;
-      }
-      state = State::RUNNING;
+    case State::INIT: {
+      handleInitState();
       break;
-    case State::RUNNING:
+    }
+    case State::RUNNING: {
       if (newTcReceived()) {
         handleNewTc();
       }
       checkLocks();
       break;
+    }
+    case State::PDEC_RESET: {
+      ReturnValue_t result = pdecToReset();
+      if (result != returnvalue::OK) {
+        triggerEvent(PDEC_RESET_FAILED);
+      }
+      state = State::INIT;
+      break;
+    }
     case State::WAIT_FOR_RECOVERY:
       break;
     default:
@@ -137,13 +165,7 @@ 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 = open(uioNames.irq, O_RDWR);
-  if (fd < 0) {
-    sif::error << "PdecHandler::irqOperation: Opening UIO IRQ file" << uioNames.irq << " failed"
-               << std::endl;
-    return returnvalue::FAILED;
-  }
-
+  int fd = -1;
   // Used to unmask IRQ
   uint32_t info = 1;
 
@@ -158,18 +180,23 @@ ReturnValue_t PdecHandler::irqOperation() {
     info = 1;
     readCommandQueue();
     switch (state) {
-      case State::INIT:
-        result = resetFarStatFlag();
-        if (result != returnvalue::OK) {
-          // Requires reconfiguration and reinitialization of PDEC
-          triggerEvent(INVALID_FAR);
-          state = State::WAIT_FOR_RECOVERY;
-          return result;
+      case State::INIT: {
+        result = handleInitState();
+        if (result == returnvalue::OK) {
+          openIrqFile(&fd);
         }
-        state = State::RUNNING;
-        checkLocks();
         break;
+      }
+      case State::PDEC_RESET: {
+        result = pdecToReset();
+        if (result != returnvalue::OK) {
+          triggerEvent(PDEC_RESET_FAILED);
+        }
+        state = State::INIT;
+        break;
+      }
       case State::RUNNING: {
+        checkLocks();
         checkAndHandleIrqs(fd, info);
         break;
       }
@@ -188,6 +215,38 @@ ReturnValue_t PdecHandler::irqOperation() {
   return returnvalue::OK;
 }
 
+ReturnValue_t PdecHandler::handleInitState() {
+  ReturnValue_t result = firstLoop();
+  if (result != returnvalue::OK) {
+    if (result == LocalParameterHandler::SD_NOT_READY) {
+      TaskFactory::delayTask(400);
+      if (initTries == MAX_INIT_TRIES) {
+        sif::error << "PdecHandler::handleInitState: SD card never "
+                      "becomes ready"
+                   << std::endl;
+        state = State::WAIT_FOR_RECOVERY;
+      } else {
+        state = State::INIT;
+      }
+      return result;
+    }
+    state = State::WAIT_FOR_RECOVERY;
+    return result;
+  }
+  state = State::RUNNING;
+  return returnvalue::OK;
+}
+
+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;
+  }
+}
+
 ReturnValue_t PdecHandler::checkAndHandleIrqs(int fd, uint32_t& info) {
   ssize_t nb = write(fd, &info, sizeof(info));
   if (nb != static_cast<ssize_t>(sizeof(info))) {
@@ -201,7 +260,6 @@ ReturnValue_t PdecHandler::checkAndHandleIrqs(int fd, uint32_t& info) {
   int ret = poll(&fds, 1, IRQ_TIMEOUT_MS);
   if (ret == 0) {
     // No TCs for timeout period
-    checkLocks();
     genericCheckCd.resetTimer();
     resetIrqLimiters();
   } else if (ret >= 1) {
@@ -228,7 +286,6 @@ ReturnValue_t PdecHandler::checkAndHandleIrqs(int fd, uint32_t& info) {
       static_cast<void>(dummy);
 
       if (genericCheckCd.hasTimedOut()) {
-        checkLocks();
         genericCheckCd.resetTimer();
         if (interruptWindowCd.hasTimedOut()) {
           if (interruptCounter >= MAX_ALLOWED_IRQS_PER_WINDOW) {
@@ -254,17 +311,21 @@ ReturnValue_t PdecHandler::checkAndHandleIrqs(int fd, uint32_t& info) {
 }
 
 void PdecHandler::readCommandQueue(void) {
-  CommandMessage commandMessage;
+  CommandMessage message;
   ReturnValue_t result = returnvalue::FAILED;
 
-  result = commandQueue->receiveMessage(&commandMessage);
+  result = commandQueue->receiveMessage(&message);
   if (result == returnvalue::OK) {
-    result = actionHelper.handleActionMessage(&commandMessage);
+    result = actionHelper.handleActionMessage(&message);
+    if (result == returnvalue::OK) {
+      return;
+    }
+    result = paramHelper.handleParameterMessage(&message);
     if (result == returnvalue::OK) {
       return;
     }
     CommandMessage reply;
-    reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND, commandMessage.getCommand());
+    reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND, message.getCommand());
     commandQueue->reply(&reply);
     return;
   }
@@ -272,26 +333,69 @@ void PdecHandler::readCommandQueue(void) {
 
 MessageQueueId_t PdecHandler::getCommandQueue() const { return commandQueue->getId(); }
 
-void PdecHandler::writePdecConfigDuringReset(PdecConfig& pdecConfig) {
-  *(memoryBaseAddress + FRAME_HEADER_OFFSET) = pdecConfig.getConfigWord(0);
-  *(memoryBaseAddress + FRAME_HEADER_OFFSET + 1) = pdecConfig.getConfigWord(1);
-
-  // Configure all MAP IDs as invalid
-  for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) {
-    *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + idx / 4) =
-        NO_DESTINATION << 24 | NO_DESTINATION << 16 | NO_DESTINATION << 8 | NO_DESTINATION;
+ReturnValue_t PdecHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
+                                         const uint8_t* data, size_t size) {
+  switch (actionId) {
+    case PRINT_CLCW:
+      printClcw();
+      return EXECUTION_FINISHED;
+    case PRINT_PDEC_MON:
+      printPdecMon();
+      return EXECUTION_FINISHED;
+    default:
+      return COMMAND_NOT_IMPLEMENTED;
   }
+}
 
-  // All TCs with MAP ID 7 will be routed to the PM module (can then be read from memory)
-  uint8_t routeToPm = calcMapAddrEntry(PM_BUFFER);
-  *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + 1) =
-      (NO_DESTINATION << 24) | (NO_DESTINATION << 16) | (NO_DESTINATION << 8) | routeToPm;
-
-  // Write map id clock frequencies
-  for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) {
-    *(memoryBaseAddress + MAP_CLK_FREQ_OFFSET + idx / 4) =
-        MAP_CLK_FREQ << 24 | MAP_CLK_FREQ << 16 | MAP_CLK_FREQ << 8 | MAP_CLK_FREQ;
+ReturnValue_t PdecHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
+                                        ParameterWrapper* parameterWrapper,
+                                        const ParameterWrapper* newValues, uint16_t startAtIndex) {
+  if ((domainId == 0) and (uniqueIdentifier == ParameterId::POSITIVE_WINDOW)) {
+    uint8_t newVal = 0;
+    ReturnValue_t result = newValues->getElement(&newVal);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    uint8_t positiveWindow = 0;
+    result = pdecConfig.getPositiveWindow(positiveWindow);
+    if (result != returnvalue::OK) {
+      sif::warning << "PdecHandler::getParameter: Failed to get positive window from pdec config"
+                   << std::endl;
+      return returnvalue::FAILED;
+    }
+    parameterWrapper->set(positiveWindow);
+    result = pdecConfig.setPositiveWindow(newVal);
+    if (result != returnvalue::OK) {
+      sif::warning << "PdecHandler::getParameter: Failed to set positive window" << std::endl;
+      return returnvalue::FAILED;
+    }
+    // PDEC needs reset to apply this parameter change
+    state = State::PDEC_RESET;
+    return returnvalue::OK;
+  } else if ((domainId == 0) and (uniqueIdentifier == ParameterId::NEGATIVE_WINDOW)) {
+    uint8_t newVal = 0;
+    ReturnValue_t result = newValues->getElement(&newVal);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    uint8_t negativeWindow = 0;
+    result = pdecConfig.getNegativeWindow(negativeWindow);
+    if (result != returnvalue::OK) {
+      sif::warning << "PdecHandler::getParameter: Failed to get negative window from pdec config"
+                   << std::endl;
+      return returnvalue::FAILED;
+    }
+    parameterWrapper->set(negativeWindow);
+    result = pdecConfig.setNegativeWindow(newVal);
+    if (result != returnvalue::OK) {
+      sif::warning << "PdecHandler::getParameter: Failed to set negative window" << std::endl;
+      return returnvalue::FAILED;
+    }
+    // PDEC needs reset to apply this parameter change
+    state = State::PDEC_RESET;
+    return returnvalue::OK;
   }
+  return returnvalue::OK;
 }
 
 ReturnValue_t PdecHandler::resetFarStatFlag() {
@@ -320,6 +424,17 @@ ReturnValue_t PdecHandler::releasePdec() {
   return result;
 }
 
+ReturnValue_t PdecHandler::pdecToReset() {
+  ReturnValue_t result = returnvalue::OK;
+  result = gpioComIF->pullLow(pdecReset);
+  if (result != returnvalue::OK) {
+    sif::error << "PdecHandler::pdecToReset: Failed to pull PDEC reset line"
+                  " to low"
+               << std::endl;
+  }
+  return result;
+}
+
 bool PdecHandler::newTcReceived() {
   uint32_t pdecFar = readFar();
 
@@ -557,23 +672,6 @@ void PdecHandler::printTC(uint32_t tcLength) {
   sif::info << tcSegmentStream.str() << std::endl;
 }
 
-uint8_t PdecHandler::calcMapAddrEntry(uint8_t moduleId) {
-  uint8_t lutEntry = 0;
-  uint8_t parity = getOddParity(moduleId | (1 << VALID_POSITION));
-  lutEntry = (parity << PARITY_POSITION) | (1 << VALID_POSITION) | moduleId;
-  return lutEntry;
-}
-
-uint8_t PdecHandler::getOddParity(uint8_t number) {
-  uint8_t parityBit = 0;
-  uint8_t countBits = 0;
-  for (unsigned int idx = 0; idx < sizeof(number) * 8; idx++) {
-    countBits += (number >> idx) & 0x1;
-  }
-  parityBit = ~(countBits & 0x1) & 0x1;
-  return parityBit;
-}
-
 uint32_t PdecHandler::getClcw() { return *(registerBaseAddress + PDEC_CLCW_OFFSET); }
 
 uint32_t PdecHandler::getPdecMon() { return *(registerBaseAddress + PDEC_MON_OFFSET); }
@@ -664,17 +762,3 @@ std::string PdecHandler::getMonStatusString(uint32_t status) {
       break;
   }
 }
-
-ReturnValue_t PdecHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
-                                         const uint8_t* data, size_t size) {
-  switch (actionId) {
-    case PRINT_CLCW:
-      printClcw();
-      return EXECUTION_FINISHED;
-    case PRINT_PDEC_MON:
-      printPdecMon();
-      return EXECUTION_FINISHED;
-    default:
-      return COMMAND_NOT_IMPLEMENTED;
-  }
-}

linux/ipcore/PdecHandler.h

@@ -9,6 +9,8 @@
 #include "fsfw/action/ActionHelper.h"
 #include "fsfw/action/HasActionsIF.h"
 #include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/parameters/ParameterHelper.h"
+#include "fsfw/parameters/ReceivesParameterMessagesIF.h"
 #include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw/storagemanager/StorageManagerIF.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
@@ -41,7 +43,10 @@ struct UioNames {
  *
  * @author  J. Meier
  */
-class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasActionsIF {
+class PdecHandler : public SystemObject,
+                    public ExecutableObjectIF,
+                    public HasActionsIF,
+                    public ReceivesParameterMessagesIF {
  public:
   static constexpr dur_millis_t IRQ_TIMEOUT_MS = 500;
 
@@ -70,6 +75,10 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
   ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                               const uint8_t* data, size_t size) override;
 
+  ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
+                             ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
+                             uint16_t startAtIndex) override;
+
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PDEC_HANDLER;
 
   //! [EXPORT] : [COMMENT] Frame acceptance report signals an invalid frame
@@ -92,7 +101,11 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
   static constexpr Event POLL_SYSCALL_ERROR_PDEC =
       event::makeEvent(SUBSYSTEM_ID, 8, severity::MEDIUM);
   static constexpr Event WRITE_SYSCALL_ERROR_PDEC =
-      event::makeEvent(SUBSYSTEM_ID, 9, severity::MEDIUM);
+      event::makeEvent(SUBSYSTEM_ID, 9, severity::HIGH);
+  //! [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);
 
  private:
   static const uint8_t INTERFACE_ID = CLASS_ID::PDEC_HANDLER;
@@ -143,9 +156,6 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
   static const int REGISTER_MAP_SIZE = 0x4000;
 #endif /* BOARD_TE0720 == 1 */
 
-  // 0x200 / 4 = 0x80
-  static const uint32_t FRAME_HEADER_OFFSET = 0x80;
-
   static const size_t MAX_TC_SEGMENT_SIZE = 1017;
   static const uint8_t MAP_ID_MASK = 0x3F;
 
@@ -155,15 +165,6 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
   static const uint32_t PHYSICAL_RAM_BASE_ADDRESS = 0x26000000;
 #endif
 
-  static const uint32_t MAP_ADDR_LUT_OFFSET = 0xA0;
-  static const uint32_t MAP_CLK_FREQ_OFFSET = 0x90;
-
-  static const uint8_t MAX_MAP_ADDR = 63;
-  // Writing this to the map address in the look up table will invalidate a MAP ID.
-  static const uint8_t NO_DESTINATION = 0;
-  static const uint8_t VALID_POSITION = 6;
-  static const uint8_t PARITY_POSITION = 7;
-
   // Expected value stored in FAR register after reset
   static const uint32_t FAR_RESET = 0x7FE0;
 
@@ -172,15 +173,15 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
   static const uint32_t NO_RF_MASK = 0x8000;
   static const uint32_t NO_BITLOCK_MASK = 0x4000;
 
-  /**
-   * TCs with map addresses (also know as Map IDs) assigned to this channel will be stored in
-   * the PDEC memory.
-   */
-  static const uint8_t PM_BUFFER = 7;
+  static const uint32_t MAX_INIT_TRIES = 20;
 
-  // MAP clock frequency. Must be a value between 1 and 13 otherwise the TC segment will be
-  // discarded
-  static const uint8_t MAP_CLK_FREQ = 2;
+  class ParameterId {
+   public:
+    // ID of the parameter to update the positive window of AD frames
+    static const uint8_t POSITIVE_WINDOW = 0;
+    // ID of the parameter to update the negative window of AD frames
+    static const uint8_t NEGATIVE_WINDOW = 1;
+  };
 
   static constexpr uint32_t MAX_ALLOWED_IRQS_PER_WINDOW = 800;
 
@@ -206,7 +207,7 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
     INCORRECT_BC_CC
   };
 
-  enum class State : uint8_t { INIT, RUNNING, WAIT_FOR_RECOVERY };
+  enum class State : uint8_t { INIT, PDEC_RESET, RUNNING, WAIT_FOR_RECOVERY };
 
   static uint32_t CURRENT_FAR;
 
@@ -259,6 +260,20 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
 
   UioNames uioNames;
 
+  ParameterHelper paramHelper;
+
+  PdecConfig pdecConfig;
+
+  uint32_t initTries = 0;
+
+  /**
+   * @brief	Performs initialization stuff which must be performed in first
+   * 				loop of running task
+   *
+   * @return OK if successful, otherwise FAILED
+   */
+  ReturnValue_t firstLoop();
+
   /**
    * @brief   Reads and handles messages stored in the commandQueue
    */
@@ -266,6 +281,8 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
 
   ReturnValue_t polledOperation();
   ReturnValue_t irqOperation();
+  ReturnValue_t handleInitState();
+  void openIrqFile(int* fd);
   ReturnValue_t checkAndHandleIrqs(int fd, uint32_t& info);
 
   uint32_t readFar();
@@ -291,6 +308,14 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
    */
   ReturnValue_t releasePdec();
 
+  /**
+   * @brief Will set PDEC in reset state. Use releasePdec() to release PDEC
+   * 				from reset state
+   *
+   * @return OK if successful, otherwise error return value
+   */
+  ReturnValue_t pdecToReset();
+
   /**
    * @brief   Reads the FAR register and checks if a new TC has been received.
    */
@@ -354,13 +379,6 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
    */
   uint8_t calcMapAddrEntry(uint8_t moduleId);
 
-  /**
-   * @brief   This functions calculates the odd parity of the bits in number.
-   *
-   * @param number    The number from which to calculate the odd parity.
-   */
-  uint8_t getOddParity(uint8_t number);
-
   /**
    * brief    Returns the 32-bit wide communication link control word (CLCW)
    */

linux/ipcore/pdecconfigdefs.h

@@ -0,0 +1,20 @@
+#ifndef LINUX_IPCORE_PDECCONFIGDEFS_H_
+#define LINUX_IPCORE_PDECCONFIGDEFS_H_
+
+#include <string>
+
+namespace pdecconfigdefs {
+
+namespace paramkeys {
+static const std::string POSITIVE_WINDOW = "positive_window";
+static const std::string NEGATIVE_WINDOW = "negattive_window";
+}  // namespace paramkeys
+
+namespace defaultvalue {
+static const uint8_t positiveWindow = 10;
+static const uint8_t negativeWindow = 151;
+}  // namespace defaultvalue
+
+}  // namespace pdecconfigdefs
+
+#endif /* LINUX_IPCORE_PDECCONFIGDEFS_H_ */

mission/acsDefs.h

@@ -18,6 +18,8 @@ enum AcsMode : Mode_t {
   PTG_INERTIAL = 16,
 };
 
+// static constexpr uint8_t ACS_SYSTEM_DETUMBLE_SUBMODE = 1;
+
 static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
 //!< The limits for the rotation in safe mode were violated.
 static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);

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

mission/config/configfile.h

@@ -0,0 +1,9 @@
+#ifndef MISSION_CONFIG_CONFIGFILE_H_
+#define MISSION_CONFIG_CONFIGFILE_H_
+
+namespace configfile {
+// Name of global config file relative to currently mounted SD card
+static const char sdrelative[] = "config/global_config.json";
+}  // namespace configfile
+
+#endif /* MISSION_CONFIG_CONFIGFILE_H_ */

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;

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);
@@ -195,9 +198,9 @@ void AcsController::performSafe() {
 
   updateCtrlValData(errAng);
   updateActuatorCmdData(cmdDipolMtqs);
-  //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
-  //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
-  //                   acsParameters.rwHandlingParameters.rampTime);
+  // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2]/*500, 500, 500*/,
+  //                  acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
+  //                  acsParameters.rwHandlingParameters.rampTime);
 }
 
 void AcsController::performDetumble() {
@@ -261,8 +264,9 @@ void AcsController::performPointingCtrl() {
       triggerEvent(acs::MEKF_INVALID_INFO);
       mekfInvalidFlag = true;
     }
-    if (mekfInvalidCounter > 4) {
-      triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION);
+    if (mekfInvalidCounter == 5) {
+      // Trigger this so STR FDIR can set the device faulty.
+      EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
     }
     mekfInvalidCounter++;
     //    commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration,
@@ -278,7 +282,7 @@ void AcsController::performPointingCtrl() {
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
-    if (multipleRwUnavailableCounter > 4) {
+    if (multipleRwUnavailableCounter == 5) {
       triggerEvent(acs::MULTIPLE_RW_INVALID);
     }
     multipleRwUnavailableCounter++;
@@ -429,7 +433,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);
   }
@@ -661,7 +666,6 @@ void AcsController::announceMode(bool recursive) {
 }
 
 void AcsController::copyMgmData() {
-  ACS::SensorValues sensorValues;
   {
     PoolReadGuard pg(&sensorValues.mgm0Lis3Set);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -806,7 +810,6 @@ void AcsController::copySusData() {
 }
 
 void AcsController::copyGyrData() {
-  ACS::SensorValues sensorValues;
   {
     PoolReadGuard pg(&sensorValues.gyr0AdisSet);
     if (pg.getReadResult() == returnvalue::OK) {

mission/controller/AcsController.h

@@ -4,13 +4,14 @@
 #include <eive/objects.h>
 #include <fsfw/controller/ExtendedControllerBase.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/health/HealthTable.h>
 #include <fsfw/parameters/ParameterHelper.h>
 #include <fsfw/parameters/ReceivesParameterMessagesIF.h>
 #include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
 #include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
-#include <mission/devices/devicedefinitions/SusDefinitions.h>
 #include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
+#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
 #include <mission/trace.h>
 
 #include "acs/ActuatorCmd.h"

mission/controller/ThermalController.cpp

@@ -3,13 +3,13 @@
 #include <bsp_q7s/core/CoreDefinitions.h>
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/thermal/ThermalComponentIF.h>
-#include <fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h>
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
 #include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
 #include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
 #include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
-#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
 #include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 #include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
@@ -474,8 +474,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg0(&susSet0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg0.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = susSet0.temperatureCelcius.value;
-      susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.setValid(susSet0.temperatureCelcius.isValid());
+      susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = susSet0.tempC.value;
+      susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.setValid(susSet0.tempC.isValid());
       if (not susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.isValid()) {
         susTemperatures.sus_0_n_loc_xfyfzm_pt_xf.value = INVALID_TEMPERATURE;
       }
@@ -485,8 +485,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg1(&susSet1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg1.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = susSet1.temperatureCelcius.value;
-      susTemperatures.sus_6_r_loc_xfybzm_pt_xf.setValid(susSet1.temperatureCelcius.isValid());
+      susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = susSet1.tempC.value;
+      susTemperatures.sus_6_r_loc_xfybzm_pt_xf.setValid(susSet1.tempC.isValid());
       if (not susTemperatures.sus_6_r_loc_xfybzm_pt_xf.isValid()) {
         susTemperatures.sus_6_r_loc_xfybzm_pt_xf.value = INVALID_TEMPERATURE;
       }
@@ -496,8 +496,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg2(&susSet2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg2.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = susSet2.temperatureCelcius.value;
-      susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.setValid(susSet2.temperatureCelcius.isValid());
+      susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = susSet2.tempC.value;
+      susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.setValid(susSet2.tempC.isValid());
       if (not susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.isValid()) {
         susTemperatures.sus_1_n_loc_xbyfzm_pt_xb.value = INVALID_TEMPERATURE;
       }
@@ -507,8 +507,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg3(&susSet3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg3.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = susSet3.temperatureCelcius.value;
-      susTemperatures.sus_7_r_loc_xbybzm_pt_xb.setValid(susSet3.temperatureCelcius.isValid());
+      susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = susSet3.tempC.value;
+      susTemperatures.sus_7_r_loc_xbybzm_pt_xb.setValid(susSet3.tempC.isValid());
       if (not susTemperatures.sus_7_r_loc_xbybzm_pt_xb.isValid()) {
         susTemperatures.sus_7_r_loc_xbybzm_pt_xb.value = INVALID_TEMPERATURE;
       }
@@ -518,8 +518,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg4(&susSet4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg4.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = susSet4.temperatureCelcius.value;
-      susTemperatures.sus_2_n_loc_xfybzb_pt_yb.setValid(susSet4.temperatureCelcius.isValid());
+      susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = susSet4.tempC.value;
+      susTemperatures.sus_2_n_loc_xfybzb_pt_yb.setValid(susSet4.tempC.isValid());
       if (not susTemperatures.sus_2_n_loc_xfybzb_pt_yb.isValid()) {
         susTemperatures.sus_2_n_loc_xfybzb_pt_yb.value = INVALID_TEMPERATURE;
       }
@@ -529,8 +529,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg5(&susSet5, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg5.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = susSet5.temperatureCelcius.value;
-      susTemperatures.sus_8_r_loc_xbybzb_pt_yb.setValid(susSet5.temperatureCelcius.isValid());
+      susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = susSet5.tempC.value;
+      susTemperatures.sus_8_r_loc_xbybzb_pt_yb.setValid(susSet5.tempC.isValid());
       if (not susTemperatures.sus_8_r_loc_xbybzb_pt_yb.isValid()) {
         susTemperatures.sus_8_r_loc_xbybzb_pt_yb.value = INVALID_TEMPERATURE;
       }
@@ -540,8 +540,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg6(&susSet6, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg6.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = susSet6.temperatureCelcius.value;
-      susTemperatures.sus_3_n_loc_xfybzf_pt_yf.setValid(susSet6.temperatureCelcius.isValid());
+      susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = susSet6.tempC.value;
+      susTemperatures.sus_3_n_loc_xfybzf_pt_yf.setValid(susSet6.tempC.isValid());
       if (not susTemperatures.sus_3_n_loc_xfybzf_pt_yf.isValid()) {
         susTemperatures.sus_3_n_loc_xfybzf_pt_yf.value = INVALID_TEMPERATURE;
       }
@@ -551,8 +551,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg7(&susSet7, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg7.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = susSet7.temperatureCelcius.value;
-      susTemperatures.sus_9_r_loc_xbybzb_pt_yf.setValid(susSet7.temperatureCelcius.isValid());
+      susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = susSet7.tempC.value;
+      susTemperatures.sus_9_r_loc_xbybzb_pt_yf.setValid(susSet7.tempC.isValid());
       if (not susTemperatures.sus_9_r_loc_xbybzb_pt_yf.isValid()) {
         susTemperatures.sus_9_r_loc_xbybzb_pt_yf.value = INVALID_TEMPERATURE;
       }
@@ -562,8 +562,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg8(&susSet8, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg8.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = susSet8.temperatureCelcius.value;
-      susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.setValid(susSet8.temperatureCelcius.isValid());
+      susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = susSet8.tempC.value;
+      susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.setValid(susSet8.tempC.isValid());
       if (not susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.isValid()) {
         susTemperatures.sus_4_n_loc_xmyfzf_pt_zf.value = INVALID_TEMPERATURE;
       }
@@ -573,8 +573,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg9(&susSet9, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg9.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = susSet9.temperatureCelcius.value;
-      susTemperatures.sus_10_n_loc_xmybzf_pt_zf.setValid(susSet9.temperatureCelcius.isValid());
+      susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = susSet9.tempC.value;
+      susTemperatures.sus_10_n_loc_xmybzf_pt_zf.setValid(susSet9.tempC.isValid());
       if (not susTemperatures.sus_10_n_loc_xmybzf_pt_zf.isValid()) {
         susTemperatures.sus_10_n_loc_xmybzf_pt_zf.value = INVALID_TEMPERATURE;
       }
@@ -584,8 +584,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg10(&susSet10, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg10.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = susSet10.temperatureCelcius.value;
-      susTemperatures.sus_5_n_loc_xfymzb_pt_zb.setValid(susSet10.temperatureCelcius.isValid());
+      susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = susSet10.tempC.value;
+      susTemperatures.sus_5_n_loc_xfymzb_pt_zb.setValid(susSet10.tempC.isValid());
       if (not susTemperatures.sus_5_n_loc_xfymzb_pt_zb.isValid()) {
         susTemperatures.sus_5_n_loc_xfymzb_pt_zb.value = INVALID_TEMPERATURE;
       }
@@ -595,8 +595,8 @@ void ThermalController::copySus() {
   {
     PoolReadGuard pg11(&susSet11, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg11.getReadResult() == returnvalue::OK) {
-      susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = susSet11.temperatureCelcius.value;
-      susTemperatures.sus_11_r_loc_xbymzb_pt_zb.setValid(susSet11.temperatureCelcius.isValid());
+      susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = susSet11.tempC.value;
+      susTemperatures.sus_11_r_loc_xbymzb_pt_zb.setValid(susSet11.tempC.isValid());
       if (not susTemperatures.sus_11_r_loc_xbymzb_pt_zb.isValid()) {
         susTemperatures.sus_11_r_loc_xbymzb_pt_zb.value = INVALID_TEMPERATURE;
       }
@@ -854,7 +854,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempGyro0 =
-        lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
+        lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE);
     PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl;
@@ -867,7 +867,7 @@ void ThermalController::copyDevices() {
   }
 
   {
-    lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE);
+    lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE);
     PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl;
@@ -881,7 +881,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempGyro2 =
-        lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
+        lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE);
     PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl;
@@ -894,7 +894,7 @@ void ThermalController::copyDevices() {
   }
 
   {
-    lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE);
+    lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE);
     PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl;
@@ -908,7 +908,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempMgm0 =
-        lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS);
+        lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
     PoolReadGuard pg(&tempMgm0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl;
@@ -922,7 +922,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempMgm2 =
-        lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS);
+        lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
     PoolReadGuard pg(&tempMgm2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl;

mission/controller/ThermalController.h

@@ -6,8 +6,8 @@
 #include <fsfw/timemanager/Countdown.h>
 #include <mission/controller/controllerdefinitions/ThermalControllerDefinitions.h>
 #include <mission/devices/devicedefinitions/Max31865Definitions.h>
-#include <mission/devices/devicedefinitions/SusDefinitions.h>
 #include <mission/devices/devicedefinitions/Tmp1075Definitions.h>
+#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
 
 #include <list>
 
@@ -112,18 +112,18 @@ class ThermalController : public ExtendedControllerBase {
   TMP1075::Tmp1075Dataset tmp1075SetIfBoard;
 
   // SUS
-  SUS::SusDataset susSet0;
-  SUS::SusDataset susSet1;
-  SUS::SusDataset susSet2;
-  SUS::SusDataset susSet3;
-  SUS::SusDataset susSet4;
-  SUS::SusDataset susSet5;
-  SUS::SusDataset susSet6;
-  SUS::SusDataset susSet7;
-  SUS::SusDataset susSet8;
-  SUS::SusDataset susSet9;
-  SUS::SusDataset susSet10;
-  SUS::SusDataset susSet11;
+  susMax1227::SusDataset susSet0;
+  susMax1227::SusDataset susSet1;
+  susMax1227::SusDataset susSet2;
+  susMax1227::SusDataset susSet3;
+  susMax1227::SusDataset susSet4;
+  susMax1227::SusDataset susSet5;
+  susMax1227::SusDataset susSet6;
+  susMax1227::SusDataset susSet7;
+  susMax1227::SusDataset susSet8;
+  susMax1227::SusDataset susSet9;
+  susMax1227::SusDataset susSet10;
+  susMax1227::SusDataset susSet11;
 
   // TempLimits
   TempLimits acsBoardLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);

mission/controller/acs/Guidance.cpp

@@ -540,8 +540,10 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
 }
 
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
-  if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
-      not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {  // ToDo: if file does not exist anymore
+  std::error_code e;
+  if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
+      not std::filesystem::exists(SD_1_SKEWED_PTG_FILE,
+                                  e)) {  // ToDo: if file does not exist anymore
     std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
                 3 * sizeof(double));
   } else {
@@ -553,15 +555,16 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3
 }
 
 ReturnValue_t Guidance::solarArrayDeploymentComplete() {
-  if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) {
+  std::error_code e;
+  if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) {
     std::remove(SD_0_SKEWED_PTG_FILE);
-    if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) {
+    if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) {
       return returnvalue::FAILED;
     }
   }
-  if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {
+  if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
     std::remove(SD_1_SKEWED_PTG_FILE);
-    if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {
+    if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
       return returnvalue::FAILED;
     }
   }

mission/controller/acs/SensorValues.h

@@ -1,16 +1,16 @@
 #ifndef SENSORVALUES_H_
 #define SENSORVALUES_H_
 
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 #include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
+#include <mission/devices/devicedefinitions/susMax1227Helpers.h>
 
 #include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
 #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
 #include "mission/devices/devicedefinitions/GPSDefinitions.h"
-#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
-#include "mission/devices/devicedefinitions/SusDefinitions.h"
 
 namespace ACS {
 
@@ -27,30 +27,28 @@ class SensorValues {
   ReturnValue_t updateStr();
   ReturnValue_t updateRw();
 
-  MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set =
-      MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
-  RM3100::Rm3100PrimaryDataset mgm1Rm3100Set =
-      RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
-  MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set =
-      MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
-  RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
-      RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
+  mgmLis3::MgmPrimaryDataset mgm0Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
+  mgmRm3100::Rm3100PrimaryDataset mgm1Rm3100Set =
+      mgmRm3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
+  mgmLis3::MgmPrimaryDataset mgm2Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
+  mgmRm3100::Rm3100PrimaryDataset mgm3Rm3100Set =
+      mgmRm3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
   imtq::RawMtmMeasurementNoTorque imtqMgmSet =
       imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER);
 
-  std::array<SUS::SusDataset, 12> susSets{
-      SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF),
-      SUS::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB),
-      SUS::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB),
-      SUS::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF),
-      SUS::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF),
-      SUS::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB),
-      SUS::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF),
-      SUS::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB),
-      SUS::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB),
-      SUS::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF),
-      SUS::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF),
-      SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
+  std::array<susMax1227::SusDataset, 12> susSets{
+      susMax1227::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF),
+      susMax1227::SusDataset(objects::SUS_1_N_LOC_XBYFZM_PT_XB),
+      susMax1227::SusDataset(objects::SUS_2_N_LOC_XFYBZB_PT_YB),
+      susMax1227::SusDataset(objects::SUS_3_N_LOC_XFYBZF_PT_YF),
+      susMax1227::SusDataset(objects::SUS_4_N_LOC_XMYFZF_PT_ZF),
+      susMax1227::SusDataset(objects::SUS_5_N_LOC_XFYMZB_PT_ZB),
+      susMax1227::SusDataset(objects::SUS_6_R_LOC_XFYBZM_PT_XF),
+      susMax1227::SusDataset(objects::SUS_7_R_LOC_XBYBZM_PT_XB),
+      susMax1227::SusDataset(objects::SUS_8_R_LOC_XBYBZB_PT_YB),
+      susMax1227::SusDataset(objects::SUS_9_R_LOC_XBYBZB_PT_YF),
+      susMax1227::SusDataset(objects::SUS_10_N_LOC_XMYBZF_PT_ZF),
+      susMax1227::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
   };
 
   AdisGyroPrimaryDataset gyr0AdisSet = AdisGyroPrimaryDataset(objects::GYRO_0_ADIS_HANDLER);

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

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,39 +80,38 @@ 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,
-                          bool sunRateBValid, double *magFieldB, bool magFieldBValid,
-                          double *magRateB, bool magRateBValid, double *sunDirRef,
-                          double *satRateRef, double *outputAngle, double *outputMagMomB,
-                          bool *outputValid) {
+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) {
-    *outputValid = false;
-    return;
+    return returnvalue::FAILED;
   }
 
+  // change unit from uT to T
+  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
   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 +127,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 +135,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};
@@ -165,11 +161,11 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
   // 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));
   *outputAngle = angleAlignErr;
-  *outputValid = true;
+  return returnvalue::OK;
 }

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:

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
@@ -139,9 +141,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
@@ -236,7 +239,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) {

mission/core/pollingSeqTables.cpp

@@ -30,22 +30,6 @@ ReturnValue_t pst::pstSpiAndSyrlinks(FixedTimeslotTaskIF *thisSequence) {
 #endif
 
   static_cast<void>(length);
-#if OBSW_ADD_PL_PCDU == 1
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
-#endif
-
-#if OBSW_ADD_RAD_SENSORS == 1
-  /* Radiation sensor */
-  thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * 0.2, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * 0.4, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * 0.6, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ);
-#endif
 
   return thisSequence->checkSequence();
 }
@@ -189,390 +173,155 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) {
 ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) {
   /* Length of a communication cycle */
   uint32_t length = thisSequence->getPeriodMs();
-  bool enableAside = true;
-  bool enableBside = true;
-  if (cfg.scheduleAcsBoard) {
-    if (enableAside) {
-      // A side
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_READ);
 
-      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_READ);
-    }
-    if (enableBside) {
-      // B side
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_READ);
-    }
-  }
   // SUS: 16 ms
-  bool addSus0 = true;
-  bool addSus1 = true;
-  bool addSus2 = true;
-  bool addSus3 = true;
-  bool addSus4 = true;
-  bool addSus5 = true;
-  bool addSus6 = true;
-  bool addSus7 = true;
-  bool addSus8 = true;
-  bool addSus9 = true;
-  bool addSus10 = true;
-  bool addSus11 = true;
-
   if (cfg.scheduleSus) {
-    if (addSus0) {
-      /* Write setup */
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                            DeviceHandlerIF::GET_READ);
+    /* Write setup */
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
 
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus1) {
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus2) {
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus3) {
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus4) {
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus5) {
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus6) {
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus7) {
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus8) {
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus9) {
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus10) {
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
-    if (addSus11) {
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                            length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
-    }
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF,
+                          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);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB,
+                          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);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB,
+                          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);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF,
+                          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);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF,
+                          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);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
+                          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);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
+                          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);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
+                          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);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
+                          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);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
+                          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);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
+                          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);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB,
+                          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);
   }
 
   if (cfg.scheduleStr) {
@@ -583,83 +332,250 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
     thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ);
   }
 
+  bool enableAside = true;
+  bool enableBside = true;
+  if (cfg.scheduleAcsBoard) {
+    if (enableAside) {
+      // A side
+      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,
+                            DeviceHandlerIF::SEND_WRITE);
+      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,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
+                            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);
+      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
+                            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);
+    }
+    if (enableBside) {
+      // B side
+      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,
+                            DeviceHandlerIF::SEND_WRITE);
+      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,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
+                            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);
+      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
+                            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);
+    }
+    if (enableAside) {
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            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,
+                            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) {
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::spiSched::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            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,
+                            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);
+    }
+  }
+
   if (cfg.scheduleImtq) {
     // This is the MTM measurement cycle
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD,
                           imtq::ComStep::DHB_OP);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD,
                           imtq::ComStep::START_MEASURE_SEND);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_1_PERIOD,
                           imtq::ComStep::START_MEASURE_GET);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                           imtq::ComStep::READ_MEASURE_SEND);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_3_PERIOD,
                           imtq::ComStep::READ_MEASURE_GET);
   }
 
-  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::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.
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           imtq::ComStep::START_ACTUATE_SEND);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           imtq::ComStep::START_ACTUATE_GET);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_6_PERIOD,
                           imtq::ComStep::READ_ACTUATE_SEND);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::spiSched::SCHED_BLOCK_6_PERIOD,
                           imtq::ComStep::READ_ACTUATE_GET);
   }
 
   if (cfg.scheduleRws) {
     // this is the torquing cycle
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
 
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::SEND_WRITE);
 
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
+    thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::GET_WRITE);
 
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
+    thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
+    thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
+    thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
+    thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::SEND_READ);
 
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
+    thisSequence->addSlot(objects::RW1, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
+    thisSequence->addSlot(objects::RW2, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
+    thisSequence->addSlot(objects::RW3, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
+    thisSequence->addSlot(objects::RW4, length * config::spiSched::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::GET_READ);
   }
 
-  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::acs::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);
+#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);
+#endif
   return returnvalue::OK;
 }

mission/devices/CMakeLists.txt

@@ -14,7 +14,10 @@ target_sources(
           ImtqHandler.cpp
           HeaterHandler.cpp
           RadiationSensorHandler.cpp
-          GyroADIS1650XHandler.cpp
+          GyrAdis1650XHandler.cpp
+          GyrL3gCustomHandler.cpp
+          MgmRm3100CustomHandler.cpp
+          MgmLis3CustomHandler.cpp
           RwHandler.cpp
           max1227.cpp
           SusHandler.cpp

mission/devices/GyrAdis1650XHandler.cpp

@@ -0,0 +1,226 @@
+#include "mission/devices/GyrAdis1650XHandler.h"
+
+#include "fsfw/action/HasActionsIF.h"
+#include "fsfw/datapool/PoolReadGuard.h"
+#include "mission/devices/devicedefinitions/acsPolling.h"
+
+GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
+                                         CookieIF *comCookie, adis1650x::Type type)
+    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+      adisType(type),
+      primaryDataset(this),
+      configDataset(this),
+      breakCountdown() {
+  adisRequest.type = adisType;
+}
+
+void GyrAdis1650XHandler::doStartUp() {
+  if (internalState != InternalState::STARTUP) {
+    internalState = InternalState::STARTUP;
+    commandExecuted = false;
+  }
+  // Initial 310 ms start up time after power-up
+  if (internalState == InternalState::STARTUP) {
+    if (not commandExecuted) {
+      warningSwitch = true;
+      breakCountdown.setTimeout(adis1650x::START_UP_TIME);
+      commandExecuted = true;
+    }
+    if (breakCountdown.hasTimedOut()) {
+      updatePeriodicReply(true, adis1650x::REPLY);
+      if (goToNormalMode) {
+        setMode(MODE_NORMAL);
+      } else {
+        setMode(MODE_ON);
+      }
+      internalState = InternalState::NONE;
+    }
+  }
+}
+
+void GyrAdis1650XHandler::doShutDown() {
+  if (internalState != InternalState::SHUTDOWN) {
+    commandExecuted = false;
+    primaryDataset.setValidity(false, true);
+    internalState = InternalState::SHUTDOWN;
+  }
+  if (internalState == InternalState::SHUTDOWN and commandExecuted) {
+    updatePeriodicReply(false, adis1650x::REPLY);
+    internalState = InternalState::NONE;
+    commandExecuted = false;
+    setMode(MODE_OFF);
+  }
+}
+
+ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
+  *id = adis1650x::REQUEST;
+  return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
+}
+
+ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+  switch (internalState) {
+    case (InternalState::STARTUP): {
+      *id = adis1650x::REQUEST;
+      return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
+    }
+    case (InternalState::SHUTDOWN): {
+      *id = adis1650x::REQUEST;
+      acs::Adis1650XRequest *request = reinterpret_cast<acs::Adis1650XRequest *>(cmdBuf.data());
+      request->mode = acs::SimpleSensorMode::OFF;
+      request->type = adisType;
+      return returnvalue::OK;
+    }
+    default: {
+      return NOTHING_TO_SEND;
+    }
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+                                                           const uint8_t *commandData,
+                                                           size_t commandDataLen) {
+  return NOTHING_TO_SEND;
+}
+
+void GyrAdis1650XHandler::fillCommandAndReplyMap() {
+  insertInCommandMap(adis1650x::REQUEST);
+  insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true);
+}
+
+ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
+                                                DeviceCommandId_t *foundId, size_t *foundLen) {
+  if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or
+      getMode() == _MODE_POWER_DOWN) {
+    return IGNORE_FULL_PACKET;
+  }
+  if (remainingSize != sizeof(acs::Adis1650XReply)) {
+    *foundLen = remainingSize;
+    return returnvalue::FAILED;
+  }
+  *foundId = adis1650x::REPLY;
+  *foundLen = remainingSize;
+  if (internalState == InternalState::SHUTDOWN) {
+    commandExecuted = true;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
+                                                        const uint8_t *packet) {
+  using namespace adis1650x;
+  const acs::Adis1650XReply *reply = reinterpret_cast<const acs::Adis1650XReply *>(packet);
+  if (internalState == InternalState::STARTUP and reply->cfgWasSet) {
+    switch (adisType) {
+      case (adis1650x::Type::ADIS16507): {
+        if (reply->cfg.prodId != adis1650x::PROD_ID_16507) {
+          sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
+                     << reply->cfg.prodId << "for ADIS type 16507" << std::endl;
+          return returnvalue::FAILED;
+        }
+        break;
+      }
+      case (adis1650x::Type::ADIS16505): {
+        if (reply->cfg.prodId != adis1650x::PROD_ID_16505) {
+          sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
+                     << reply->cfg.prodId << "for ADIS type 16505" << std::endl;
+          return returnvalue::FAILED;
+        }
+        break;
+      }
+    }
+    PoolReadGuard rg(&configDataset);
+    configDataset.setValidity(true, true);
+    configDataset.mscCtrlReg = reply->cfg.mscCtrlReg;
+    configDataset.rangMdl = reply->cfg.rangMdl;
+    configDataset.diagStatReg = reply->cfg.diagStat;
+    configDataset.filterSetting = reply->cfg.filterSetting;
+    configDataset.decRateReg = reply->cfg.decRateReg;
+    commandExecuted = true;
+  }
+  if (reply->dataWasSet) {
+    {
+      PoolReadGuard rg(&configDataset);
+      configDataset.diagStatReg = reply->cfg.diagStat;
+    }
+    auto sensitivity = reply->data.sensitivity;
+    auto accelSensitivity = reply->data.accelScaling;
+    PoolReadGuard pg(&primaryDataset);
+    primaryDataset.setValidity(true, true);
+    primaryDataset.angVelocX = static_cast<double>(reply->data.angVelocities[0]) * sensitivity;
+    primaryDataset.angVelocY = static_cast<double>(reply->data.angVelocities[1]) * sensitivity;
+    primaryDataset.angVelocZ = static_cast<double>(reply->data.angVelocities[2]) * sensitivity;
+    // TODO: Check whether we need to divide by INT16_MAX
+    primaryDataset.accelX = static_cast<double>(reply->data.accelerations[0]) * accelSensitivity;
+    primaryDataset.accelY = static_cast<double>(reply->data.accelerations[1]) * accelSensitivity;
+    primaryDataset.accelZ = static_cast<double>(reply->data.accelerations[2]) * accelSensitivity;
+    primaryDataset.temperature.value = static_cast<float>(reply->data.temperatureRaw) * 0.1;
+  }
+  return returnvalue::OK;
+}
+
+LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) {
+  if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) {
+    return &primaryDataset;
+  } else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) {
+    return &configDataset;
+  }
+  return nullptr;
+}
+
+uint32_t GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
+
+void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
+                                              uint8_t valueTwo) {
+  uint8_t secondReg = startReg + 1;
+  startReg |= adis1650x::WRITE_MASK;
+  secondReg |= adis1650x::WRITE_MASK;
+  cmdBuf[0] = startReg;
+  cmdBuf[1] = valueOne;
+  cmdBuf[2] = secondReg;
+  cmdBuf[3] = valueTwo;
+  this->rawPacketLen = 4;
+  this->rawPacket = cmdBuf.data();
+}
+
+ReturnValue_t GyrAdis1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                                           LocalDataPoolManager &poolManager) {
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry<float>({0.0}));
+
+  localDataPoolMap.emplace(adis1650x::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
+  localDataPoolMap.emplace(adis1650x::FILTER_SETTINGS, new PoolEntry<uint8_t>());
+  localDataPoolMap.emplace(adis1650x::RANG_MDL, &rangMdl);
+  localDataPoolMap.emplace(adis1650x::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
+  localDataPoolMap.emplace(adis1650x::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
+  return returnvalue::OK;
+}
+
+adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() {
+  using namespace adis1650x;
+  configDataset.mscCtrlReg.read();
+  uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
+  configDataset.mscCtrlReg.commit();
+  return adis1650x::burstModeFromMscCtrl(currentCtrlReg);
+}
+
+void GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
+
+void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
+  periodicPrintout = enable;
+  debugDivider.setDivider(divider);
+}
+
+ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) {
+  adisRequest.mode = mode;
+  rawPacket = reinterpret_cast<uint8_t *>(&adisRequest);
+  rawPacketLen = sizeof(acs::Adis1650XRequest);
+  return returnvalue::OK;
+}

mission/devices/GyrAdis1650XHandler.h

@@ -1,27 +1,24 @@
 #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
 #define MISSION_DEVICES_GYROADIS16507HANDLER_H_
 
+#include <mission/devices/devicedefinitions/acsPolling.h>
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
+
 #include "FSFWConfig.h"
 #include "OBSWConfig.h"
-#include "devicedefinitions/GyroADIS1650XDefinitions.h"
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 
-#ifdef FSFW_OSAL_LINUX
-class SpiComIF;
-class SpiCookie;
-#endif
-
 /**
  * @brief   Device handle for the ADIS16507 Gyroscope by Analog Devices
  * @details
  * Flight manual:
  * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro
  */
-class GyroADIS1650XHandler : public DeviceHandlerBase {
+class GyrAdis1650XHandler : public DeviceHandlerBase {
  public:
-  GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
-                       ADIS1650X::Type type);
+  GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
+                      adis1650x::Type type);
 
   void enablePeriodicPrintouts(bool enable, uint8_t divider);
   void setToGoToNormalModeImmediately();
@@ -40,42 +37,31 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                         LocalDataPoolManager &poolManager) override;
+  LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
 
  private:
-  std::array<uint8_t, 32> commandBuffer;
-  ADIS1650X::Type adisType;
+  std::array<uint8_t, 32> cmdBuf;
+  acs::Adis1650XRequest adisRequest;
+  adis1650x::Type adisType;
   AdisGyroPrimaryDataset primaryDataset;
   AdisGyroConfigDataset configDataset;
-  double sensitivity = ADIS1650X::SENSITIVITY_UNSET;
+  double sensitivity = adis1650x::SENSITIVITY_UNSET;
 
   bool goToNormalMode = false;
   bool warningSwitch = true;
 
-  enum class InternalState { STARTUP, CONFIG, IDLE };
-
-  enum class BurstModes {
-    BURST_16_BURST_SEL_0,
-    BURST_16_BURST_SEL_1,
-    BURST_32_BURST_SEL_0,
-    BURST_32_BURST_SEL_1
-  };
+  enum class InternalState { NONE, STARTUP, SHUTDOWN };
 
   InternalState internalState = InternalState::STARTUP;
   bool commandExecuted = false;
 
   PoolEntry<uint16_t> rangMdl = PoolEntry<uint16_t>();
 
-  void prepareReadCommand(uint8_t *regList, size_t len);
-
-  BurstModes getBurstMode();
-
-#ifdef FSFW_OSAL_LINUX
-  static ReturnValue_t spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, const uint8_t *sendData,
-                                       size_t sendLen, void *args);
-#endif
+  adis1650x::BurstModes getBurstMode();
 
   Countdown breakCountdown;
   void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo);
+  ReturnValue_t preparePeriodicRequest(acs::SimpleSensorMode mode);
 
   ReturnValue_t handleSensorData(const uint8_t *packet);
 

mission/devices/GyrL3gCustomHandler.cpp

@@ -0,0 +1,191 @@
+
+#include <mission/devices/GyrL3gCustomHandler.h>
+
+#include <cmath>
+
+#include "fsfw/datapool/PoolReadGuard.h"
+#include "mission/devices/devicedefinitions/acsPolling.h"
+
+GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication,
+                                         CookieIF *comCookie, uint32_t transitionDelayMs)
+    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+      transitionDelayMs(transitionDelayMs),
+      dataset(this) {}
+
+GyrL3gCustomHandler::~GyrL3gCustomHandler() = default;
+
+void GyrL3gCustomHandler::doStartUp() {
+  if (internalState != InternalState::STARTUP) {
+    internalState = InternalState::STARTUP;
+    updatePeriodicReply(true, l3gd20h::REPLY);
+    commandExecuted = false;
+  }
+
+  if (internalState == InternalState::STARTUP) {
+    if (commandExecuted) {
+      if (goNormalModeImmediately) {
+        setMode(MODE_NORMAL);
+      } else {
+        setMode(_MODE_TO_ON);
+      }
+      internalState = InternalState::NONE;
+      commandExecuted = false;
+    }
+  }
+}
+
+void GyrL3gCustomHandler::doShutDown() {
+  if (internalState != InternalState::SHUTDOWN) {
+    internalState = InternalState::SHUTDOWN;
+    dataset.setValidity(false, true);
+    commandExecuted = false;
+  }
+  if (internalState == InternalState::SHUTDOWN and commandExecuted) {
+    internalState = InternalState::NONE;
+    updatePeriodicReply(false, l3gd20h::REPLY);
+    commandExecuted = false;
+    setMode(MODE_OFF);
+  }
+}
+
+ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+  switch (internalState) {
+    case (InternalState::NONE):
+    case (InternalState::NORMAL): {
+      return NOTHING_TO_SEND;
+    }
+    case (InternalState::STARTUP): {
+      *id = l3gd20h::REQUEST;
+      gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL;
+      gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL;
+      gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL;
+      gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL;
+      gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL;
+      return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
+    }
+    case (InternalState::SHUTDOWN): {
+      *id = l3gd20h::REQUEST;
+      return doPeriodicRequest(acs::SimpleSensorMode::OFF);
+    }
+    default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      /* Might be a configuration error. */
+      sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: "
+                      "Unknown internal state!"
+                   << std::endl;
+#else
+      sif::printDebug(
+          "GyroL3GD20Handler::buildTransitionDeviceCommand: "
+          "Unknown internal state!\n");
+#endif
+      return returnvalue::OK;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
+  *id = l3gd20h::REQUEST;
+  return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
+}
+
+ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+                                                           const uint8_t *commandData,
+                                                           size_t commandDataLen) {
+  switch (deviceCommand) {
+    default:
+      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrL3gCustomHandler::scanForReply(const uint8_t *start, size_t len,
+                                                DeviceCommandId_t *foundId, size_t *foundLen) {
+  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or getMode() == _MODE_POWER_DOWN) {
+    return IGNORE_FULL_PACKET;
+  }
+  if (len != sizeof(acs::GyroL3gReply)) {
+    *foundLen = len;
+    return returnvalue::FAILED;
+  }
+  *foundId = l3gd20h::REPLY;
+  *foundLen = len;
+  if (internalState == InternalState::SHUTDOWN) {
+    commandExecuted = true;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id,
+                                                        const uint8_t *packet) {
+  const acs::GyroL3gReply *reply = reinterpret_cast<const acs::GyroL3gReply *>(packet);
+  if (reply->cfgWasSet) {
+    if (internalState == InternalState::STARTUP) {
+      commandExecuted = true;
+    }
+    PoolReadGuard pg(&dataset);
+    dataset.setValidity(true, true);
+    dataset.angVelocX = static_cast<float>(reply->angVelocities[0]) * reply->sensitivity;
+    dataset.angVelocY = static_cast<float>(reply->angVelocities[1]) * reply->sensitivity;
+    dataset.angVelocZ = static_cast<float>(reply->angVelocities[2]) * reply->sensitivity;
+    if (std::abs(dataset.angVelocX.value) > absLimitX) {
+      dataset.angVelocX.setValid(false);
+    }
+    if (std::abs(dataset.angVelocY.value) > absLimitY) {
+      dataset.angVelocY.setValid(false);
+    }
+    if (std::abs(dataset.angVelocZ.value) > absLimitZ) {
+      dataset.angVelocZ.setValid(false);
+    }
+    dataset.temperature = 25.0 - reply->tempOffsetRaw;
+  }
+  return returnvalue::OK;
+}
+
+uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
+  return this->transitionDelayMs;
+}
+
+void GyrL3gCustomHandler::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; }
+
+ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                                           LocalDataPoolManager &poolManager) {
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0));
+  return returnvalue::OK;
+}
+
+void GyrL3gCustomHandler::fillCommandAndReplyMap() {
+  insertInCommandMap(l3gd20h::REQUEST);
+  insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true);
+}
+
+void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; }
+
+void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) {
+  this->absLimitX = limitX;
+  this->absLimitY = limitY;
+  this->absLimitZ = limitZ;
+}
+
+void GyrL3gCustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
+  periodicPrintout = enable;
+  debugDivider.setDivider(divider);
+}
+
+LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) {
+  if (sid == dataset.getSid()) {
+    return &dataset;
+  }
+  return nullptr;
+}
+
+ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) {
+  gyrRequest.mode = mode;
+  rawPacket = reinterpret_cast<uint8_t *>(&gyrRequest);
+  rawPacketLen = sizeof(acs::GyroL3gRequest);
+  return returnvalue::OK;
+}

mission/devices/GyrL3gCustomHandler.h

@@ -0,0 +1,91 @@
+#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_
+#define MISSION_DEVICES_GYROL3GD20HANDLER_H_
+
+#include <fsfw/devicehandlers/DeviceHandlerBase.h>
+#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
+#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
+#include <mission/devices/devicedefinitions/acsPolling.h>
+
+/**
+ * @brief 	Device Handler for the L3GD20H gyroscope sensor
+ *          (https://www.st.com/en/mems-and-sensors/l3gd20h.html)
+ * @details
+ * Advanced documentation:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro
+ *
+ * Data is read big endian with the smallest possible range of 245 degrees per second.
+ */
+class GyrL3gCustomHandler : public DeviceHandlerBase {
+ public:
+  GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
+                      uint32_t transitionDelayMs);
+  virtual ~GyrL3gCustomHandler();
+
+  void enablePeriodicPrintouts(bool enable, uint8_t divider);
+
+  /**
+   * Set the absolute limit for the values on the axis in degrees per second.
+   * The dataset values will be marked as invalid if that limit is exceeded
+   * @param xLimit
+   * @param yLimit
+   * @param zLimit
+   */
+  void setAbsoluteLimits(float limitX, float limitY, float limitZ);
+
+  /**
+   * @brief   Configure device handler to go to normal mode immediately
+   */
+  void setToGoToNormalMode(bool enable);
+
+ protected:
+  /* DeviceHandlerBase overrides */
+  ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
+  void doStartUp() override;
+  void doShutDown() override;
+  ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
+  ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+                                        size_t commandDataLen) override;
+  ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
+                             size_t *foundLen) override;
+  virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
+
+  void fillCommandAndReplyMap() override;
+  void modeChanged() override;
+  virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
+  ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                        LocalDataPoolManager &poolManager) override;
+  LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
+
+ private:
+  uint32_t transitionDelayMs = 0;
+  GyroPrimaryDataset dataset;
+
+  float absLimitX = l3gd20h::RANGE_DPS_00;
+  float absLimitY = l3gd20h::RANGE_DPS_00;
+  float absLimitZ = l3gd20h::RANGE_DPS_00;
+
+  enum class InternalState { NONE, STARTUP, SHUTDOWN, NORMAL };
+  InternalState internalState = InternalState::NONE;
+  bool commandExecuted = false;
+
+  uint8_t statusReg = 0;
+  bool goNormalModeImmediately = false;
+
+  uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL;
+  uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL;
+  uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL;
+  uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL;
+  uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL;
+
+  acs::GyroL3gRequest gyrRequest{};
+
+  // Set default value
+  float sensitivity = l3gd20h::SENSITIVITY_00;
+
+  bool periodicPrintout = false;
+  PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
+
+  ReturnValue_t doPeriodicRequest(acs::SimpleSensorMode mode);
+};
+
+#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */

mission/devices/GyroADIS1650XHandler.cpp

@@ -1,524 +0,0 @@
-#include "GyroADIS1650XHandler.h"
-
-#include <fsfw/action/HasActionsIF.h>
-#include <fsfw/datapool/PoolReadGuard.h>
-
-#include "fsfw/FSFW.h"
-
-#ifdef FSFW_OSAL_LINUX
-#include <sys/ioctl.h>
-#include <unistd.h>
-
-#include "fsfw_hal/linux/UnixFileGuard.h"
-#include "fsfw_hal/linux/spi/SpiComIF.h"
-#include "fsfw_hal/linux/spi/SpiCookie.h"
-#include "fsfw_hal/linux/utility.h"
-#endif
-
-GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
-                                           CookieIF *comCookie, ADIS1650X::Type type)
-    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
-      adisType(type),
-      primaryDataset(this),
-      configDataset(this),
-      breakCountdown() {
-#ifdef FSFW_OSAL_LINUX
-  SpiCookie *cookie = dynamic_cast<SpiCookie *>(comCookie);
-  if (cookie != nullptr) {
-    cookie->setCallbackMode(&spiSendCallback, this);
-  }
-#endif
-}
-
-void GyroADIS1650XHandler::doStartUp() {
-  // Initial 310 ms start up time after power-up
-  if (internalState == InternalState::STARTUP) {
-    if (not commandExecuted) {
-      warningSwitch = true;
-      breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
-      commandExecuted = true;
-    }
-    if (breakCountdown.hasTimedOut()) {
-      internalState = InternalState::CONFIG;
-      commandExecuted = false;
-    }
-  }
-
-  // Read all configuration registers first
-  if (internalState == InternalState::CONFIG) {
-    if (commandExecuted) {
-      commandExecuted = false;
-      internalState = InternalState::IDLE;
-    }
-  }
-
-  if (internalState == InternalState::IDLE) {
-    if (goToNormalMode) {
-      setMode(MODE_NORMAL);
-    } else {
-      setMode(MODE_ON);
-    }
-  }
-}
-
-void GyroADIS1650XHandler::doShutDown() {
-  commandExecuted = false;
-  internalState = InternalState::STARTUP;
-  setMode(_MODE_POWER_DOWN);
-}
-
-ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
-  *id = ADIS1650X::READ_SENSOR_DATA;
-  return buildCommandFromCommand(*id, nullptr, 0);
-}
-
-ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
-  switch (internalState) {
-    case (InternalState::CONFIG): {
-      *id = ADIS1650X::READ_OUT_CONFIG;
-      buildCommandFromCommand(*id, nullptr, 0);
-      break;
-    }
-    case (InternalState::STARTUP): {
-      return NOTHING_TO_SEND;
-      break;
-    }
-    default: {
-      // Might be a configuration error
-      sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
-                    "Unknown internal state!"
-                 << std::endl;
-      return returnvalue::OK;
-    }
-  }
-  return returnvalue::OK;
-}
-
-ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
-                                                            const uint8_t *commandData,
-                                                            size_t commandDataLen) {
-  switch (deviceCommand) {
-    case (ADIS1650X::READ_OUT_CONFIG): {
-      this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
-      uint8_t regList[6] = {};
-      regList[0] = ADIS1650X::DIAG_STAT_REG;
-      regList[1] = ADIS1650X::FILTER_CTRL_REG;
-      regList[2] = ADIS1650X::RANG_MDL_REG;
-      regList[3] = ADIS1650X::MSC_CTRL_REG;
-      regList[4] = ADIS1650X::DEC_RATE_REG;
-      regList[5] = ADIS1650X::PROD_ID_REG;
-      prepareReadCommand(regList, sizeof(regList));
-      this->rawPacket = commandBuffer.data();
-      break;
-    }
-    case (ADIS1650X::READ_SENSOR_DATA): {
-      if (breakCountdown.isBusy()) {
-        // A glob command is pending and sensor data can't be read currently
-        return NO_REPLY_EXPECTED;
-      }
-      std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(),
-                  ADIS1650X::BURST_READ_ENABLE.size());
-      std::memset(commandBuffer.data() + 2, 0, 10 * 2);
-      this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE;
-      this->rawPacket = commandBuffer.data();
-      break;
-    }
-    case (ADIS1650X::SELF_TEST_SENSORS): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK);
-      break;
-    }
-    case (ADIS1650X::SELF_TEST_MEMORY): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK);
-      break;
-    }
-    case (ADIS1650X::UPDATE_NV_CONFIGURATION): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK);
-      break;
-    }
-    case (ADIS1650X::RESET_SENSOR_CONFIGURATION): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK);
-      break;
-    }
-    case (ADIS1650X::SW_RESET): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK);
-      break;
-    }
-    case (ADIS1650X::PRINT_CURRENT_CONFIGURATION): {
-#if OBSW_VERBOSE_LEVEL >= 1
-      PoolReadGuard pg(&configDataset);
-      sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4)
-                << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
-      sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x"
-                << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x"
-                << configDataset.filterSetting.value << " | DEC_RATE: 0x"
-                << configDataset.decRateReg.value << std::setfill(' ') << std::endl;
-#endif /* OBSW_VERBOSE_LEVEL >= 1 */
-    }
-  }
-  return returnvalue::OK;
-}
-
-void GyroADIS1650XHandler::fillCommandAndReplyMap() {
-  insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset);
-  insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset);
-  insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1);
-  insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1);
-  insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1);
-  insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1);
-  insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1);
-  insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1);
-}
-
-ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
-                                                 DeviceCommandId_t *foundId, size_t *foundLen) {
-  // For SPI, the ID will always be the one of the last sent command
-  *foundId = this->getPendingCommand();
-  *foundLen = this->rawPacketLen;
-
-  return returnvalue::OK;
-}
-
-ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
-                                                         const uint8_t *packet) {
-  using namespace ADIS1650X;
-  switch (id) {
-    case (ADIS1650X::READ_OUT_CONFIG): {
-      uint16_t readProdId = packet[12] << 8 | packet[13];
-      if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or
-          ((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) {
-#if OBSW_VERBOSE_LEVEL >= 1
-        if (warningSwitch) {
-          sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
-                       << readProdId << std::endl;
-        }
-        warningSwitch = false;
-#endif
-        return returnvalue::FAILED;
-      }
-      PoolReadGuard rg(&configDataset);
-      configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
-      configDataset.filterSetting.value = packet[4] << 8 | packet[5];
-      uint16_t rangMdlRaw = packet[6] << 8 | packet[7];
-      ADIS1650X::RangMdlBitfield bitfield =
-          static_cast<ADIS1650X::RangMdlBitfield>((rangMdlRaw >> 2) & 0b11);
-      switch (bitfield) {
-        case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): {
-          sensitivity = SENSITIVITY_1BMLZ;
-          break;
-        }
-        case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): {
-          sensitivity = SENSITIVITY_2BMLZ;
-          break;
-        }
-        case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): {
-          sensitivity = SENSITIVITY_3BMLZ;
-          break;
-        }
-        case (RangMdlBitfield::RESERVED): {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-          sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
-#endif
-          break;
-        }
-      }
-      configDataset.rangMdl.value = rangMdlRaw;
-      configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9];
-      configDataset.decRateReg.value = packet[10] << 8 | packet[11];
-      configDataset.setValidity(true, true);
-      if (internalState == InternalState::CONFIG) {
-        commandExecuted = true;
-      }
-      break;
-    }
-    case (ADIS1650X::READ_SENSOR_DATA): {
-      return handleSensorData(packet);
-    }
-  }
-  return returnvalue::OK;
-}
-
-ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
-  BurstModes burstMode = getBurstMode();
-  switch (burstMode) {
-    case (BurstModes::BURST_16_BURST_SEL_1):
-    case (BurstModes::BURST_32_BURST_SEL_1): {
-      sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
-                      " not implemented!"
-                   << std::endl;
-      return returnvalue::OK;
-    }
-    case (BurstModes::BURST_16_BURST_SEL_0): {
-      uint16_t checksum = packet[20] << 8 | packet[21];
-      // Now verify the read checksum with the expected checksum according to datasheet p. 20
-      uint16_t calcChecksum = 0;
-      for (size_t idx = 2; idx < 20; idx++) {
-        calcChecksum += packet[idx];
-      }
-      if (checksum != calcChecksum) {
-#if OBSW_VERBOSE_LEVEL >= 1
-        sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
-                        "Invalid checksum detected!"
-                     << std::endl;
-#endif
-        return returnvalue::FAILED;
-      }
-
-      ReturnValue_t result = configDataset.diagStatReg.read();
-      if (result == returnvalue::OK) {
-        configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
-        configDataset.diagStatReg.setValid(true);
-      }
-      configDataset.diagStatReg.commit();
-
-      {
-        PoolReadGuard pg(&primaryDataset);
-        int16_t angVelocXRaw = packet[4] << 8 | packet[5];
-        primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) * sensitivity;
-        int16_t angVelocYRaw = packet[6] << 8 | packet[7];
-        primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
-        int16_t angVelocZRaw = packet[8] << 8 | packet[9];
-        primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) * sensitivity;
-
-        float accelScaling = 0;
-        if (adisType == ADIS1650X::Type::ADIS16507) {
-          accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507;
-        } else if (adisType == ADIS1650X::Type::ADIS16505) {
-          accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505;
-        } else {
-          sif::warning << "GyroADIS1650XHandler::handleSensorData: "
-                          "Unknown ADIS type"
-                       << std::endl;
-        }
-        int16_t accelXRaw = packet[10] << 8 | packet[11];
-        primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX * accelScaling;
-        int16_t accelYRaw = packet[12] << 8 | packet[13];
-        primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX * accelScaling;
-        int16_t accelZRaw = packet[14] << 8 | packet[15];
-        primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX * accelScaling;
-
-        int16_t temperatureRaw = packet[16] << 8 | packet[17];
-        primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
-        // Ignore data counter for now
-        primaryDataset.setValidity(true, true);
-      }
-
-      if (periodicPrintout) {
-        if (debugDivider.checkAndIncrement()) {
-          sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
-          sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
-          sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
-          sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
-          sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
-          sif::info << "X: " << primaryDataset.accelX.value << std::endl;
-          sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
-          sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
-        }
-      }
-
-      break;
-    }
-    case (BurstModes::BURST_32_BURST_SEL_0): {
-      break;
-    }
-  }
-  return returnvalue::OK;
-}
-
-uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
-
-void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
-                                               uint8_t valueTwo) {
-  uint8_t secondReg = startReg + 1;
-  startReg |= ADIS1650X::WRITE_MASK;
-  secondReg |= ADIS1650X::WRITE_MASK;
-  commandBuffer[0] = startReg;
-  commandBuffer[1] = valueOne;
-  commandBuffer[2] = secondReg;
-  commandBuffer[3] = valueTwo;
-  this->rawPacketLen = 4;
-  this->rawPacket = commandBuffer.data();
-}
-
-void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) {
-  for (size_t idx = 0; idx < len; idx++) {
-    commandBuffer[idx * 2] = regList[idx];
-    commandBuffer[idx * 2 + 1] = 0x00;
-  }
-  commandBuffer[len * 2] = 0x00;
-  commandBuffer[len * 2 + 1] = 0x00;
-}
-
-ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
-                                                            LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
-
-  localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
-  localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
-  localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl);
-  localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
-  localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
-  poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
-  return returnvalue::OK;
-}
-
-GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() {
-  configDataset.mscCtrlReg.read();
-  uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
-  configDataset.mscCtrlReg.commit();
-  if ((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) {
-    if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
-      return BurstModes::BURST_32_BURST_SEL_1;
-    } else {
-      return BurstModes::BURST_32_BURST_SEL_0;
-    }
-  } else {
-    if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
-      return BurstModes::BURST_16_BURST_SEL_1;
-    } else {
-      return BurstModes::BURST_16_BURST_SEL_0;
-    }
-  }
-}
-
-#ifdef FSFW_OSAL_LINUX
-
-ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
-                                                    const uint8_t *sendData, size_t sendLen,
-                                                    void *args) {
-  GyroADIS1650XHandler *handler = reinterpret_cast<GyroADIS1650XHandler *>(args);
-  if (handler == nullptr) {
-    sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
-               << std::endl;
-    return returnvalue::FAILED;
-  }
-  DeviceCommandId_t currentCommand = handler->getPendingCommand();
-  switch (currentCommand) {
-    case (ADIS1650X::READ_SENSOR_DATA): {
-      return comIf->performRegularSendOperation(cookie, sendData, sendLen);
-    }
-    case (ADIS1650X::READ_OUT_CONFIG):
-    default: {
-      ReturnValue_t result = returnvalue::OK;
-      int retval = 0;
-      // Prepare transfer
-      int fileDescriptor = 0;
-      std::string device = comIf->getSpiDev();
-      UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
-      if (fileHelper.getOpenResult() != returnvalue::OK) {
-        return SpiComIF::OPENING_FILE_FAILED;
-      }
-      spi::SpiModes spiMode = spi::SpiModes::MODE_0;
-      uint32_t spiSpeed = 0;
-      cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
-      comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
-      cookie->assignWriteBuffer(sendData);
-      cookie->setTransferSize(2);
-
-      gpioId_t gpioId = cookie->getChipSelectPin();
-      GpioIF &gpioIF = comIf->getGpioInterface();
-      MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
-      uint32_t timeoutMs = 0;
-      MutexIF *mutex = comIf->getCsMutex();
-      cookie->getMutexParams(timeoutType, timeoutMs);
-      if (mutex == nullptr) {
-#if OBSW_VERBOSE_LEVEL >= 1
-        sif::warning << "GyroADIS16507Handler::spiSendCallback: "
-                        "Mutex or GPIO interface invalid"
-                     << std::endl;
-        return returnvalue::FAILED;
-#endif
-      }
-
-      if (gpioId != gpio::NO_GPIO) {
-        result = mutex->lockMutex(timeoutType, timeoutMs);
-        if (result != returnvalue::OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-          sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
-#endif
-          return result;
-        }
-      }
-
-      size_t idx = 0;
-      spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
-      uint64_t origTx = transferStruct->tx_buf;
-      uint64_t origRx = transferStruct->rx_buf;
-      while (idx < sendLen) {
-        // Pull SPI CS low. For now, no support for active high given
-        if (gpioId != gpio::NO_GPIO) {
-          gpioIF.pullLow(gpioId);
-        }
-
-        // Execute transfer
-        // Initiate a full duplex SPI transfer.
-        retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
-        if (retval < 0) {
-          utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
-          result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
-        }
-#if FSFW_HAL_SPI_WIRETAPPING == 1
-        comIf->performSpiWiretapping(cookie);
-#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
-
-        if (gpioId != gpio::NO_GPIO) {
-          gpioIF.pullHigh(gpioId);
-        }
-
-        idx += 2;
-        if (idx < sendLen) {
-          usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
-        }
-
-        transferStruct->tx_buf += 2;
-        transferStruct->rx_buf += 2;
-      }
-      transferStruct->tx_buf = origTx;
-      transferStruct->rx_buf = origRx;
-      if (gpioId != gpio::NO_GPIO) {
-        mutex->unlockMutex();
-      }
-    }
-  }
-  return returnvalue::OK;
-}
-
-void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
-
-void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
-  periodicPrintout = enable;
-  debugDivider.setDivider(divider);
-}
-
-#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */

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;

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;

mission/devices/ImtqHandler.cpp

@@ -53,6 +53,13 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
 
 ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
   uint8_t dhbOpCode = DeviceHandlerIF::PERFORM_OPERATION;
+  auto actuateStep = [&]() {
+    if (ignoreActForRestOfComSteps) {
+      requestStep = imtq::RequestType::DO_NOTHING;
+    } else {
+      requestStep = imtq::RequestType::ACTUATE;
+    }
+  };
   switch (static_cast<imtq::ComStep>(opCode)) {
     case (imtq::ComStep::DHB_OP): {
       break;
@@ -78,22 +85,38 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
       break;
     }
     case (imtq::ComStep::START_ACTUATE_SEND): {
-      requestStep = imtq::RequestType::ACTUATE;
+      if (manualTorqueCmdActive) {
+        if (manuallyCommandedTorqueDuration.isBusy()) {
+          ignoreActForRestOfComSteps = true;
+          requestStep = imtq::RequestType::DO_NOTHING;
+        } else {
+          manualTorqueCmdActive = false;
+          PoolReadGuard pg(&dipoleSet);
+          dipoleSet.dipoles[0] = 0;
+          dipoleSet.dipoles[1] = 0;
+          dipoleSet.dipoles[2] = 0;
+          dipoleSet.currentTorqueDurationMs = 0;
+          requestStep = imtq::RequestType::ACTUATE;
+          ignoreActForRestOfComSteps = false;
+        }
+      } else {
+        requestStep = imtq::RequestType::ACTUATE;
+      }
       dhbOpCode = DeviceHandlerIF::SEND_WRITE;
       break;
     }
     case (imtq::ComStep::START_ACTUATE_GET): {
-      requestStep = imtq::RequestType::ACTUATE;
+      actuateStep();
       dhbOpCode = DeviceHandlerIF::GET_WRITE;
       break;
     }
     case (imtq::ComStep::READ_ACTUATE_SEND): {
-      requestStep = imtq::RequestType::ACTUATE;
+      actuateStep();
       dhbOpCode = DeviceHandlerIF::SEND_READ;
       break;
     }
     case (imtq::ComStep::READ_ACTUATE_GET): {
-      requestStep = imtq::RequestType::ACTUATE;
+      actuateStep();
       dhbOpCode = DeviceHandlerIF::GET_READ;
       break;
     }
@@ -108,18 +131,37 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
 ImtqHandler::~ImtqHandler() = default;
 
 void ImtqHandler::doStartUp() {
-  updatePeriodicReply(true, imtq::cmdIds::REPLY);
-  if (goToNormalMode) {
-    setMode(MODE_NORMAL);
-  } else {
-    setMode(_MODE_TO_ON);
+  if (internalState != InternalState::STARTUP) {
+    commandExecuted = false;
+    updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
+    updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
+    internalState = InternalState::STARTUP;
+  }
+  if (internalState == InternalState::STARTUP) {
+    if (commandExecuted) {
+      if (goToNormalMode) {
+        setMode(MODE_NORMAL);
+      } else {
+        setMode(_MODE_TO_ON);
+      }
+      commandExecuted = false;
+    }
   }
 }
 
 void ImtqHandler::doShutDown() {
-  updatePeriodicReply(false, imtq::cmdIds::REPLY);
+  updatePeriodicReply(false, imtq::cmdIds::REPLY_NO_TORQUE);
+  updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE);
   specialRequestActive = false;
   firstReplyCycle = true;
+  internalState = InternalState::NONE;
+  commandExecuted = false;
+  statusSet.setValidity(false, true);
+  rawMtmNoTorque.setValidity(false, true);
+  rawMtmWithTorque.setValidity(false, true);
+  hkDatasetNoTorque.setValidity(false, true);
+  hkDatasetWithTorque.setValidity(false, true);
+  calMtmMeasurementSet.setValidity(false, true);
   setMode(_MODE_POWER_DOWN);
 }
 
@@ -133,11 +175,24 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
       *id = imtq::cmdIds::START_ACTUATION_DIPOLE;
       return buildCommandFromCommand(*id, nullptr, 0);
     }
+    default: {
+      *id = imtq::cmdIds::REQUEST;
+      request.request = imtq::RequestType::DO_NOTHING;
+      request.specialRequest = imtq::SpecialRequest::NONE;
+      expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
+      rawPacket = reinterpret_cast<uint8_t*>(&request);
+      rawPacketLen = sizeof(imtq::Request);
+      return returnvalue::OK;
+    }
   }
   return NOTHING_TO_SEND;
 }
 
 ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
+  if (internalState == InternalState::STARTUP) {
+    *id = imtq::cmdIds::REQUEST;
+    return buildCommandFromCommand(*id, nullptr, 0);
+  }
   return NOTHING_TO_SEND;
 }
 
@@ -145,11 +200,12 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
                                                    const uint8_t* commandData,
                                                    size_t commandDataLen) {
   auto genericSpecialRequest = [&](imtq::SpecialRequest specialRequest) {
-    ImtqRequest request(commandBuffer, sizeof(commandBuffer));
-    request.setMeasureRequest(specialRequest);
+    request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
+    request.specialRequest = specialRequest;
+    expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
     specialRequestActive = true;
-    rawPacket = commandBuffer;
-    rawPacketLen = ImtqRequest::REQUEST_LEN;
+    rawPacket = reinterpret_cast<uint8_t*>(&request);
+    rawPacketLen = sizeof(imtq::Request);
   };
   switch (deviceCommand) {
     case (imtq::cmdIds::POS_X_SELF_TEST): {
@@ -181,44 +237,50 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
       return returnvalue::OK;
     }
     case (imtq::cmdIds::REQUEST): {
-      ImtqRequest request(commandBuffer, sizeof(commandBuffer));
-      request.setMeasureRequest(imtq::SpecialRequest::NONE);
-      rawPacket = commandBuffer;
-      rawPacketLen = ImtqRequest::REQUEST_LEN;
+      request.request = imtq::RequestType::MEASURE_NO_ACTUATION;
+      request.specialRequest = imtq::SpecialRequest::NONE;
+      expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
+      rawPacket = reinterpret_cast<uint8_t*>(&request);
+      rawPacketLen = sizeof(imtq::Request);
       return returnvalue::OK;
     }
     case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
-      /* IMTQ expects low byte first */
-      // commandBuffer[0] = imtq::CC::START_ACTUATION_DIPOLE;
       if (commandData != nullptr && commandDataLen < 8) {
         return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
       }
-      ImtqRequest request(commandBuffer, sizeof(commandBuffer));
-      // Commands override anything which was set in the software
-      if (commandData != nullptr) {
-        // Read set dipole values from local pool
+      {
+        // Do this in any case to read values which might be commanded by the ACS controller.
         PoolReadGuard pg(&dipoleSet);
-
-        int16_t xDipole = 0, yDipole = 0, zDipole = 0;
-        uint16_t torqueDuration = 0;
-        dipoleSet.xDipole = xDipole;
-        dipoleSet.yDipole = yDipole;
-        dipoleSet.zDipole = zDipole;
-        dipoleSet.currentTorqueDurationMs = torqueDuration;
+        // Commands override anything which was set in the software
+        if (commandData != nullptr) {
+          dipoleSet.setValidityBufferGeneration(false);
+          ReturnValue_t result = dipoleSet.deSerialize(&commandData, &commandDataLen,
+                                                       SerializeIF::Endianness::NETWORK);
+          dipoleSet.setValidityBufferGeneration(true);
+          if (result != returnvalue::OK) {
+            return result;
+          }
+          manualTorqueCmdActive = true;
+          manuallyCommandedTorqueDuration.setTimeout(dipoleSet.currentTorqueDurationMs.value);
+        }
       }
 
-      request.setActuateRequest(dipoleSet.xDipole.value, dipoleSet.yDipole.value,
-                                dipoleSet.zDipole.value, dipoleSet.currentTorqueDurationMs.value);
+      expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
+      request.request = imtq::RequestType::ACTUATE;
+      request.specialRequest = imtq::SpecialRequest::NONE;
+      std::memcpy(request.dipoles, dipoleSet.dipoles.value, sizeof(request.dipoles));
+      request.torqueDuration = dipoleSet.currentTorqueDurationMs.value;
       if (ACTUATION_WIRETAPPING) {
-        sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
-                   << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
+        sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.dipoles[0]
+                   << ", y = " << dipoleSet.dipoles[1] << ", z = " << dipoleSet.dipoles[2]
                    << ", 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;
-      rawPacketLen = ImtqRequest::REQUEST_LEN;
+      rawPacket = reinterpret_cast<uint8_t*>(&request);
+      rawPacketLen = sizeof(imtq::Request);
       return returnvalue::OK;
     }
     default:
@@ -230,7 +292,8 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
 void ImtqHandler::fillCommandAndReplyMap() {
   insertInCommandMap(imtq::cmdIds::REQUEST);
   insertInCommandMap(imtq::cmdIds::START_ACTUATION_DIPOLE);
-  insertInReplyMap(imtq::cmdIds::REPLY, 5, nullptr, 0, true);
+  insertInReplyMap(imtq::cmdIds::REPLY_NO_TORQUE, 5, nullptr, 0, true);
+  insertInReplyMap(imtq::cmdIds::REPLY_WITH_TORQUE, 20, nullptr, 0, true);
   insertInCommandMap(imtq::cmdIds::POS_X_SELF_TEST);
   insertInCommandMap(imtq::cmdIds::NEG_X_SELF_TEST);
   insertInCommandMap(imtq::cmdIds::POS_Y_SELF_TEST);
@@ -242,12 +305,12 @@ void ImtqHandler::fillCommandAndReplyMap() {
 
 ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                         DeviceCommandId_t* foundId, size_t* foundLen) {
-  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
+  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or getMode() == _MODE_POWER_DOWN) {
     return IGNORE_FULL_PACKET;
   }
   if (remainingSize > 0) {
     *foundLen = remainingSize;
-    *foundId = imtq::cmdIds::REPLY;
+    *foundId = expectedReply;
     return returnvalue::OK;
   }
   return returnvalue::FAILED;
@@ -257,14 +320,20 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
   ReturnValue_t result;
   ReturnValue_t status = returnvalue::OK;
   if (getMode() != MODE_NORMAL) {
-    // Ignore replies during transitions.
+    if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
+      ImtqRepliesDefault replies(packet);
+      if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
+        commandExecuted = true;
+      }
+    }
     return returnvalue::OK;
   }
-  // arrayprinter::print(packet, ImtqReplies::BASE_LEN);
-  if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) {
-    requestStep = imtq::RequestType::ACTUATE;
+  if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
     // sif::debug << "handle measure" << std::endl;
     ImtqRepliesDefault replies(packet);
+    if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
+      commandExecuted = true;
+    }
     if (specialRequestActive) {
       if (replies.wasSpecialRequestRead()) {
         uint8_t* specialRequest = replies.getSpecialRequest();
@@ -307,29 +376,29 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     }
 
     if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
-      sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
+      sif::warning << "IMTQ: Possible timing issue, raw MGM measurement was not read" << std::endl;
     }
     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;
     }
 
     if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) {
-      sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
+      sif::warning << "IMTQ: Possible timing issue, calib MGM measurement was not read"
+                   << std::endl;
     }
     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;
     }
-  } else {
+  } else if (expectedReply == imtq::cmdIds::REPLY_WITH_TORQUE) {
     // sif::debug << "handle measure with torque" << std::endl;
-    requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
     ImtqRepliesWithTorque replies(packet);
     if (replies.wasDipoleActuationRead()) {
       parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
@@ -345,7 +414,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 +430,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     } else {
       status = result;
     }
-    fillEngHkDataset(hkDatasetNoTorque, engHkReply);
+    fillEngHkDataset(hkDatasetWithTorque, engHkReply);
     if (firstReplyCycle) {
       firstReplyCycle = false;
     }
@@ -374,6 +443,8 @@ LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
     return &hkDatasetNoTorque;
   } else if (sid == dipoleSet.getSid()) {
     return &dipoleSet;
+  } else if (sid == statusSet.getSid()) {
+    return &statusSet;
   } else if (sid == hkDatasetWithTorque.getSid()) {
     return &hkDatasetWithTorque;
   } else if (sid == rawMtmWithTorque.getSid()) {
@@ -395,7 +466,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;
   }
 }
@@ -409,21 +480,26 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
   localDataPoolMap.emplace(imtq::STATUS_BYTE_CONF, &statusConfig);
   localDataPoolMap.emplace(imtq::STATUS_BYTE_ERROR, &statusError);
   localDataPoolMap.emplace(imtq::STATUS_BYTE_UPTIME, &statusUptime);
+
+  // ENG HK No Torque
   localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
   localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
   localDataPoolMap.emplace(imtq::DIGITAL_CURRENT, new PoolEntry<float>({0}));
   localDataPoolMap.emplace(imtq::ANALOG_CURRENT, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(imtq::COIL_X_CURRENT, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(imtq::COIL_Y_CURRENT, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(imtq::COIL_Z_CURRENT, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(imtq::COIL_X_TEMPERATURE, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(imtq::COIL_Y_TEMPERATURE, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(imtq::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(imtq::COIL_CURRENTS, &coilCurrentsMilliampsNoTorque);
+  localDataPoolMap.emplace(imtq::COIL_TEMPERATURES, &coilTempsNoTorque);
   localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
 
-  localDataPoolMap.emplace(imtq::DIPOLES_X, &dipoleXEntry);
-  localDataPoolMap.emplace(imtq::DIPOLES_Y, &dipoleYEntry);
-  localDataPoolMap.emplace(imtq::DIPOLES_Z, &dipoleZEntry);
+  // ENG HK With Torque
+  localDataPoolMap.emplace(imtq::DIGITAL_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(imtq::ANALOG_VOLTAGE_MV_WT, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(imtq::DIGITAL_CURRENT_WT, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(imtq::ANALOG_CURRENT_WT, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(imtq::COIL_CURRENTS_WT, &coilCurrentsMilliampsWithTorque);
+  localDataPoolMap.emplace(imtq::COIL_TEMPERATURES_WT, &coilTempsWithTorque);
+  localDataPoolMap.emplace(imtq::MCU_TEMPERATURE_WT, new PoolEntry<int16_t>({0}));
+
+  localDataPoolMap.emplace(imtq::DIPOLES_ID, &dipolesPoolEntry);
   localDataPoolMap.emplace(imtq::CURRENT_TORQUE_DURATION, &torqueDurationEntry);
 
   /** Entries of calibrated MTM measurement dataset */
@@ -431,8 +507,11 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
   localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
 
   /** Entries of raw MTM measurement dataset */
-  localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry<float>(3));
-  localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
+  localDataPoolMap.emplace(imtq::MTM_RAW, &mtmRawNoTorque);
+  localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, &actStatusNoTorque);
+
+  localDataPoolMap.emplace(imtq::MTM_RAW_WT, &mtmRawWithTorque);
+  localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS_WT, &actStatusWithTorque);
 
   /** INIT measurements for positive X axis test */
   localDataPoolMap.emplace(imtq::INIT_POS_X_ERR, new PoolEntry<uint8_t>({0}));
@@ -708,6 +787,10 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
       subdp::DiagnosticsHkPeriodicParams(rawMtmWithTorque.getSid(), false, 10.0));
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(statusSet.getSid(), false, 10.0));
+  poolManager.subscribeForDiagPeriodicPacket(
+      subdp::DiagnosticsHkPeriodicParams(dipoleSet.getSid(), false, 10.0));
   return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
 }
 
@@ -731,7 +814,7 @@ ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
 }
 
 ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t* packet) {
-  uint8_t cmdErrorField = *(packet + 1) & 0xF;
+  uint8_t cmdErrorField = packet[1] & 0xF;
   if (cmdErrorField == 0) {
     return returnvalue::OK;
   }
@@ -754,16 +837,20 @@ ReturnValue_t ImtqHandler::parseStatusByte(imtq::CC::CC command, const uint8_t*
                  << " has invalid parameter" << std::endl;
       return imtq::PARAMETER_INVALID;
     case 5:
-      sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << " unavailable" << std::endl;
+      sif::error << "IMTQ::parseStatusByte: CC 0x" << std::setw(2) << command << " unavailable"
+                 << std::endl;
       return imtq::CC_UNAVAILABLE;
     case 7:
-      sif::error << "IMTQ::parseStatusByte: IMTQ replied internal processing error" << std::endl;
+      sif::error << "IMTQ::parseStatusByte: Internal processing error for command 0x"
+                 << std::setw(2) << command << std::endl;
       return imtq::INTERNAL_PROCESSING_ERROR;
     default:
-      sif::error << "IMTQ::parseStatusByte: CMD Error field contains unknown error code 0x"
-                 << static_cast<int>(cmdErrorField) << std::endl;
+      sif::error << "IMTQ::parseStatusByte: CMD error field for command 0x" << std::setw(2)
+                 << command << " contains unknown error code 0x" << static_cast<int>(cmdErrorField)
+                 << std::endl;
       return imtq::CMD_ERR_UNKNOWN;
   }
+  sif::error << std::dec;
 }
 
 void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet) {
@@ -777,20 +864,20 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
   offset += 2;
   hkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
   offset += 2;
-  hkDataset.coilXCurrentmA =
+  hkDataset.coilCurrentsMilliamps[0] =
       static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
   offset += 2;
-  hkDataset.coilYCurrentmA =
+  hkDataset.coilCurrentsMilliamps[1] =
       static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
   offset += 2;
-  hkDataset.coilZCurrentmA =
+  hkDataset.coilCurrentsMilliamps[2] =
       static_cast<int16_t>(*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
   offset += 2;
-  hkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
+  hkDataset.coilTemperatures[0] = (*(packet + offset + 1) << 8 | *(packet + offset));
   offset += 2;
-  hkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
+  hkDataset.coilTemperatures[1] = (*(packet + offset + 1) << 8 | *(packet + offset));
   offset += 2;
-  hkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
+  hkDataset.coilTemperatures[2] = (*(packet + offset + 1) << 8 | *(packet + offset));
   offset += 2;
   size_t dummy = 2;
   SerializeAdapter::deSerialize(&hkDataset.mcuTemperature.value, packet + offset, &dummy,
@@ -804,12 +891,15 @@ void ImtqHandler::fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* pa
     sif::info << "IMTQ analog voltage: " << hkDataset.analogVoltageMv << " mV" << std::endl;
     sif::info << "IMTQ digital current: " << hkDataset.digitalCurrentmA << " mA" << std::endl;
     sif::info << "IMTQ analog current: " << hkDataset.analogCurrentmA << " mA" << std::endl;
-    sif::info << "IMTQ coil X current: " << hkDataset.coilXCurrentmA << " mA" << std::endl;
-    sif::info << "IMTQ coil Y current: " << hkDataset.coilYCurrentmA << " mA" << std::endl;
-    sif::info << "IMTQ coil Z current: " << hkDataset.coilZCurrentmA << " mA" << std::endl;
-    sif::info << "IMTQ coil X temperature: " << hkDataset.coilXTemperature << " °C" << std::endl;
-    sif::info << "IMTQ coil Y temperature: " << hkDataset.coilYTemperature << " °C" << std::endl;
-    sif::info << "IMTQ coil Z temperature: " << hkDataset.coilZTemperature << " °C" << std::endl;
+    sif::info << "IMTQ coil X current: " << hkDataset.coilCurrentsMilliamps[0] << " mA"
+              << std::endl;
+    sif::info << "IMTQ coil Y current: " << hkDataset.coilCurrentsMilliamps[1] << " mA"
+              << std::endl;
+    sif::info << "IMTQ coil Z current: " << hkDataset.coilCurrentsMilliamps[2] << " mA"
+              << std::endl;
+    sif::info << "IMTQ coil X temperature: " << hkDataset.coilTemperatures[0] << " °C" << std::endl;
+    sif::info << "IMTQ coil Y temperature: " << hkDataset.coilTemperatures[1] << " °C" << std::endl;
+    sif::info << "IMTQ coil Z temperature: " << hkDataset.coilTemperatures[2] << " °C" << std::endl;
     sif::info << "IMTQ coil MCU temperature: " << hkDataset.mcuTemperature << " °C" << std::endl;
 #endif
   }
@@ -847,8 +937,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: Read failure" << std::endl;
+  }
   unsigned int offset = 2;
   size_t deSerLen = 16;
   const uint8_t* dataStart = packet + offset;
@@ -876,18 +969,19 @@ 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 << "Set ID: " << set.getSid().ownerSetId << 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
   }
 }

mission/devices/ImtqHandler.h

@@ -83,6 +83,11 @@ class ImtqHandler : public DeviceHandlerBase {
   //! link between IMTQ and OBC.
   static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);
 
+  enum class InternalState { NONE, STARTUP, SHUTDOWN } internalState = InternalState::NONE;
+  bool commandExecuted = false;
+
+  imtq::Request request{};
+
   imtq::StatusDataset statusSet;
   imtq::DipoleActuationSet dipoleSet;
   imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
@@ -98,6 +103,9 @@ class ImtqHandler : public DeviceHandlerBase {
   imtq::NegYSelfTestSet negYselfTestDataset;
   imtq::PosZSelfTestSet posZselfTestDataset;
   imtq::NegZSelfTestSet negZselfTestDataset;
+  bool manualTorqueCmdActive = false;
+  bool ignoreActForRestOfComSteps = false;
+  Countdown manuallyCommandedTorqueDuration = Countdown();
 
   NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
 
@@ -107,13 +115,20 @@ class ImtqHandler : public DeviceHandlerBase {
   PoolEntry<uint32_t> statusUptime = PoolEntry<uint32_t>({0});
 
   PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
-  PoolEntry<int16_t> dipoleXEntry = PoolEntry<int16_t>(0, false);
-  PoolEntry<int16_t> dipoleYEntry = PoolEntry<int16_t>(0, false);
-  PoolEntry<int16_t> dipoleZEntry = PoolEntry<int16_t>(0, false);
-  PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>(0, false);
+  PoolEntry<int16_t> dipolesPoolEntry = PoolEntry<int16_t>({0, 0, 0}, false);
+  PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>({0}, false);
+  PoolEntry<float> coilCurrentsMilliampsNoTorque = PoolEntry<float>(3);
+  PoolEntry<float> coilCurrentsMilliampsWithTorque = PoolEntry<float>(3);
+  PoolEntry<int16_t> coilTempsNoTorque = PoolEntry<int16_t>(3);
+  PoolEntry<int16_t> coilTempsWithTorque = PoolEntry<int16_t>(3);
+  PoolEntry<float> mtmRawNoTorque = PoolEntry<float>(3);
+  PoolEntry<uint8_t> actStatusNoTorque = PoolEntry<uint8_t>(1);
+  PoolEntry<float> mtmRawWithTorque = PoolEntry<float>(3);
+  PoolEntry<uint8_t> actStatusWithTorque = PoolEntry<uint8_t>(1);
+
   power::Switch_t switcher = power::NO_SWITCH;
 
-  uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
+  DeviceCommandId_t expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
   bool goToNormalMode = false;
   bool debugMode = false;
   bool specialRequestActive = false;
@@ -159,7 +174,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

mission/devices/LegacySusHandler.cpp

@@ -0,0 +1,233 @@
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/arrayprinter.h>
+#include <mission/devices/LegacySusHandler.h>
+
+#include "OBSWConfig.h"
+
+LegacySusHandler::LegacySusHandler(object_id_t objectId, uint8_t susIdx, object_id_t comIF,
+                                   CookieIF *comCookie)
+    : DeviceHandlerBase(objectId, comIF, comCookie), divider(5), dataset(this), susIdx(susIdx) {}
+
+LegacySusHandler::~LegacySusHandler() {}
+
+void LegacySusHandler::doStartUp() {
+  if (comState == ComStates::IDLE) {
+    comState = ComStates::WRITE_SETUP;
+    commandExecuted = false;
+  }
+  if (comState == ComStates::WRITE_SETUP) {
+    if (commandExecuted) {
+      if (goToNormalModeImmediately) {
+        setMode(MODE_NORMAL);
+      } else {
+        setMode(_MODE_TO_ON);
+      }
+      commandExecuted = false;
+      if (clkMode == ClkModes::INT_CLOCKED) {
+        comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
+      } else {
+        comState = ComStates::EXT_CLOCKED_CONVERSIONS;
+      }
+    }
+  }
+}
+
+void LegacySusHandler::doShutDown() {
+  setMode(_MODE_POWER_DOWN);
+  comState = ComStates::IDLE;
+}
+
+ReturnValue_t LegacySusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
+  switch (comState) {
+    case (ComStates::IDLE): {
+      break;
+    }
+    case (ComStates::WRITE_SETUP): {
+      *id = susMax1227::WRITE_SETUP;
+      return buildCommandFromCommand(*id, nullptr, 0);
+    }
+    case (ComStates::EXT_CLOCKED_CONVERSIONS): {
+      *id = susMax1227::READ_EXT_TIMED_CONVERSIONS;
+      return buildCommandFromCommand(*id, nullptr, 0);
+    }
+    case (ComStates::START_INT_CLOCKED_CONVERSIONS): {
+      *id = susMax1227::START_INT_TIMED_CONVERSIONS;
+      comState = ComStates::READ_INT_CLOCKED_CONVERSIONS;
+      return buildCommandFromCommand(*id, nullptr, 0);
+    }
+    case (ComStates::READ_INT_CLOCKED_CONVERSIONS): {
+      *id = susMax1227::READ_INT_TIMED_CONVERSIONS;
+      comState = ComStates::START_INT_CLOCKED_CONVERSIONS;
+      return buildCommandFromCommand(*id, nullptr, 0);
+    }
+    case (ComStates::EXT_CLOCKED_TEMP): {
+      *id = susMax1227::READ_EXT_TIMED_TEMPS;
+      return buildCommandFromCommand(*id, nullptr, 0);
+    }
+  }
+  return NOTHING_TO_SEND;
+}
+
+ReturnValue_t LegacySusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+  if (comState == ComStates::WRITE_SETUP) {
+    *id = susMax1227::WRITE_SETUP;
+    return buildCommandFromCommand(*id, nullptr, 0);
+  }
+  return NOTHING_TO_SEND;
+}
+
+ReturnValue_t LegacySusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+                                                        const uint8_t *commandData,
+                                                        size_t commandDataLen) {
+  using namespace max1227;
+  switch (deviceCommand) {
+    case (susMax1227::WRITE_SETUP): {
+      if (clkMode == ClkModes::INT_CLOCKED) {
+        cmdBuffer[0] = susMax1227::SETUP_INT_CLOKED;
+      } else {
+        cmdBuffer[0] = susMax1227::SETUP_EXT_CLOCKED;
+      }
+
+      rawPacket = cmdBuffer;
+      rawPacketLen = 1;
+      break;
+    }
+    case (susMax1227::START_INT_TIMED_CONVERSIONS): {
+      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
+      cmdBuffer[0] = max1227::buildResetByte(true);
+      cmdBuffer[1] = susMax1227::CONVERSION;
+      rawPacket = cmdBuffer;
+      rawPacketLen = 2;
+      break;
+    }
+    case (susMax1227::READ_INT_TIMED_CONVERSIONS): {
+      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
+      rawPacket = cmdBuffer;
+      rawPacketLen = susMax1227::SIZE_READ_INT_CONVERSIONS;
+      break;
+    }
+    case (susMax1227::READ_EXT_TIMED_CONVERSIONS): {
+      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
+      rawPacket = cmdBuffer;
+      for (uint8_t idx = 0; idx < 6; idx++) {
+        cmdBuffer[idx * 2] = buildConvByte(ScanModes::N_ONCE, idx, false);
+        cmdBuffer[idx * 2 + 1] = 0;
+      }
+      cmdBuffer[12] = 0x00;
+      rawPacketLen = susMax1227::SIZE_READ_EXT_CONVERSIONS;
+      break;
+    }
+    case (susMax1227::READ_EXT_TIMED_TEMPS): {
+      cmdBuffer[0] = buildConvByte(ScanModes::N_ONCE, 0, true);
+      std::memset(cmdBuffer + 1, 0, 24);
+      rawPacket = cmdBuffer;
+      rawPacketLen = 25;
+      break;
+    }
+    default:
+      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+  }
+  return returnvalue::OK;
+}
+
+void LegacySusHandler::fillCommandAndReplyMap() {
+  insertInCommandAndReplyMap(susMax1227::WRITE_SETUP, 1);
+  insertInCommandAndReplyMap(susMax1227::START_INT_TIMED_CONVERSIONS, 1);
+  insertInCommandAndReplyMap(susMax1227::READ_INT_TIMED_CONVERSIONS, 1, &dataset,
+                             susMax1227::SIZE_READ_INT_CONVERSIONS);
+  insertInCommandAndReplyMap(susMax1227::READ_EXT_TIMED_CONVERSIONS, 1, &dataset,
+                             susMax1227::SIZE_READ_EXT_CONVERSIONS);
+  insertInCommandAndReplyMap(susMax1227::READ_EXT_TIMED_TEMPS, 1);
+}
+
+ReturnValue_t LegacySusHandler::scanForReply(const uint8_t *start, size_t remainingSize,
+                                             DeviceCommandId_t *foundId, size_t *foundLen) {
+  *foundId = this->getPendingCommand();
+  *foundLen = remainingSize;
+  return returnvalue::OK;
+}
+
+ReturnValue_t LegacySusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
+  switch (id) {
+    case susMax1227::WRITE_SETUP: {
+      if (getMode() == _MODE_START_UP) {
+        commandExecuted = true;
+      }
+      return returnvalue::OK;
+    }
+    case susMax1227::START_INT_TIMED_CONVERSIONS: {
+      return returnvalue::OK;
+    }
+    case susMax1227::READ_INT_TIMED_CONVERSIONS: {
+      PoolReadGuard readSet(&dataset);
+      dataset.tempC = max1227::getTemperature(((packet[0] & 0x0f) << 8) | packet[1]);
+      for (uint8_t idx = 0; idx < 6; idx++) {
+        dataset.channels[idx] = packet[idx * 2 + 2] << 8 | packet[idx * 2 + 3];
+      }
+      dataset.setValidity(true, true);
+      printDataset();
+      break;
+    }
+    case (susMax1227::READ_EXT_TIMED_CONVERSIONS): {
+      PoolReadGuard readSet(&dataset);
+      for (uint8_t idx = 0; idx < 6; idx++) {
+        dataset.channels[idx] = packet[idx * 2 + 1] << 8 | packet[idx * 2 + 2];
+      }
+      dataset.channels.setValid(true);
+      // Read temperature in next read cycle
+      if (clkMode == ClkModes::EXT_CLOCKED_WITH_TEMP) {
+        comState = ComStates::EXT_CLOCKED_TEMP;
+      }
+      printDataset();
+      break;
+    }
+    case (susMax1227::READ_EXT_TIMED_TEMPS): {
+      PoolReadGuard readSet(&dataset);
+      dataset.tempC = max1227::getTemperature(((packet[23] & 0x0f) << 8) | packet[24]);
+      dataset.tempC.setValid(true);
+      comState = ComStates::EXT_CLOCKED_CONVERSIONS;
+      break;
+    }
+    default: {
+      sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
+      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
+    }
+  }
+  return returnvalue::OK;
+}
+
+uint32_t LegacySusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 3000; }
+
+ReturnValue_t LegacySusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                                        LocalDataPoolManager &poolManager) {
+  localDataPoolMap.emplace(susMax1227::TEMPERATURE_C, &tempC);
+  localDataPoolMap.emplace(susMax1227::CHANNEL_VEC, &channelVec);
+  poolManager.subscribeForDiagPeriodicPacket(
+      subdp::DiagnosticsHkPeriodicParams(dataset.getSid(), false, 5.0));
+  return returnvalue::OK;
+}
+
+void LegacySusHandler::setToGoToNormalMode(bool enable) {
+  this->goToNormalModeImmediately = enable;
+}
+
+void LegacySusHandler::printDataset() {
+  if (periodicPrintout) {
+    if (divider.checkAndIncrement()) {
+      sif::info << "SUS ADC " << static_cast<int>(susIdx) << " hex [" << std::setfill('0')
+                << std::hex;
+      for (uint8_t idx = 0; idx < 6; idx++) {
+        sif::info << std::setw(3) << dataset.channels[idx];
+        if (idx < 6 - 1) {
+          sif::info << ",";
+        }
+      }
+      sif::info << "] | T[C] " << std::dec << dataset.tempC.value << std::endl;
+    }
+  }
+}
+
+void LegacySusHandler::enablePeriodicPrintout(bool enable, uint8_t divider) {
+  this->periodicPrintout = enable;
+  this->divider.setDivider(divider);
+}