v1.32.0 version

Reviewed-on: #320

.gitignore

@@ -12,8 +12,13 @@
 #vscode
 /.vscode
 
+# IntelliJ
+/.idea/*
+
 # Python
 __pycache__
-.idea
+
+# CLion
+!/.idea/cmake.xml
 
 generators/*.db

.run/Q7S FM.run.xml

@@ -0,0 +1,10 @@
+<component name="ProjectRunConfigurationManager">
+  <configuration default="false" name="Q7S FM" type="com.jetbrains.cidr.remote.gdbserver.type" factoryName="com.jetbrains.cidr.remote.gdbserver.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="eive-obsw" TARGET_NAME="eive-obsw" CONFIG_NAME="Debug" version="1" RUN_TARGET_PROJECT_NAME="eive-obsw" RUN_TARGET_NAME="eive-obsw">
+    <custom-gdb-server version="1" gdb-connect="localhost:1234" executable="" warmup-ms="0" download-type="NONE" sshConfigName="Q7S FM" uploadFile="/tmp/eive-obsw" defaultGdbServerArgs=":1234 /tmp/eive-obsw">
+      <debugger kind="GDB" isBundled="true" />
+    </custom-gdb-server>
+    <method v="2">
+      <option name="CLION.COMPOUND.BUILD" enabled="true" />
+    </method>
+  </configuration>
+</component>

CHANGELOG.md

@@ -8,26 +8,177 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/).
 The [milestone](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/milestones)
 list yields a list of all related PRs for each release.
 
-Starting at v2.0.0, the following changes will consitute of a breaking
-change warranting a new major release:
+Starting at v2.0.0, this project will adhere to semantic versioning and the the following changes
+will consitute of a breaking change warranting a new major release:
 
 - The TMTC interface changes in any shape of form.
-- The behavour of the OBSW changes in a major shape or form relevant
-  for operations
+- The behaviour of the OBSW changes in a major shape or form relevant for operations
 
 # [unreleased]
 
+# [v1.32.0]
+
+eive-tmtc: v2.16.1
+
+## Fixed
+
+- ADIS1650X: Added missing MDL_RANG pool entry for configuration set
+- Bumped FSFW for bugfix in health service: No execution complete for targeted health announce
+  command.
+- Removed matrix determinant calculation as part of the `MEKF`, which would take about
+  300ms of runtime
+- Resetting the `MEKF` now also actually resets its stored state
+- Bumped FSFW for bugfix in destination handler: Better error handling and able to process
+  destination folder path.
+
+## Changed
+
+- Added basic persistent TM store for PUS telemetry and basic interface to dump and delete
+  telemetry. Implementation is based on a timed rotating files, with the addition that files
+  might be generated more often if the maximum file size of 8192 bytes is exceeded.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/320/files
+- Commented out commanding of actuators as part of the `AcsController`
+- Collection sets of the `AcsController` now get updated before running the actual ACS
+  algorithm
+- `GpsController` now always gets scheduled
+- The `CoreController` now initializes the initial clock from the time file as early as possible
+  (in the constructor) if possible, which should usually be the case.
+
+## Added
+
+- Added basic persistent TM store for PUS telemetry and basic interface to dump and delete
+  telemetry.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/320/files
+- `ExecutableComIfDummy` class to have a dummy for classes like the RTD polling class.
+- Added `AcsController` action command to confirm solar array deployment, which then deletes
+  two files
+- Added `AcsController` action command to reset `MEKF`
+- `GpsCtrlDummy` now initializes the `gpsSet`
+- `RwDummy` now initializes with a non faulty state
+
+
+# [v1.31.1]
+
+## Fixed
+
+- ADIS1650X configuration set was empty because the local pool variables were not registered.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/402
+- ACS Controller: Correction for size of MEKF dataset and some optimization and fixes
+  for actuator control which lead to a crash.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/403
+
+# [v1.31.0]
+
+eive-tmtc: v2.16.0
+
+## Fixed
+
+- Usage of floats as iterators and using them to calculate a uint8_t index in `SusConverter`
+- Removed unused variables in the `AcsController`
+- Remove shadowing variables inside ACS assembly classes.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/385
+
+## Changed
+
+COM PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/364
+
+* Moved transmitter timer and handling of carrier and bitlock event from CCSDS handler to COM
+  subsystem
+* Added parameter command to be able to change the transmitter timeout
+* Solves [#362](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/362)
+* Solves [#360](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/360)
+* Solves [#361](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/361)
+* Solves [#386](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/386)
+- All `targetQuat` functions in `Guidance` now return the target quaternion (target
+  in ECI frame), which is passed on to `CtrlValData`.
+- Moved polling sequence table definitions and source code to `mission/core` folder.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/395
+
+## Added
+
+- `MEKF` now returns an unique returnvalue depending on why the function terminates. These
+  returnvalues are used in the `AcsController` to determine on how to procede with its
+  perform functions. In case the `MEKF` did terminate before estimating the quaternion
+  and rotational rate, an info event will be triggered. Another info event can only be
+  triggered after the `MEKF` has run successfully again. If the `AcsController` tries to
+  perform any pointing mode and the `MEKF` fails, the `performPointingCtrl` function will
+  set the RWs to the last RW speeds and set a zero dipole vector. If the `MEKF` does not
+  recover within 5 cycles (2 mins) the `AcsController` triggers another event, resulting in
+  the `AcsSubsystem` being commanded to `SAFE`.
+- `MekfData` now includes `mekfStatus`
+- `CtrlValData` now includes `tgtRotRate`
+
+# [v1.30.0]
+
+eive-tmtc: v2.14.0
+
+Event IDs for PDEC handler have changed in a breaking manner.
+
+## Added and Fixed
+
+- PDEC: Added basic FDIR to limit the number of allowed TC interrupts and to allow complete task
+  lockups in the case an IRQ is immediately re-raised by the PDEC module. This is done by only
+  allowing a certain number of handled IRQs (whether they yield a valid TC or not) during
+  time windows of one second. Right now, 800 IRQs/TCs are allowed per time window.
+  This time window is reset if a TC reception timeout after 500ms occurs. TBD whether the maximum
+  allowed number will be a configurable parameter. If the number of occured IRQs is exceeded,
+  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]
+
+## Fixed
+
+- Limit number of handled messages for core TM handlers:
+  - https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/391
+  - https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/390
+  - https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/389
+- HeaterHandler better handling for faulty message reception
+  Issue: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/388
+- Disable stopwatch in MAX31865 polling task
+
+# [v1.29.0]
+
+eive-tmtc: v2.13.0
+
+## Changed
+
+- Refactored IMTQ handlers to also perform low level I2C communication tasks in separate thread.
+  This avoids the various delays needed for I2C communication with that device inside the ACS PST.
+  (e.g. 1 ms delay between each transfer, or 10 ms integration delay for MGM measurements).
+
+## Added
+
+- Added new heater info set for the TCS controller. This set contains the heater switch states
+  and the current draw.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/351
+- The HeaterHandler now exposes a mode which reflects whether the heater power
+  is on or off. It also triggers mode events for its heater children objects
+  which show whether the specific heaters are on or off. The heater handler
+  will be part of the TCS tree.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/351
+
 # [v1.28.1]
 
 ## Fixed
 
 - Patch version which compiles for EM
 - CFDP Funnel bugfix: CCSDS wrapping was buggy and works properly now.
+- PDEC: Some adaptions to prevent task lockups on invalid FAR states.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/393
 - CMakeLists.txt fix which broke CI/CD builds when server could not retrieve full git SHA.
+- Possible regression in the MAX31865 polling task: Using a `ManualCsLockGuard` for reconfiguring
+  and then polling the sensor is problematic, invalid sensor values will be read.
+  CS probably needs to be de-asserted or some other HW/SPI specific issue. Letting the SPI ComIF
+  do the locking does the job.
 
 ## Changed
 
 - Add `-Wshadow=local` shadowing warnings and fixed all of them
+- Updated generated CSV files: Support for skip directive and explicit
+  "No description" info string
+- The polling threads for actuator polling now have a slightly higher priority than the ACS PST
+  to ensure timing requirements are met.
 
 ## Added
 
@@ -158,6 +309,7 @@ eive-tmtc: v2.12.2
 ## Changed
 
 - Reworked dummy handling for the TCS controller.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/325
 - Generator scripts now generate files for hosted and for Q7S build.
 
 ## Fixed

CMakeLists.txt

@@ -10,8 +10,8 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 1)
-set(OBSW_VERSION_MINOR 28)
-set(OBSW_VERSION_REVISION 1)
+set(OBSW_VERSION_MINOR 32)
+set(OBSW_VERSION_REVISION 0)
 
 # set(CMAKE_VERBOSE TRUE)
 
@@ -223,6 +223,7 @@ set(LIB_JSON_PATH ${THIRD_PARTY_FOLDER}/json)
 
 set(FSFW_WARNING_SHADOW_LOCAL_GCC OFF)
 set(EIVE_ADD_LINUX_FILES OFF)
+set(FSFW_ADD_TMSTORAGE ON)
 
 # Analyse different OS and architecture/target options, determine BSP_PATH,
 # display information about compiler etc.

README.md

@@ -18,6 +18,7 @@
 11. [Q7S OBC](#q7s)
 12. [Static Code Analysis](#static-code-analysis)
 13. [Eclipse](#eclipse)
+14. [CLion](#clion)
 14. [Running the OBSW on a Raspberry Pi](#rpi)
 15. [Running OBSW on EGSE](#egse)
 16. [Manually preparing sysroots to compile gpsd](#gpsd)
@@ -1229,6 +1230,22 @@ Finally, you can convert the generated `.xml` file to HTML with the following co
 cppcheck-htmlreport --file=report.xml --report-dir=cppcheck --source-dir=..
 ```
 
+# <a id="CLion"></a> CLion
+
+CLion is the recommended IDE for the development of the hosted version of EIVE.
+You can also set up CLion for cross-compilation of the primary OBSW.
+
+There is a shared `.idea/cmake.xml` file to get started with this.
+To make cross-compilation work, two special environment variables
+need to be set:
+
+- `ZYNQ_7020_ROOTFS` pointing to the root filesystem
+- `CROSS_COMPILE` pointing to the the full path of the cross-compiler
+  without the specific tool suffix. For example, if the the cross-compiler
+  tools are located at `/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin`,
+  this variable would be set
+  to `/opt/q7s-gcc/gcc-arm-8.3-2019.03-x86_64-arm-linux-gnueabihf/bin/arm-linux-gnueabihf`
+
 # <a id="eclipse"></a> Eclipse
 
 When using Eclipse, there are two special build variables in the project properties

bsp_hosted/ObjectFactory.cpp

@@ -8,6 +8,7 @@
 #include <mission/tmtc/TmFunnelHandler.h>
 #include <objects/systemObjectList.h>
 
+#include "../mission/utility/DummySdCardManager.h"
 #include "OBSWConfig.h"
 #include "fsfw/platform.h"
 #include "fsfw_tests/integration/task/TestTask.h"
@@ -58,7 +59,8 @@ void ObjectFactory::produce(void* args) {
   Factory::setStaticFrameworkObjectIds();
   PusTmFunnel* pusFunnel;
   CfdpTmFunnel* cfdpFunnel;
-  ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel);
+  auto sdcMan = new DummySdCardManager("/tmp");
+  ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan);
 
   auto* dummyGpioIF = new DummyGpioIF();
   auto* dummySwitcher = new DummyPowerSwitcher(objects::PCDU_HANDLER, 18, 0);

bsp_hosted/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 257 translations.
+ * @brief    Auto-generated event translation file. Contains 263 translations.
  * @details
- * Generated on: 2023-02-21 00:24:44
+ * Generated on: 2023-02-24 16:57:00
  */
 #include "translateEvents.h"
 
@@ -90,9 +90,13 @@ const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER";
 const char *STORE_ERROR_STRING = "STORE_ERROR";
 const char *MSG_QUEUE_ERROR_STRING = "MSG_QUEUE_ERROR";
 const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR";
+const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
+const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
 const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
+const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
+const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -151,6 +155,7 @@ const char *CARRIER_LOCK_STRING = "CARRIER_LOCK";
 const char *BIT_LOCK_PDEC_STRING = "BIT_LOCK_PDEC";
 const char *LOST_CARRIER_LOCK_PDEC_STRING = "LOST_CARRIER_LOCK_PDEC";
 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 *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED";
@@ -250,6 +255,7 @@ 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 *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";
@@ -430,12 +436,20 @@ const char *translateEvents(Event event) {
       return MSG_QUEUE_ERROR_STRING;
     case (10802):
       return SERIALIZATION_ERROR_STRING;
+    case (10803):
+      return FILESTORE_ERROR_STRING;
+    case (10804):
+      return FILENAME_TOO_LARGE_ERROR_STRING;
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
       return SAFE_RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
+    case (11203):
+      return MEKF_INVALID_INFO_STRING;
+    case (11204):
+      return MEKF_INVALID_MODE_VIOLATION_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):
@@ -553,8 +567,10 @@ const char *translateEvents(Event event) {
     case (12406):
       return LOST_BIT_LOCK_PDEC_STRING;
     case (12407):
-      return POLL_SYSCALL_ERROR_PDEC_STRING;
+      return TOO_MANY_IRQS_STRING;
     case (12408):
+      return POLL_SYSCALL_ERROR_PDEC_STRING;
+    case (12409):
       return WRITE_SYSCALL_ERROR_PDEC_STRING;
     case (12500):
       return IMAGE_UPLOAD_FAILED_STRING;
@@ -751,18 +767,20 @@ const char *translateEvents(Event event) {
     case (14006):
       return CURRENT_IMAGE_INFO_STRING;
     case (14100):
+      return POSSIBLE_FILE_CORRUPTION_STRING;
+    case (14200):
       return NO_VALID_SENSOR_TEMPERATURE_STRING;
-    case (14101):
+    case (14201):
       return NO_HEALTHY_HEATER_AVAILABLE_STRING;
-    case (14102):
+    case (14202):
       return SYRLINKS_OVERHEATING_STRING;
-    case (14103):
+    case (14203):
       return PLOC_OVERHEATING_STRING;
-    case (14104):
+    case (14204):
       return OBC_OVERHEATING_STRING;
-    case (14105):
+    case (14205):
       return HPA_OVERHEATING_STRING;
-    case (14106):
+    case (14206):
       return PLPCDU_OVERHEATING_STRING;
     default:
       return "UNKNOWN_EVENT";

bsp_hosted/fsfwconfig/objects/translateObjects.cpp

@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 147 translations.
- * Generated on: 2023-02-21 00:24:44
+ * Contains 154 translations.
+ * Generated on: 2023-02-24 16:57:00
  */
 #include "translateObjects.h"
 
@@ -38,6 +38,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
 const char *RW4_STRING = "RW4";
 const char *STAR_TRACKER_STRING = "STAR_TRACKER";
 const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER";
+const char *IMTQ_POLLING_STRING = "IMTQ_POLLING";
 const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
 const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
 const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
@@ -111,6 +112,7 @@ const char *PUS_SERVICE_5_EVENT_REPORTING_STRING = "PUS_SERVICE_5_EVENT_REPORTIN
 const char *PUS_SERVICE_8_FUNCTION_MGMT_STRING = "PUS_SERVICE_8_FUNCTION_MGMT";
 const char *PUS_SERVICE_9_TIME_MGMT_STRING = "PUS_SERVICE_9_TIME_MGMT";
 const char *PUS_SERVICE_11_TC_SCHEDULER_STRING = "PUS_SERVICE_11_TC_SCHEDULER";
+const char *PUS_SERVICE_15_TM_STORAGE_STRING = "PUS_SERVICE_15_TM_STORAGE";
 const char *PUS_SERVICE_17_TEST_STRING = "PUS_SERVICE_17_TEST";
 const char *PUS_SERVICE_20_PARAMETERS_STRING = "PUS_SERVICE_20_PARAMETERS";
 const char *PUS_SERVICE_200_MODE_MGMT_STRING = "PUS_SERVICE_200_MODE_MGMT";
@@ -149,6 +151,11 @@ const char *ACS_SUBSYSTEM_STRING = "ACS_SUBSYSTEM";
 const char *PL_SUBSYSTEM_STRING = "PL_SUBSYSTEM";
 const char *TCS_SUBSYSTEM_STRING = "TCS_SUBSYSTEM";
 const char *COM_SUBSYSTEM_STRING = "COM_SUBSYSTEM";
+const char *MISC_TM_STORE_STRING = "MISC_TM_STORE";
+const char *OK_TM_STORE_STRING = "OK_TM_STORE";
+const char *NOT_OK_TM_STORE_STRING = "NOT_OK_TM_STORE";
+const char *HK_TM_STORE_STRING = "HK_TM_STORE";
+const char *CFDP_TM_STORE_STRING = "CFDP_TM_STORE";
 const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE";
 const char *THERMAL_TEMP_INSERTER_STRING = "THERMAL_TEMP_INSERTER";
 const char *DUMMY_INTERFACE_STRING = "DUMMY_INTERFACE";
@@ -220,6 +227,8 @@ const char *translateObject(object_id_t object) {
       return STAR_TRACKER_STRING;
     case 0x44130045:
       return GPS_CONTROLLER_STRING;
+    case 0x44140013:
+      return IMTQ_POLLING_STRING;
     case 0x44140014:
       return IMTQ_HANDLER_STRING;
     case 0x442000A1:
@@ -366,6 +375,8 @@ const char *translateObject(object_id_t object) {
       return PUS_SERVICE_9_TIME_MGMT_STRING;
     case 0x53000011:
       return PUS_SERVICE_11_TC_SCHEDULER_STRING;
+    case 0x53000015:
+      return PUS_SERVICE_15_TM_STORAGE_STRING;
     case 0x53000017:
       return PUS_SERVICE_17_TEST_STRING;
     case 0x53000020:
@@ -442,6 +453,16 @@ const char *translateObject(object_id_t object) {
       return TCS_SUBSYSTEM_STRING;
     case 0x73010004:
       return COM_SUBSYSTEM_STRING;
+    case 0x73020001:
+      return MISC_TM_STORE_STRING;
+    case 0x73020002:
+      return OK_TM_STORE_STRING;
+    case 0x73020003:
+      return NOT_OK_TM_STORE_STRING;
+    case 0x73020004:
+      return HK_TM_STORE_STRING;
+    case 0x73030000:
+      return CFDP_TM_STORE_STRING;
     case 0x73500000:
       return CCSDS_IP_CORE_BRIDGE_STRING;
     case 0x90000003:

bsp_hosted/scheduling.cpp

@@ -95,46 +95,43 @@ void scheduling::initTasks() {
     sif::error << "Add component UDP Polling failed" << std::endl;
   }
 
-  /* PUS Services */
-  PeriodicTaskIF* pusVerification = factory->createPeriodicTask(
-      "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
-  result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
+  PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
+      "PUS_HIGH_PRIO", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
+  result = pusHighPrio->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
   if (result != returnvalue::OK) {
     sif::error << "Object add component failed" << std::endl;
   }
-
-  PeriodicTaskIF* eventHandling = factory->createPeriodicTask(
-      "EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
-  result = eventHandling->addComponent(objects::EVENT_MANAGER);
+  result = pusHighPrio->addComponent(objects::EVENT_MANAGER);
   if (result != returnvalue::OK) {
-    scheduling::printAddObjectError("EVENT_MNGR", objects::EVENT_MANAGER);
+    scheduling::printAddObjectError("EVENT_MGMT", objects::EVENT_MANAGER);
   }
-  result = eventHandling->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
+  result = pusHighPrio->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS5", objects::PUS_SERVICE_5_EVENT_REPORTING);
   }
-
-  PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
-      "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
-  result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
-  if (result != returnvalue::OK) {
-    scheduling::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS);
-  }
   result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT);
   }
+
+  PeriodicTaskIF* pusMedPrio = factory->createPeriodicTask(
+      "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc);
+  result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS);
+  }
   result = pusHighPrio->addComponent(objects::PUS_SERVICE_3_HOUSEKEEPING);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS3", objects::PUS_SERVICE_3_HOUSEKEEPING);
   }
-
-  PeriodicTaskIF* pusMedPrio = factory->createPeriodicTask(
-      "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc);
   result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT);
   }
+  result = pusMedPrio->addComponent(objects::PUS_SERVICE_15_TM_STORAGE);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("PUS15", objects::PUS_SERVICE_15_TM_STORAGE);
+  }
   result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT);
@@ -143,10 +140,7 @@ void scheduling::initTasks() {
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS);
   }
-
-  PeriodicTaskIF* pusLowPrio = factory->createPeriodicTask(
-      "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc);
-  result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
+  result = pusMedPrio->addComponent(objects::PUS_SERVICE_17_TEST);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST);
   }
@@ -220,11 +214,8 @@ void scheduling::initTasks() {
   udpPollingTask->startTask();
   tcpPollingTask->startTask();
 
-  pusVerification->startTask();
-  eventHandling->startTask();
   pusHighPrio->startTask();
   pusMedPrio->startTask();
-  pusLowPrio->startTask();
 
   pstTask->startTask();
   thermalTask->startTask();

bsp_linux_board/CMakeLists.txt

@@ -1,5 +1,5 @@
 target_sources(${OBSW_NAME} PUBLIC InitMission.cpp main.cpp gpioInit.cpp
-                                   ObjectFactory.cpp RPiSdCardManager.cpp)
+                                   ObjectFactory.cpp)
 
 add_subdirectory(boardconfig)
 add_subdirectory(boardtest)

bsp_linux_board/ObjectFactory.cpp

@@ -82,7 +82,7 @@ void ObjectFactory::produce(void* args) {
 #endif
 
 #if OBSW_ADD_SCEX_DEVICE == 1
-  auto* sdcMan = new RPiSdCardManager("/tmp");
+  auto* sdcMan = new DummySdCardManager("/tmp");
   createScexComponents(uart::DEV, pwrSwitcher, *sdcMan, true, std::nullopt);
 #endif
 

bsp_linux_board/RPiSdCardManager.cpp

@@ -1,13 +0,0 @@
-#include "RPiSdCardManager.h"
-
-RPiSdCardManager::RPiSdCardManager(std::string prefix) : prefix(std::move(prefix)) {}
-
-const std::string& RPiSdCardManager::getCurrentMountPrefix() const { return prefix; }
-
-bool RPiSdCardManager::isSdCardUsable(sd::SdCard sdCard) { return true; }
-
-std::optional<sd::SdCard> RPiSdCardManager::getPreferredSdCard() const { return std::nullopt; }
-
-void RPiSdCardManager::setActiveSdCard(sd::SdCard sdCard) {}
-
-std::optional<sd::SdCard> RPiSdCardManager::getActiveSdCard() const { return std::nullopt; }

bsp_q7s/core/CoreController.cpp

@@ -49,6 +49,8 @@ CoreController::CoreController(object_id_t objectId)
     }
 
     getCurrentBootCopy(CURRENT_CHIP, CURRENT_COPY);
+
+    initClockFromTimeFile();
   } catch (const std::filesystem::filesystem_error &e) {
     sif::error << "CoreController::CoreController: Failed with exception " << e.what() << std::endl;
   }
@@ -159,6 +161,9 @@ ReturnValue_t CoreController::initializeAfterTaskCreation() {
   }
   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) {
@@ -1266,10 +1271,12 @@ void CoreController::performMountedSdCardOperations() {
       if (result != returnvalue::OK) {
         sif::warning << "CoreController::CoreController: Boot copy init" << std::endl;
       }
-      initClockFromTimeFile();
+      if (not timeFileInitDone) {
+        initClockFromTimeFile();
+      }
       performRebootFileHandling(false);
     }
-    timeFileHandler();
+    backupTimeFileHandler();
   };
   bool someSdCardActive = false;
   if (sdInfo.active == sd::SdCard::SLOT_0 and sdcMan->isSdCardUsable(sd::SdCard::SLOT_0)) {
@@ -1783,7 +1790,7 @@ void CoreController::setRebootMechanismLock(bool lock, xsc::Chip tgtChip, xsc::C
   rewriteRebootFile(rebootFile);
 }
 
-ReturnValue_t CoreController::timeFileHandler() {
+ReturnValue_t CoreController::backupTimeFileHandler() {
   // Always set time. We could only set it if it is updated by GPS, but then the backup time would
   // become obsolete on GPS problems.
   if (opDivider10.check()) {
@@ -1793,14 +1800,14 @@ ReturnValue_t CoreController::timeFileHandler() {
     if (result != returnvalue::OK) {
       return result;
     }
-    std::string fileName = currMntPrefix + TIME_FILE;
+    std::string fileName = currMntPrefix + BACKUP_TIME_FILE;
     std::ofstream timeFile(fileName);
     if (not timeFile.good()) {
       sif::error << "CoreController::timeFileHandler: Error opening time file: " << strerror(errno)
                  << std::endl;
       return returnvalue::FAILED;
     }
-    timeFile << "UNIX SECONDS: " << currentTime.tv_sec << std::endl;
+    timeFile << "UNIX SECONDS: " << currentTime.tv_sec + BOOT_OFFSET_SECONDS << std::endl;
   }
   return returnvalue::OK;
 }
@@ -1808,8 +1815,8 @@ ReturnValue_t CoreController::timeFileHandler() {
 ReturnValue_t CoreController::initClockFromTimeFile() {
   using namespace GpsHyperion;
   using namespace std;
-  std::string fileName = currMntPrefix + TIME_FILE;
-  if (std::filesystem::exists(fileName) and
+  std::string fileName = currMntPrefix + BACKUP_TIME_FILE;
+  if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName) and
       ((gpsFix == FixMode::UNKNOWN or gpsFix == FixMode::NOT_SEEN) or
        not utility::timeSanityCheck())) {
     ifstream timeFile(fileName);
@@ -1837,6 +1844,7 @@ ReturnValue_t CoreController::initClockFromTimeFile() {
     sif::info << "Setting system time from time files: " << std::put_time(time, "%c %Z")
               << std::endl;
 #endif
+    timeFileInitDone = true;
     return Clock::setClock(&currentTime);
   }
   return returnvalue::OK;

bsp_q7s/core/CoreController.h

@@ -58,13 +58,14 @@ class CoreController : public ExtendedControllerBase {
 
   static constexpr char VERSION_FILE_NAME[] = "version.txt";
   static constexpr char REBOOT_FILE_NAME[] = "reboot.txt";
-  static constexpr char TIME_FILE_NAME[] = "time.txt";
+  static constexpr char TIME_FILE_NAME[] = "time_backup.txt";
 
   const std::string VERSION_FILE =
       "/" + std::string(CONF_FOLDER) + "/" + std::string(VERSION_FILE_NAME);
   const std::string REBOOT_FILE =
       "/" + std::string(CONF_FOLDER) + "/" + std::string(REBOOT_FILE_NAME);
-  const std::string TIME_FILE = "/" + std::string(CONF_FOLDER) + "/" + std::string(TIME_FILE_NAME);
+  const std::string BACKUP_TIME_FILE =
+      "/" + std::string(CONF_FOLDER) + "/" + std::string(TIME_FILE_NAME);
 
   static constexpr char CHIP_0_COPY_0_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi0-nom-rootfs";
   static constexpr char CHIP_0_COPY_1_MOUNT_DIR[] = "/tmp/mntupdate-xdi-qspi0-gold-rootfs";
@@ -160,6 +161,7 @@ class CoreController : public ExtendedControllerBase {
   bool sdInitFinished() const;
 
  private:
+  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
@@ -221,6 +223,7 @@ class CoreController : public ExtendedControllerBase {
 
   RebootFile rebootFile = {};
   std::string currMntPrefix;
+  bool timeFileInitDone = false;
   bool performOneShotSdCardOpsSwitch = false;
   uint8_t shortSdCardCdCounter = 0;
 #if OBSW_THREAD_TRACING == 1
@@ -258,7 +261,7 @@ class CoreController : public ExtendedControllerBase {
 
   ReturnValue_t initClockFromTimeFile();
   ReturnValue_t performSdCardCheck();
-  ReturnValue_t timeFileHandler();
+  ReturnValue_t backupTimeFileHandler();
   ReturnValue_t initBootCopyFile();
   ReturnValue_t initWatchdogFifo();
   ReturnValue_t initSdCardBlocking();

bsp_q7s/core/ObjectFactory.cpp

@@ -1,6 +1,7 @@
 #include "ObjectFactory.h"
 
 #include <fsfw/subsystem/Subsystem.h>
+#include <linux/devices/ImtqPollingTask.h>
 #include <linux/devices/RwPollingTask.h>
 #include <mission/system/objects/CamSwitcher.h>
 
@@ -572,7 +573,6 @@ void ObjectFactory::createSyrlinksComponents(PowerSwitchIF* pwrSwitcher) {
       new SyrlinksHandler(objects::SYRLINKS_HANDLER, objects::UART_COM_IF, syrlinksUartCookie,
                           pcdu::PDU1_CH1_SYRLINKS_12V, syrlinksFdir);
   syrlinksHandler->setPowerSwitcher(pwrSwitcher);
-  syrlinksHandler->setStartUpImmediately();
   syrlinksHandler->connectModeTreeParent(satsystem::com::SUBSYSTEM);
 #if OBSW_DEBUG_SYRLINKS == 1
   syrlinksHandler->setDebugMode(true);
@@ -755,15 +755,10 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF,
   AxiPtmeConfig* axiPtmeConfig =
       new AxiPtmeConfig(objects::AXI_PTME_CONFIG, q7s::UIO_PTME, q7s::uiomapids::PTME_CONFIG);
   PtmeConfig* ptmeConfig = new PtmeConfig(objects::PTME_CONFIG, axiPtmeConfig);
-#if OBSW_ENABLE_SYRLINKS_TRANSMIT_TIMEOUT == 1
-  // Set to high value when not sending via syrlinks
-  static const uint32_t TRANSMITTER_TIMEOUT = 86400000;  // 1 day
-#else
-  static const uint32_t TRANSMITTER_TIMEOUT = 900000;  // 15 minutes
-#endif
-  *ipCoreHandler = new CcsdsIpCoreHandler(
-      objects::CCSDS_HANDLER, objects::PTME, objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig,
-      gpioComIF, gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA, TRANSMITTER_TIMEOUT);
+
+  *ipCoreHandler = new CcsdsIpCoreHandler(objects::CCSDS_HANDLER, objects::PTME,
+                                          objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig, gpioComIF,
+                                          gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA);
   VirtualChannel* vc = nullptr;
   vc = new VirtualChannel(ccsds::VC0, config::VC0_QUEUE_SIZE, objects::CCSDS_HANDLER);
   (*ipCoreHandler)->addVirtualChannel(ccsds::VC0, vc);
@@ -906,8 +901,9 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) {
 }
 
 void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) {
-  I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE);
-  auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie,
+  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);

bsp_q7s/core/scheduling.cpp

@@ -17,10 +17,10 @@
 #include "fsfw/tasks/FixedTimeslotTaskIF.h"
 #include "fsfw/tasks/PeriodicTaskIF.h"
 #include "fsfw/tasks/TaskFactory.h"
+#include "mission/core/pollingSeqTables.h"
 #include "mission/core/scheduling.h"
 #include "mission/devices/devicedefinitions/Max31865Definitions.h"
 #include "mission/utility/InitMission.h"
-#include "pollingsequence/pollingSequenceFactory.h"
 
 /* This is configured for linux without CR */
 #ifdef PLATFORM_UNIX
@@ -187,23 +187,29 @@ void scheduling::initTasks() {
   }
 #endif
 
-#if OBSW_ADD_GPS_CTRL == 1
   PeriodicTaskIF* gpsTask = factory->createPeriodicTask(
       "GPS_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
   result = gpsTask->addComponent(objects::GPS_CONTROLLER);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER);
   }
-#endif /* OBSW_ADD_GPS_CTRL */
 
 #if OBSW_ADD_RW == 1
   PeriodicTaskIF* rwPolling = factory->createPeriodicTask(
-      "RW_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+      "RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
   result = rwPolling->addComponent(objects::RW_POLLING_TASK);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("RW_POLLING_TASK", objects::RW_POLLING_TASK);
   }
 #endif
+#if OBSW_ADD_MGT == 1
+  PeriodicTaskIF* imtqPolling = factory->createPeriodicTask(
+      "IMTQ_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+  result = imtqPolling->addComponent(objects::IMTQ_POLLING);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("IMTQ_POLLING_TASK", objects::IMTQ_POLLING);
+  }
+#endif
 
   PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
       "ACS_SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
@@ -345,6 +351,9 @@ void scheduling::initTasks() {
 #if OBSW_ADD_SA_DEPL == 1
   solarArrayDeplTask->startTask();
 #endif
+#if OBSW_ADD_MGT == 1
+  imtqPolling->startTask();
+#endif
 
   taskStarter(pstTasks, "PST task vector");
   taskStarter(pusTasks, "PUS task vector");
@@ -367,9 +376,7 @@ void scheduling::initTasks() {
 #if OBSW_ADD_RW == 1
   rwPolling->startTask();
 #endif
-#if OBSW_ADD_GPS_CTRL == 1
   gpsTask->startTask();
-#endif
   acsSysTask->startTask();
   if (not tcsSystemTask->isEmpty()) {
     tcsSystemTask->startTask();
@@ -393,10 +400,10 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
 #ifdef RELEASE_BUILD
   static constexpr float acsPstPeriod = 0.4;
 #else
-  static constexpr float acsPstPeriod = 0.8;
+  static constexpr float acsPstPeriod = 0.4;
 #endif
   FixedTimeslotTaskIF* acsTcsPst = factory.createFixedTimeslotTask(
-      "ACS_TCS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc);
+      "ACS_TCS_PST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc);
   result = pst::pstTcsAndAcs(acsTcsPst, cfg);
   if (result != returnvalue::OK) {
     if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {

bsp_q7s/core/scheduling.h

@@ -1,11 +1,10 @@
 #ifndef BSP_Q7S_INITMISSION_H_
 #define BSP_Q7S_INITMISSION_H_
 
-#include <pollingsequence/pollingSequenceFactory.h>
-
 #include <vector>
 
 #include "fsfw/tasks/definitions.h"
+#include "mission/core/pollingSeqTables.h"
 
 using pst::AcsPstCfg;
 

bsp_q7s/em/emObjectFactory.cpp

@@ -3,6 +3,7 @@
 #include <fsfw/power/DummyPowerSwitcher.h>
 #include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
 #include <mission/system/tree/system.h>
+#include <mission/utility/DummySdCardManager.h>
 
 #include "OBSWConfig.h"
 #include "bsp_q7s/core/CoreController.h"
@@ -22,7 +23,8 @@ void ObjectFactory::produce(void* args) {
   HealthTableIF* healthTable = nullptr;
   PusTmFunnel* pusFunnel = nullptr;
   CfdpTmFunnel* cfdpFunnel = nullptr;
-  ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel);
+  ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel,
+                                       *SdCardManager::instance());
 
   LinuxLibgpioIF* gpioComIF = nullptr;
   SerialComIF* uartComIF = nullptr;

bsp_q7s/fmObjectFactory.cpp

@@ -18,7 +18,8 @@ void ObjectFactory::produce(void* args) {
   HealthTableIF* healthTable = nullptr;
   PusTmFunnel* pusFunnel = nullptr;
   CfdpTmFunnel* cfdpFunnel = nullptr;
-  ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel);
+  ObjectFactory::produceGenericObjects(&healthTable, &pusFunnel, &cfdpFunnel,
+                                       *SdCardManager::instance());
 
   LinuxLibgpioIF* gpioComIF = nullptr;
   SerialComIF* uartComIF = nullptr;

bsp_q7s/fs/SdCardManager.cpp

@@ -410,9 +410,12 @@ ReturnValue_t SdCardManager::updateSdCardStateFile() {
   return result;
 }
 
-const std::string& SdCardManager::getCurrentMountPrefix() const {
+const char* SdCardManager::getCurrentMountPrefix() const {
   MutexGuard mg(mutex);
-  return currentPrefix;
+  if (currentPrefix.has_value()) {
+    return currentPrefix.value().c_str();
+  }
+  return nullptr;
 }
 
 SdCardManager::OpStatus SdCardManager::checkCurrentOp(Operations& currentOp) {

bsp_q7s/fs/SdCardManager.h

@@ -187,7 +187,7 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
    * @param prefSdCardPtr
    * @return
    */
-  const std::string& getCurrentMountPrefix() const override;
+  const char* getCurrentMountPrefix() const override;
 
   OpStatus checkCurrentOp(Operations& currentOp);
 
@@ -232,7 +232,7 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
   void processSdStatusLine(SdStatePair& active, std::string& line, uint8_t& idx,
                            sd::SdCard& currentSd);
 
-  std::string currentPrefix;
+  std::optional<std::string> currentPrefix;
 
   static SdCardManager* INSTANCE;
 };

bsp_q7s/fs/helpers.cpp

@@ -3,6 +3,9 @@
 std::filesystem::path fshelpers::getPrefixedPath(SdCardManager &man,
                                                  std::filesystem::path pathWihtoutPrefix) {
   auto prefix = man.getCurrentMountPrefix();
+  if (prefix == nullptr) {
+    return pathWihtoutPrefix;
+  }
   auto resPath = prefix / pathWihtoutPrefix;
   return resPath;
 }

common/config/eive/definitions.h

@@ -59,17 +59,25 @@ namespace acs {
 
 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_ACS_CTRL_MS = 45;
-static constexpr uint32_t SCHED_BLOCK_4_ACTUATOR_MS = 50;
-static constexpr uint32_t SCHED_BLOCK_5_RW_READ_MS = 300;
+static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42;
+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_RTD = 150;
+static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300;
 
 // 15 ms for FM
 static constexpr float SCHED_BLOCK_1_PERIOD = static_cast<float>(SCHED_BLOCK_1_SUS_READ_MS) / 400.0;
 static constexpr float SCHED_BLOCK_2_PERIOD =
     static_cast<float>(SCHED_BLOCK_2_SENSOR_READ_MS) / 400.0;
-static constexpr float SCHED_BLOCK_3_PERIOD = static_cast<float>(SCHED_BLOCK_3_ACS_CTRL_MS) / 400.0;
-static constexpr float SCHED_BLOCK_4_PERIOD = static_cast<float>(SCHED_BLOCK_4_ACTUATOR_MS) / 400.0;
-static constexpr float SCHED_BLOCK_5_PERIOD = static_cast<float>(SCHED_BLOCK_5_RW_READ_MS) / 400.0;
+static constexpr float SCHED_BLOCK_3_PERIOD =
+    static_cast<float>(SCHED_BLOCK_3_READ_IMTQ_MGM_MS) / 400.0;
+static constexpr float SCHED_BLOCK_4_PERIOD = static_cast<float>(SCHED_BLOCK_4_ACS_CTRL_MS) / 400.0;
+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;
 
 }  // namespace acs
 

common/config/eive/eventSubsystemIds.h

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

common/config/eive/objects.h

@@ -44,6 +44,7 @@ enum commonObjects : uint32_t {
   STAR_TRACKER = 0x44130001,
   GPS_CONTROLLER = 0x44130045,
 
+  IMTQ_POLLING = 0x44140013,
   IMTQ_HANDLER = 0x44140014,
   TMP1075_HANDLER_TCS_0 = 0x44420004,
   TMP1075_HANDLER_TCS_1 = 0x44420005,
@@ -151,6 +152,11 @@ enum commonObjects : uint32_t {
   CFDP_TM_FUNNEL = 0x73000102,
   CFDP_HANDLER = 0x73000205,
   CFDP_DISTRIBUTOR = 0x73000206,
+  MISC_TM_STORE = 0x73020001,
+  OK_TM_STORE = 0x73020002,
+  NOT_OK_TM_STORE = 0x73020003,
+  HK_TM_STORE = 0x73020004,
+  CFDP_TM_STORE = 0x73030000,
 
   // Other stuff
   THERMAL_TEMP_INSERTER = 0x90000003,

common/config/eive/resultClassIds.h

@@ -35,14 +35,13 @@ enum commonClassIds : uint8_t {
   SA_DEPL_HANDLER,          // SADPL
   MPSOC_RETURN_VALUES_IF,   // MPSOCRTVIF
   SUPV_RETURN_VALUES_IF,    // SPVRTVIF
-  ACS_KALMAN,               // ACSKAL
+  ACS_CTRL,                 // ACSCTRL
+  ACS_MEKF,                 // ACSMEKF
   ACS_SAFE,                 // ACSSAF
   ACS_PTG,                  // ACSPTG
   ACS_DETUMBLE,             // ACSDTB
-  ACS_MEKF,                 // ACSMEK
   COMMON_CLASS_ID_END       // [EXPORT] : [END]
 };
-
 }
 
 #endif /* COMMON_CONFIG_COMMONCLASSIDS_H_ */

common/config/tmtc/pusIds.h

@@ -12,6 +12,7 @@ enum Ids {
   PUS_SERVICE_8 = 8,
   PUS_SERVICE_9 = 9,
   PUS_SERVICE_11 = 11,
+  PUS_SERVICE_15 = 15,
   PUS_SERVICE_17 = 17,
   PUS_SERVICE_19 = 19,
   PUS_SERVICE_20 = 20,

dummies/CMakeLists.txt

@@ -20,6 +20,7 @@ target_sources(
          GyroL3GD20Dummy.cpp
          MgmLIS3MDLDummy.cpp
          PlPcduDummy.cpp
+         ExecutableComIfDummy.cpp
          ScexDummy.cpp
          CoreControllerDummy.cpp
          PlocMpsocDummy.cpp

dummies/ExecutableComIfDummy.cpp

@@ -0,0 +1,27 @@
+#include <dummies/ExecutableComIfDummy.h>
+
+ExecutableComIfDummy::ExecutableComIfDummy(object_id_t objectId) : SystemObject(objectId) {}
+
+ReturnValue_t ExecutableComIfDummy::initializeInterface(CookieIF *cookie) {
+  return returnvalue::OK;
+}
+
+ReturnValue_t ExecutableComIfDummy::sendMessage(CookieIF *cookie, const uint8_t *sendData,
+                                                size_t sendLen) {
+  return returnvalue::OK;
+}
+
+ReturnValue_t ExecutableComIfDummy::getSendSuccess(CookieIF *cookie) { return returnvalue::OK; }
+
+ReturnValue_t ExecutableComIfDummy::requestReceiveMessage(CookieIF *cookie, size_t requestLen) {
+  return returnvalue::OK;
+}
+
+ReturnValue_t ExecutableComIfDummy::performOperation(uint8_t operationCode) {
+  return returnvalue::OK;
+}
+
+ReturnValue_t ExecutableComIfDummy::readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
+                                                        size_t *size) {
+  return returnvalue::OK;
+}

dummies/ExecutableComIfDummy.h

@@ -0,0 +1,21 @@
+#ifndef DUMMIES_EXECUTABLECOMIFDUMMY_H_
+#define DUMMIES_EXECUTABLECOMIFDUMMY_H_
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+
+class ExecutableComIfDummy : public ExecutableObjectIF,
+                             public DeviceCommunicationIF,
+                             public SystemObject {
+ public:
+  ExecutableComIfDummy(object_id_t objectId);
+  ReturnValue_t performOperation(uint8_t operationCode = 0) override;
+  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;
+};
+
+#endif /* DUMMIES_EXECUTABLECOMIFDUMMY_H_ */

dummies/GpsCtrlDummy.cpp

@@ -1,6 +1,7 @@
 #include "GpsCtrlDummy.h"
 
-GpsCtrlDummy::GpsCtrlDummy(object_id_t objectId) : ExtendedControllerBase(objectId, 20) {}
+GpsCtrlDummy::GpsCtrlDummy(object_id_t objectId)
+    : ExtendedControllerBase(objectId, 20), gpsSet(this) {}
 
 ReturnValue_t GpsCtrlDummy::handleCommandMessage(CommandMessage* message) {
   return returnvalue::OK;
@@ -13,9 +14,23 @@ ReturnValue_t GpsCtrlDummy::checkModeCommand(Mode_t mode, Submode_t submode,
   return returnvalue::OK;
 }
 
+LocalPoolDataSetBase* GpsCtrlDummy::getDataSetHandle(sid_t sid) { return &gpsSet; }
+
 ReturnValue_t GpsCtrlDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                     LocalDataPoolManager& poolManager) {
+  localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry<double>({537222.3469}, true));
+  localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry<double>({-8.8579}, true));
+  localDataPoolMap.emplace(GpsHyperion::LATITUDE, new PoolEntry<double>({49.5952}, true));
+  localDataPoolMap.emplace(GpsHyperion::SPEED, new PoolEntry<double>({0}));
+  localDataPoolMap.emplace(GpsHyperion::YEAR, new PoolEntry<uint16_t>({2023}, true));
+  localDataPoolMap.emplace(GpsHyperion::MONTH, new PoolEntry<uint8_t>({5}, true));
+  localDataPoolMap.emplace(GpsHyperion::DAY, new PoolEntry<uint8_t>({16}, true));
+  localDataPoolMap.emplace(GpsHyperion::HOURS, new PoolEntry<uint8_t>({1}, true));
+  localDataPoolMap.emplace(GpsHyperion::MINUTES, new PoolEntry<uint8_t>({0}, true));
+  localDataPoolMap.emplace(GpsHyperion::SECONDS, new PoolEntry<uint8_t>({0}, true));
+  localDataPoolMap.emplace(GpsHyperion::UNIX_SECONDS, new PoolEntry<uint32_t>({1684191600}, true));
+  localDataPoolMap.emplace(GpsHyperion::SATS_IN_USE, new PoolEntry<uint8_t>());
+  localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry<uint8_t>());
+  localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
   return returnvalue::OK;
 }
-
-LocalPoolDataSetBase* GpsCtrlDummy::getDataSetHandle(sid_t sid) { return nullptr; }

dummies/GpsCtrlDummy.h

@@ -2,12 +2,15 @@
 #define DUMMIES_GPSCTRLDUMMY_H_
 
 #include <fsfw/controller/ExtendedControllerBase.h>
+#include <mission/devices/devicedefinitions/GPSDefinitions.h>
 
 class GpsCtrlDummy : public ExtendedControllerBase {
  public:
   GpsCtrlDummy(object_id_t objectId);
 
  private:
+  GpsPrimaryDataset gpsSet;
+
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;
   void performControlOperation() override;
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,

dummies/ImtqDummy.cpp

@@ -1,6 +1,6 @@
 #include "ImtqDummy.h"
 
-#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 
 ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie) {}
@@ -38,10 +38,10 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
 
 ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0, 0.0, 0.0}));
-  localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
-  localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry<float>({0.12, 0.76, -0.45}, true));
-  localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
+  localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(imtq::MGM_CAL_NT, new PoolEntry<float>({0.0, 0.0, 0.0}));
+  localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
+  localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry<float>({0.12, 0.76, -0.45}, true));
+  localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
   return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
 }

dummies/PduDummy.cpp

@@ -3,7 +3,8 @@
 #include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
 
 PduDummy::PduDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
-    : DeviceHandlerBase(objectId, comif, comCookie) {}
+    : DeviceHandlerBase(objectId, comif, comCookie),
+      coreHk(this, static_cast<uint32_t>(P60System::SetIds::CORE)) {}
 
 PduDummy::~PduDummy() {}
 
@@ -38,5 +39,7 @@ uint32_t PduDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return
 ReturnValue_t PduDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                 LocalDataPoolManager &poolManager) {
   localDataPoolMap.emplace(PDU::pool::PDU_TEMPERATURE, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(PDU::pool::PDU_VOLTAGES, &pduVoltages);
+  localDataPoolMap.emplace(PDU::pool::PDU_CURRENTS, &pduCurrents);
   return returnvalue::OK;
 }

dummies/PduDummy.h

@@ -3,6 +3,8 @@
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
 
+#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
+
 class PduDummy : public DeviceHandlerBase {
  public:
   static const DeviceCommandId_t SIMPLE_COMMAND = 1;
@@ -15,6 +17,10 @@ class PduDummy : public DeviceHandlerBase {
   virtual ~PduDummy();
 
  protected:
+  PDU::PduCoreHk coreHk;
+  PoolEntry<int16_t> pduVoltages = PoolEntry<int16_t>(9);
+  PoolEntry<int16_t> pduCurrents = PoolEntry<int16_t>(9);
+
   void doStartUp() override;
   void doShutDown() override;
   ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;

dummies/RwDummy.cpp

@@ -41,7 +41,7 @@ ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoo
 
   localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry<int32_t>({0}));
   localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(rws::STATE, new PoolEntry<uint8_t>({0}));
+  localDataPoolMap.emplace(rws::STATE, new PoolEntry<uint8_t>({1}, true));
   localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry<uint8_t>({0}));
 
   localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));

dummies/SusDummy.cpp

@@ -37,7 +37,7 @@ ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool &localDataPo
                                                 LocalDataPoolManager &poolManager) {
   localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
   localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
-                           new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0}, true));
+                           new PoolEntry<uint16_t>({2603, 781, 2760, 2048, 4056, 0}, true));
 
   return returnvalue::OK;
 }

dummies/helpers.cpp

@@ -5,6 +5,7 @@
 #include <dummies/ComCookieDummy.h>
 #include <dummies/ComIFDummy.h>
 #include <dummies/CoreControllerDummy.h>
+#include <dummies/ExecutableComIfDummy.h>
 #include <dummies/GpsCtrlDummy.h>
 #include <dummies/GpsDhbDummy.h>
 #include <dummies/GyroAdisDummy.h>
@@ -45,7 +46,7 @@ void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpio
     new CoreControllerDummy(objects::CORE_CONTROLLER);
   }
   if (cfg.addRtdComIFDummy) {
-    new ComIFDummy(objects::SPI_RTD_COM_IF);
+    new ExecutableComIfDummy(objects::SPI_RTD_COM_IF);
   }
   std::array<object_id_t, 4> rwIds = {objects::RW1, objects::RW2, objects::RW3, objects::RW4};
   std::array<DeviceHandlerBase*, 4> rws;

generators/.gitignore

@@ -1,2 +1,4 @@
 .~lock*
 /venv
+/.idea/*
+!/.idea/runConfigurations

generators/bsp_hosted_events.csv

@@ -84,9 +84,13 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 10800;0x2a30;STORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h
 10801;0x2a31;MSG_QUEUE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h
 10802;0x2a32;SERIALIZATION_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h
+10803;0x2a33;FILESTORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h
+10804;0x2a34;FILENAME_TOO_LARGE_ERROR;LOW;P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name;fsfw/src/fsfw/cfdp/handler/defs.h
 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acsDefs.h
 11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acsDefs.h
 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h
+11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h
+11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h
 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h
@@ -145,8 +149,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 12404;0x3074;BIT_LOCK_PDEC;INFO;Bit lock detected (data valid);linux/ipcore/PdecHandler.h
 12405;0x3075;LOST_CARRIER_LOCK_PDEC;INFO;Lost carrier lock;linux/ipcore/PdecHandler.h
 12406;0x3076;LOST_BIT_LOCK_PDEC;INFO;Lost bit lock;linux/ipcore/PdecHandler.h
-12407;0x3077;POLL_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h
-12408;0x3078;WRITE_SYSCALL_ERROR_PDEC;MEDIUM;No description;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
 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
@@ -249,10 +254,11 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 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;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
+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

generators/bsp_hosted_objects.csv

@@ -30,6 +30,7 @@
 0x44120350;RW4
 0x44130001;STAR_TRACKER
 0x44130045;GPS_CONTROLLER
+0x44140013;IMTQ_POLLING
 0x44140014;IMTQ_HANDLER
 0x442000A1;PCDU_HANDLER
 0x44250000;P60DOCK_HANDLER
@@ -103,6 +104,7 @@
 0x53000008;PUS_SERVICE_8_FUNCTION_MGMT
 0x53000009;PUS_SERVICE_9_TIME_MGMT
 0x53000011;PUS_SERVICE_11_TC_SCHEDULER
+0x53000015;PUS_SERVICE_15_TM_STORAGE
 0x53000017;PUS_SERVICE_17_TEST
 0x53000020;PUS_SERVICE_20_PARAMETERS
 0x53000200;PUS_SERVICE_200_MODE_MGMT
@@ -141,6 +143,11 @@
 0x73010002;PL_SUBSYSTEM
 0x73010003;TCS_SUBSYSTEM
 0x73010004;COM_SUBSYSTEM
+0x73020001;MISC_TM_STORE
+0x73020002;OK_TM_STORE
+0x73020003;NOT_OK_TM_STORE
+0x73020004;HK_TM_STORE
+0x73030000;CFDP_TM_STORE
 0x73500000;CCSDS_IP_CORE_BRIDGE
 0x90000003;THERMAL_TEMP_INSERTER
 0xCAFECAFE;DUMMY_INTERFACE

generators/bsp_hosted_returnvalues.csv

@@ -2,7 +2,16 @@ 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
-0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.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
+0x5103;IMTQ_ParameterMissing;No description;3;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5104;IMTQ_ParameterInvalid;No description;4;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5105;IMTQ_CcUnavailable;No description;5;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5106;IMTQ_InternalProcessingError;No description;6;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5107;IMTQ_RejectedWithoutReason;No description;7;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5108;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
 0x52b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
@@ -13,19 +22,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 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
-0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x51a0;IMTQ_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a1;IMTQ_InvalidRampTime;Action Message with invalid ramp time was received.;161;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a2;IMTQ_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a3;IMTQ_ExecutionFailed;Command execution failed;163;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a4;IMTQ_CrcError;Reaction wheel reply has invalid crc;164;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a5;IMTQ_ValueNotRead;No description;165;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a6;IMTQ_CmdErrUnknown;No description;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h
-0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.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
+0x66a3;SADPL_ExecutionFailed;Command execution failed;163;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a4;SADPL_CrcError;Reaction wheel reply has invalid crc;164;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a5;SADPL_ValueNotRead;No description;165;SA_DEPL_HANDLER;mission/devices/RwHandler.h
 0x50a0;SYRLINKS_CrcFailure;No description;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;No description;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
 0x50a2;SYRLINKS_BadCharacterAck;No description;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
@@ -47,12 +49,17 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x4fa4;HEATER_MainSwitchSetTimeout;No description;164;HEATER_HANDLER;mission/devices/HeaterHandler.h
 0x4fa5;HEATER_CommandAlreadyWaiting;No description;165;HEATER_HANDLER;mission/devices/HeaterHandler.h
 0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h
-0x6a01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
-0x6b01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
-0x6c01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
-0x6901;ACSKAL_KalmanNoGyrMeas;No description;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6902;ACSKAL_KalmanNoModel;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6903;ACSKAL_KalmanInversionFailed;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
+0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
+0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
+0x6a02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6900;ACSCTRL_FileDeletionFailed;No description;0;ACS_CTRL;mission/controller/AcsController.h
 0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
 0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
 0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h

generators/bsp_q7s_events.csv

@@ -84,9 +84,13 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 10800;0x2a30;STORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h
 10801;0x2a31;MSG_QUEUE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h
 10802;0x2a32;SERIALIZATION_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h
+10803;0x2a33;FILESTORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h
+10804;0x2a34;FILENAME_TOO_LARGE_ERROR;LOW;P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name;fsfw/src/fsfw/cfdp/handler/defs.h
 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acsDefs.h
 11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acsDefs.h
 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h
+11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h
+11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h
 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h
@@ -145,8 +149,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 12404;0x3074;BIT_LOCK_PDEC;INFO;Bit lock detected (data valid);linux/ipcore/PdecHandler.h
 12405;0x3075;LOST_CARRIER_LOCK_PDEC;INFO;Lost carrier lock;linux/ipcore/PdecHandler.h
 12406;0x3076;LOST_BIT_LOCK_PDEC;INFO;Lost bit lock;linux/ipcore/PdecHandler.h
-12407;0x3077;POLL_SYSCALL_ERROR_PDEC;MEDIUM;No description;linux/ipcore/PdecHandler.h
-12408;0x3078;WRITE_SYSCALL_ERROR_PDEC;MEDIUM;No description;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
 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
@@ -249,10 +254,11 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 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;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
+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

generators/bsp_q7s_objects.csv

@@ -29,6 +29,7 @@
 0x44120350;RW4
 0x44130001;STAR_TRACKER
 0x44130045;GPS_CONTROLLER
+0x44140013;IMTQ_POLLING
 0x44140014;IMTQ_HANDLER
 0x442000A1;PCDU_HANDLER
 0x44250000;P60DOCK_HANDLER
@@ -101,6 +102,7 @@
 0x53000008;PUS_SERVICE_8_FUNCTION_MGMT
 0x53000009;PUS_SERVICE_9_TIME_MGMT
 0x53000011;PUS_SERVICE_11_TC_SCHEDULER
+0x53000015;PUS_SERVICE_15_TM_STORAGE
 0x53000017;PUS_SERVICE_17_TEST
 0x53000020;PUS_SERVICE_20_PARAMETERS
 0x53000200;PUS_SERVICE_200_MODE_MGMT
@@ -147,6 +149,11 @@
 0x73010002;PL_SUBSYSTEM
 0x73010003;TCS_SUBSYSTEM
 0x73010004;COM_SUBSYSTEM
+0x73020001;MISC_TM_STORE
+0x73020002;OK_TM_STORE
+0x73020003;NOT_OK_TM_STORE
+0x73020004;HK_TM_STORE
+0x73030000;CFDP_TM_STORE
 0x73500000;CCSDS_IP_CORE_BRIDGE
 0x90000003;THERMAL_TEMP_INSERTER
 0xFFFFFFFF;NO_OBJECT

generators/bsp_q7s_returnvalues.csv

@@ -2,7 +2,16 @@ 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
-0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.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
+0x5103;IMTQ_ParameterMissing;No description;3;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5104;IMTQ_ParameterInvalid;No description;4;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5105;IMTQ_CcUnavailable;No description;5;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5106;IMTQ_InternalProcessingError;No description;6;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5107;IMTQ_RejectedWithoutReason;No description;7;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5108;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
 0x52b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
@@ -13,19 +22,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 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
-0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x51a0;IMTQ_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a1;IMTQ_InvalidRampTime;Action Message with invalid ramp time was received.;161;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a2;IMTQ_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a3;IMTQ_ExecutionFailed;Command execution failed;163;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a4;IMTQ_CrcError;Reaction wheel reply has invalid crc;164;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a5;IMTQ_ValueNotRead;No description;165;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a6;IMTQ_CmdErrUnknown;No description;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h
-0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.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
+0x66a3;SADPL_ExecutionFailed;Command execution failed;163;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a4;SADPL_CrcError;Reaction wheel reply has invalid crc;164;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a5;SADPL_ValueNotRead;No description;165;SA_DEPL_HANDLER;mission/devices/RwHandler.h
 0x50a0;SYRLINKS_CrcFailure;No description;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;No description;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
 0x50a2;SYRLINKS_BadCharacterAck;No description;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
@@ -47,12 +49,17 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x4fa4;HEATER_MainSwitchSetTimeout;No description;164;HEATER_HANDLER;mission/devices/HeaterHandler.h
 0x4fa5;HEATER_CommandAlreadyWaiting;No description;165;HEATER_HANDLER;mission/devices/HeaterHandler.h
 0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h
-0x6a01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
-0x6b01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
-0x6c01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
-0x6901;ACSKAL_KalmanNoGyrMeas;No description;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6902;ACSKAL_KalmanNoModel;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6903;ACSKAL_KalmanInversionFailed;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
+0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
+0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
+0x6a02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6900;ACSCTRL_FileDeletionFailed;No description;0;ACS_CTRL;mission/controller/AcsController.h
 0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
 0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
 0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
@@ -566,6 +573,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x53b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h
 0x53b7;STRH_StartrackerRunningFirmware;Star tracker is in firmware mode but must be in bootloader mode to execute this command;183;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h
 0x53b8;STRH_StartrackerRunningBootloader;Star tracker is in bootloader mode but must be in firmware mode to execute this command;184;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h
+0x5300;STRH_NoReplyAvailable;No description;0;STR_HANDLER;linux/devices/ImtqPollingTask.h
 0x5302;STRH_InvalidCrc;No description;2;STR_HANDLER;linux/devices/ScexHelper.h
 0x5aa0;PTME_UnknownVcId;No description;160;PTME;linux/ipcore/Ptme.h
 0x5fa0;PDEC_AbandonedCltuRetval;No description;160;PDEC_HANDLER;linux/ipcore/PdecHandler.h

generators/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 257 translations.
+ * @brief    Auto-generated event translation file. Contains 263 translations.
  * @details
- * Generated on: 2023-02-21 00:24:44
+ * Generated on: 2023-02-24 16:57:00
  */
 #include "translateEvents.h"
 
@@ -90,9 +90,13 @@ const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER";
 const char *STORE_ERROR_STRING = "STORE_ERROR";
 const char *MSG_QUEUE_ERROR_STRING = "MSG_QUEUE_ERROR";
 const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR";
+const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
+const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
 const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
+const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
+const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -151,6 +155,7 @@ const char *CARRIER_LOCK_STRING = "CARRIER_LOCK";
 const char *BIT_LOCK_PDEC_STRING = "BIT_LOCK_PDEC";
 const char *LOST_CARRIER_LOCK_PDEC_STRING = "LOST_CARRIER_LOCK_PDEC";
 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 *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED";
@@ -250,6 +255,7 @@ 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 *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";
@@ -430,12 +436,20 @@ const char *translateEvents(Event event) {
       return MSG_QUEUE_ERROR_STRING;
     case (10802):
       return SERIALIZATION_ERROR_STRING;
+    case (10803):
+      return FILESTORE_ERROR_STRING;
+    case (10804):
+      return FILENAME_TOO_LARGE_ERROR_STRING;
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
       return SAFE_RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
+    case (11203):
+      return MEKF_INVALID_INFO_STRING;
+    case (11204):
+      return MEKF_INVALID_MODE_VIOLATION_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):
@@ -553,8 +567,10 @@ const char *translateEvents(Event event) {
     case (12406):
       return LOST_BIT_LOCK_PDEC_STRING;
     case (12407):
-      return POLL_SYSCALL_ERROR_PDEC_STRING;
+      return TOO_MANY_IRQS_STRING;
     case (12408):
+      return POLL_SYSCALL_ERROR_PDEC_STRING;
+    case (12409):
       return WRITE_SYSCALL_ERROR_PDEC_STRING;
     case (12500):
       return IMAGE_UPLOAD_FAILED_STRING;
@@ -751,18 +767,20 @@ const char *translateEvents(Event event) {
     case (14006):
       return CURRENT_IMAGE_INFO_STRING;
     case (14100):
+      return POSSIBLE_FILE_CORRUPTION_STRING;
+    case (14200):
       return NO_VALID_SENSOR_TEMPERATURE_STRING;
-    case (14101):
+    case (14201):
       return NO_HEALTHY_HEATER_AVAILABLE_STRING;
-    case (14102):
+    case (14202):
       return SYRLINKS_OVERHEATING_STRING;
-    case (14103):
+    case (14203):
       return PLOC_OVERHEATING_STRING;
-    case (14104):
+    case (14204):
       return OBC_OVERHEATING_STRING;
-    case (14105):
+    case (14205):
       return HPA_OVERHEATING_STRING;
-    case (14106):
+    case (14206):
       return PLPCDU_OVERHEATING_STRING;
     default:
       return "UNKNOWN_EVENT";

generators/objects/translateObjects.cpp

@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 152 translations.
- * Generated on: 2023-02-21 00:24:44
+ * Contains 159 translations.
+ * Generated on: 2023-02-24 16:57:00
  */
 #include "translateObjects.h"
 
@@ -37,6 +37,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
 const char *RW4_STRING = "RW4";
 const char *STAR_TRACKER_STRING = "STAR_TRACKER";
 const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER";
+const char *IMTQ_POLLING_STRING = "IMTQ_POLLING";
 const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
 const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
 const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
@@ -109,6 +110,7 @@ const char *PUS_SERVICE_5_EVENT_REPORTING_STRING = "PUS_SERVICE_5_EVENT_REPORTIN
 const char *PUS_SERVICE_8_FUNCTION_MGMT_STRING = "PUS_SERVICE_8_FUNCTION_MGMT";
 const char *PUS_SERVICE_9_TIME_MGMT_STRING = "PUS_SERVICE_9_TIME_MGMT";
 const char *PUS_SERVICE_11_TC_SCHEDULER_STRING = "PUS_SERVICE_11_TC_SCHEDULER";
+const char *PUS_SERVICE_15_TM_STORAGE_STRING = "PUS_SERVICE_15_TM_STORAGE";
 const char *PUS_SERVICE_17_TEST_STRING = "PUS_SERVICE_17_TEST";
 const char *PUS_SERVICE_20_PARAMETERS_STRING = "PUS_SERVICE_20_PARAMETERS";
 const char *PUS_SERVICE_200_MODE_MGMT_STRING = "PUS_SERVICE_200_MODE_MGMT";
@@ -155,6 +157,11 @@ const char *ACS_SUBSYSTEM_STRING = "ACS_SUBSYSTEM";
 const char *PL_SUBSYSTEM_STRING = "PL_SUBSYSTEM";
 const char *TCS_SUBSYSTEM_STRING = "TCS_SUBSYSTEM";
 const char *COM_SUBSYSTEM_STRING = "COM_SUBSYSTEM";
+const char *MISC_TM_STORE_STRING = "MISC_TM_STORE";
+const char *OK_TM_STORE_STRING = "OK_TM_STORE";
+const char *NOT_OK_TM_STORE_STRING = "NOT_OK_TM_STORE";
+const char *HK_TM_STORE_STRING = "HK_TM_STORE";
+const char *CFDP_TM_STORE_STRING = "CFDP_TM_STORE";
 const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE";
 const char *THERMAL_TEMP_INSERTER_STRING = "THERMAL_TEMP_INSERTER";
 const char *NO_OBJECT_STRING = "NO_OBJECT";
@@ -223,6 +230,8 @@ const char *translateObject(object_id_t object) {
       return STAR_TRACKER_STRING;
     case 0x44130045:
       return GPS_CONTROLLER_STRING;
+    case 0x44140013:
+      return IMTQ_POLLING_STRING;
     case 0x44140014:
       return IMTQ_HANDLER_STRING;
     case 0x442000A1:
@@ -367,6 +376,8 @@ const char *translateObject(object_id_t object) {
       return PUS_SERVICE_9_TIME_MGMT_STRING;
     case 0x53000011:
       return PUS_SERVICE_11_TC_SCHEDULER_STRING;
+    case 0x53000015:
+      return PUS_SERVICE_15_TM_STORAGE_STRING;
     case 0x53000017:
       return PUS_SERVICE_17_TEST_STRING;
     case 0x53000020:
@@ -459,6 +470,16 @@ const char *translateObject(object_id_t object) {
       return TCS_SUBSYSTEM_STRING;
     case 0x73010004:
       return COM_SUBSYSTEM_STRING;
+    case 0x73020001:
+      return MISC_TM_STORE_STRING;
+    case 0x73020002:
+      return OK_TM_STORE_STRING;
+    case 0x73020003:
+      return NOT_OK_TM_STORE_STRING;
+    case 0x73020004:
+      return HK_TM_STORE_STRING;
+    case 0x73030000:
+      return CFDP_TM_STORE_STRING;
     case 0x73500000:
       return CCSDS_IP_CORE_BRIDGE_STRING;
     case 0x90000003:

generators/requirements.txt

@@ -1,2 +1,2 @@
 colorlog==6.7.0
-git+https://egit.irs.uni-stuttgart.de/fsfw/fsfwgen@66e31885a7c87fbb4340cd2a51a13e1196f377af#egg=fsfwgen
+git+https://egit.irs.uni-stuttgart.de/fsfw/fsfwgen@v0.3.0#egg=fsfwgen

linux/ObjectFactory.cpp

@@ -9,7 +9,7 @@
 #include <fsfw_hal/linux/spi/SpiComIF.h>
 #include <fsfw_hal/linux/spi/SpiCookie.h>
 #include <linux/callbacks/gpioCallbacks.h>
-#include <linux/devices/Max31865RtdLowlevelHandler.h>
+#include <linux/devices/Max31865RtdPolling.h>
 #include <mission/controller/AcsController.h>
 #include <mission/core/GenericFactory.h>
 #include <mission/devices/Max31865EiveHandler.h>
@@ -278,7 +278,7 @@ void ObjectFactory::createRtdComponents(std::string spiDev, GpioIF* gpioComIF,
   TcsBoardAssembly* tcsBoardAss = ObjectFactory::createTcsBoardAssy(*pwrSwitcher);
 
   // Create special low level reader communication interface
-  new Max31865RtdReader(objects::SPI_RTD_COM_IF, comIF, gpioComIF);
+  new Max31865RtdPolling(objects::SPI_RTD_COM_IF, comIF, gpioComIF);
   for (uint8_t idx = 0; idx < NUM_RTDS; idx++) {
     rtdCookies[idx] = new SpiCookie(cookieArgs[idx].first, cookieArgs[idx].second,
                                     MAX31865::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);

linux/devices/CMakeLists.txt

@@ -3,8 +3,9 @@ if(EIVE_BUILD_GPSD_GPS_HANDLER)
 endif()
 
 target_sources(
-  ${OBSW_NAME} PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp
-                       ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)
+  ${OBSW_NAME}
+  PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp
+          ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)
 
 add_subdirectory(ploc)
 

linux/devices/ImtqPollingTask.cpp

@@ -0,0 +1,433 @@
+#include "ImtqPollingTask.h"
+
+#include <fcntl.h>
+#include <fsfw/tasks/SemaphoreFactory.h>
+#include <fsfw/tasks/TaskFactory.h>
+#include <fsfw/timemanager/Stopwatch.h>
+#include <fsfw_hal/linux/UnixFileGuard.h>
+#include <linux/i2c-dev.h>
+#include <sys/ioctl.h>
+
+#include "fsfw/FSFW.h"
+
+ImtqPollingTask::ImtqPollingTask(object_id_t imtqPollingTask) : SystemObject(imtqPollingTask) {
+  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
+  semaphore->acquire();
+  ipcLock = MutexFactory::instance()->createMutex();
+  bufLock = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
+  while (true) {
+    ipcLock->lockMutex();
+    state = InternalState::IDLE;
+    ipcLock->unlockMutex();
+    semaphore->acquire();
+
+    comStatus = returnvalue::OK;
+    // Stopwatch watch;
+    switch (currentRequest) {
+      case imtq::RequestType::MEASURE_NO_ACTUATION: {
+        handleMeasureStep();
+        break;
+      }
+      case imtq::RequestType::ACTUATE: {
+        handleActuateStep();
+        break;
+      }
+    };
+  }
+  return returnvalue::OK;
+}
+
+void ImtqPollingTask::handleMeasureStep() {
+  size_t replyLen = 0;
+  uint8_t* replyPtr;
+  ImtqRepliesDefault replies(replyBuf.data());
+  // Can be used later to verify correct timing (e.g. all data has been read)
+  clearReadFlagsDefault(replies);
+  auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
+    ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen);
+    return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR);
+  };
+
+  cmdLen = 1;
+  cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE;
+  if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) {
+    return;
+  }
+
+  ignoreNextActuateRequest =
+      (replies.getSystemState()[2] == static_cast<uint8_t>(imtq::mode::SELF_TEST));
+  if (ignoreNextActuateRequest) {
+    // Do not command anything until self-test is done.
+    return;
+  }
+
+  if (specialRequest != imtq::SpecialRequest::NONE) {
+    auto executeSelfTest = [&](imtq::selfTest::Axis axis) {
+      cmdBuf[0] = imtq::CC::SELF_TEST_CMD;
+      cmdBuf[1] = axis;
+      return i2cCmdExecMeasure(imtq::CC::SELF_TEST_CMD);
+    };
+    // If a self-test is already ongoing, ignore the request.
+    if (replies.getSystemState()[2] != static_cast<uint8_t>(imtq::mode::SELF_TEST)) {
+      switch (specialRequest) {
+        case (imtq::SpecialRequest::DO_SELF_TEST_POS_X): {
+          executeSelfTest(imtq::selfTest::Axis::X_POSITIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X): {
+          executeSelfTest(imtq::selfTest::Axis::X_NEGATIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y): {
+          executeSelfTest(imtq::selfTest::Axis::Y_POSITIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y): {
+          executeSelfTest(imtq::selfTest::Axis::Y_NEGATIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z): {
+          executeSelfTest(imtq::selfTest::Axis::Z_POSITIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
+          executeSelfTest(imtq::selfTest::Axis::Z_NEGATIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
+          cmdBuf[0] = imtq::CC::GET_SELF_TEST_RESULT;
+          i2cCmdExecMeasure(imtq::CC::GET_SELF_TEST_RESULT);
+          break;
+        }
+        default: {
+          // Should never happen
+          break;
+        }
+      }
+      // We are done. Only request self test or results here.
+      return;
+    }
+  }
+
+  cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
+  if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+  // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
+  // commands.
+  TaskFactory::delayTask(currentIntegrationTimeMs);
+
+  cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
+  if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+
+  cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
+  if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
+    return;
+  }
+
+  cmdBuf[0] = imtq::CC::GET_CAL_MTM_MEASUREMENT;
+  if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+  // sif::debug << "measure done" << std::endl;
+  return;
+}
+
+void ImtqPollingTask::handleActuateStep() {
+  uint8_t* replyPtr = nullptr;
+  size_t replyLen = 0;
+  // No point when self-test mode is active.
+  if (ignoreNextActuateRequest) {
+    return;
+  }
+  ImtqRepliesWithTorque replies(replyBufActuation.data());
+  // Can be used later to verify correct timing (e.g. all data has been read)
+  clearReadFlagsWithTorque(replies);
+  auto i2cCmdExecActuate = [&](imtq::CC::CC cc) {
+    ccToReplyPtrActuate(replies, cc, &replyPtr, replyLen);
+    return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::ACTUATE_CMD_LOW_LEVEL_ERROR);
+  };
+  buildDipoleCommand();
+  if (i2cCmdExecActuate(imtq::CC::START_ACTUATION_DIPOLE) != returnvalue::OK) {
+    return;
+  }
+
+  cmdLen = 1;
+  cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
+  if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+
+  TaskFactory::delayTask(currentIntegrationTimeMs);
+
+  cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
+  if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+  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;
+  return;
+}
+
+ReturnValue_t ImtqPollingTask::initialize() { return returnvalue::OK; }
+
+ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
+  i2cCookie = dynamic_cast<I2cCookie*>(cookie);
+  if (i2cCookie == nullptr) {
+    sif::error << "ImtqPollingTask::initializeInterface: Invalid I2C cookie" << std::endl;
+    return returnvalue::FAILED;
+  }
+  i2cDev = i2cCookie->getDeviceFile().c_str();
+  i2cAddr = i2cCookie->getAddress();
+  return returnvalue::OK;
+}
+
+ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
+                                           size_t sendLen) {
+  ImtqRequest request(sendData, sendLen);
+  {
+    MutexGuard mg(ipcLock);
+    currentRequest = request.getRequestType();
+    if (currentRequest == imtq::RequestType::ACTUATE) {
+      std::memcpy(dipoles, request.getDipoles(), 6);
+      torqueDuration = request.getTorqueDuration();
+    }
+    specialRequest = request.getSpecialRequest();
+    if (state != InternalState::IDLE) {
+      return returnvalue::FAILED;
+    }
+    state = InternalState::BUSY;
+  }
+  semaphore->release();
+
+  return returnvalue::OK;
+}
+
+ReturnValue_t ImtqPollingTask::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
+
+ReturnValue_t ImtqPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
+  return returnvalue::OK;
+}
+
+void ImtqPollingTask::ccToReplyPtrMeasure(ImtqRepliesDefault& replies, imtq::CC::CC cc,
+                                          uint8_t** replyBuf, size_t& replyLen) {
+  replyLen = imtq::getReplySize(cc);
+  switch (cc) {
+    case (imtq::CC::CC::GET_ENG_HK_DATA): {
+      *replyBuf = replies.engHk;
+      break;
+    }
+    case (imtq::CC::CC::SOFTWARE_RESET): {
+      *replyBuf = replies.swReset;
+      break;
+    }
+    case (imtq::CC::CC::GET_SYSTEM_STATE): {
+      *replyBuf = replies.systemState;
+      break;
+    }
+    case (imtq::CC::CC::START_MTM_MEASUREMENT): {
+      *replyBuf = replies.startMtmMeasurement;
+      break;
+    }
+    case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): {
+      *replyBuf = replies.rawMgmMeasurement;
+      break;
+    }
+    case (imtq::CC::CC::GET_CAL_MTM_MEASUREMENT): {
+      *replyBuf = replies.calibMgmMeasurement;
+      break;
+    }
+    default: {
+      *replyBuf = replies.specialRequestReply;
+      break;
+    }
+  }
+}
+
+void ImtqPollingTask::ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc,
+                                          uint8_t** replyBuf, size_t& replyLen) {
+  replyLen = imtq::getReplySize(cc);
+  switch (cc) {
+    case (imtq::CC::CC::START_ACTUATION_DIPOLE): {
+      *replyBuf = replies.dipoleActuation;
+      break;
+    }
+    case (imtq::CC::CC::GET_ENG_HK_DATA): {
+      *replyBuf = replies.engHk;
+      break;
+    }
+    case (imtq::CC::CC::START_MTM_MEASUREMENT): {
+      *replyBuf = replies.startMtmMeasurement;
+      break;
+    }
+    case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): {
+      *replyBuf = replies.rawMgmMeasurement;
+      break;
+    }
+    default: {
+      *replyBuf = nullptr;
+      replyLen = 0;
+      break;
+    }
+  }
+}
+size_t ImtqPollingTask::getExchangeBufLen(imtq::SpecialRequest specialRequest) {
+  size_t baseLen = ImtqRepliesDefault::BASE_LEN;
+  switch (specialRequest) {
+    case (imtq::SpecialRequest::NONE):
+    case (imtq::SpecialRequest::DO_SELF_TEST_POS_X):
+    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X):
+    case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y):
+    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y):
+    case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z):
+    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
+      break;
+    }
+    case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
+      baseLen += imtq::replySize::SELF_TEST_RESULTS;
+      break;
+    }
+  }
+  return baseLen;
+}
+
+void ImtqPollingTask::buildDipoleCommand() {
+  cmdBuf[0] = imtq::CC::CC::START_ACTUATION_DIPOLE;
+  uint8_t* serPtr = cmdBuf.data() + 1;
+  size_t serLen = 0;
+  for (uint8_t idx = 0; idx < 3; idx++) {
+    SerializeAdapter::serialize(&dipoles[idx], &serPtr, &serLen, cmdBuf.size(),
+                                SerializeIF::Endianness::LITTLE);
+  }
+  SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(),
+                              SerializeIF::Endianness::LITTLE);
+  cmdLen = 1 + serLen;
+}
+
+ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
+                                                   size_t* size) {
+  imtq::RequestType currentRequest;
+  {
+    MutexGuard mg(ipcLock);
+    currentRequest = this->currentRequest;
+  }
+
+  size_t replyLen = 0;
+  MutexGuard mg(bufLock);
+  if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) {
+    replyLen = getExchangeBufLen(specialRequest);
+    memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
+  } else {
+    replyLen = ImtqRepliesWithTorque::BASE_LEN;
+    memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen);
+  }
+  *buffer = exchangeBuf.data();
+  *size = replyLen;
+  return comStatus;
+}
+
+void ImtqPollingTask::clearReadFlagsDefault(ImtqRepliesDefault& replies) {
+  replies.calibMgmMeasurement[0] = false;
+  replies.rawMgmMeasurement[0] = false;
+  replies.systemState[0] = false;
+  replies.specialRequestReply[0] = false;
+  replies.engHk[0] = false;
+}
+
+ReturnValue_t ImtqPollingTask::i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr,
+                                                 size_t replyLen, ReturnValue_t comErrIfFails) {
+  ReturnValue_t res = performI2cFullRequest(replyPtr + 1, replyLen);
+  if (res != returnvalue::OK) {
+    sif::error << "IMTQ: I2C transaction for command 0x" << std::hex << std::setw(2) << cc
+               << " failed" << std::dec << std::endl;
+    comStatus = comErrIfFails;
+    return returnvalue::FAILED;
+  }
+  if (replyPtr[1] != cc) {
+    sif::warning << "IMTQ: Unexpected CC 0x" << std::hex << std::setw(2)
+                 << static_cast<int>(replyPtr[1]) << " for command 0x" << cc << std::dec
+                 << std::endl;
+    comStatus = comErrIfFails;
+    return returnvalue::FAILED;
+  }
+  replyPtr[0] = true;
+  return returnvalue::OK;
+}
+
+void ImtqPollingTask::clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies) {
+  replies.dipoleActuation[0] = false;
+  replies.engHk[0] = false;
+  replies.rawMgmMeasurement[0] = false;
+  replies.startMtmMeasurement[0] = false;
+}
+
+ReturnValue_t ImtqPollingTask::performI2cFullRequest(uint8_t* reply, size_t replyLen) {
+  int fd = 0;
+  if (cmdLen == 0 or reply == nullptr) {
+    return returnvalue::FAILED;
+  }
+
+  {
+    UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest");
+    if (fileHelper.getOpenResult() != returnvalue::OK) {
+      return fileHelper.getOpenResult();
+    }
+    if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
+      sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
+                   << strerror(errno) << std::endl;
+    }
+
+    int written = write(fd, cmdBuf.data(), cmdLen);
+    if (written < 0) {
+      sif::error << "IMTQ: Failed to send with error code " << errno
+                 << ". Error description: " << strerror(errno) << std::endl;
+      return returnvalue::FAILED;
+    } else if (static_cast<size_t>(written) != cmdLen) {
+      sif::error << "IMTQ: Could not write all bytes" << std::endl;
+      return returnvalue::FAILED;
+    }
+  }
+#if FSFW_HAL_I2C_WIRETAPPING == 1
+  sif::info << "Sent I2C data to bus " << deviceFile << ":" << std::endl;
+  arrayprinter::print(sendData, sendLen);
+#endif
+
+  // wait 1 ms like specified in the datasheet. This is the time the IMTQ needs
+  // to prepare a reply.
+  usleep(1000);
+
+  {
+    UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest");
+    if (fileHelper.getOpenResult() != returnvalue::OK) {
+      return fileHelper.getOpenResult();
+    }
+    if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
+      sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
+                   << strerror(errno) << std::endl;
+    }
+    MutexGuard mg(bufLock);
+    int readLen = read(fd, reply, replyLen);
+    if (readLen != static_cast<int>(replyLen)) {
+      if (readLen < 0) {
+        sif::warning << "IMTQ: Reading failed with error code " << errno << " | " << strerror(errno)
+                     << std::endl;
+      } else {
+        sif::warning << "IMTQ: Read only" << readLen << " from " << replyLen << " bytes"
+                     << std::endl;
+      }
+    }
+  }
+  if (reply[0] == 0xff or reply[1] == 0xff) {
+    sif::warning << "IMTQ: No reply available after 1 millisecond";
+    return NO_REPLY_AVAILABLE;
+  }
+  return returnvalue::OK;
+}

linux/devices/ImtqPollingTask.h

@@ -0,0 +1,70 @@
+#ifndef LINUX_DEVICES_IMTQPOLLINGTASK_H_
+#define LINUX_DEVICES_IMTQPOLLINGTASK_H_
+
+#include <fsfw/tasks/SemaphoreIF.h>
+#include <fsfw_hal/linux/i2c/I2cCookie.h>
+
+#include "fsfw/devicehandlers/DeviceCommunicationIF.h"
+#include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/tasks/ExecutableObjectIF.h"
+#include "mission/devices/devicedefinitions/imtqHelpers.h"
+
+class ImtqPollingTask : public SystemObject,
+                        public ExecutableObjectIF,
+                        public DeviceCommunicationIF {
+ public:
+  ImtqPollingTask(object_id_t imtqPollingTask);
+
+  ReturnValue_t performOperation(uint8_t operationCode) override;
+  ReturnValue_t initialize() override;
+
+ private:
+  static constexpr ReturnValue_t NO_REPLY_AVAILABLE = returnvalue::makeCode(2, 0);
+
+  enum class InternalState { IDLE, BUSY } state = InternalState::IDLE;
+  imtq::RequestType currentRequest = imtq::RequestType::MEASURE_NO_ACTUATION;
+
+  SemaphoreIF* semaphore;
+  ReturnValue_t comStatus = returnvalue::OK;
+  MutexIF* ipcLock;
+  MutexIF* bufLock;
+  I2cCookie* i2cCookie = nullptr;
+  const char* i2cDev = nullptr;
+  address_t i2cAddr = 0;
+  uint32_t currentIntegrationTimeMs = 10;
+  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;
+  std::array<uint8_t, 524> replyBufActuation;
+  std::array<uint8_t, 524> exchangeBuf;
+  size_t cmdLen = 0;
+
+  // DeviceCommunicationIF overrides
+  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 ccToReplyPtrMeasure(ImtqRepliesDefault& replies, imtq::CC::CC cc, uint8_t** replyBuf,
+                           size_t& replyLen);
+  void ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc, uint8_t** replyBuf,
+                           size_t& replyLen);
+  void clearReadFlagsDefault(ImtqRepliesDefault& replies);
+  void clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies);
+  size_t getExchangeBufLen(imtq::SpecialRequest specialRequest);
+  void buildDipoleCommand();
+  void handleMeasureStep();
+  void handleActuateStep();
+  ReturnValue_t i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr, size_t replyLen,
+                                  ReturnValue_t comErrIfFails);
+  ReturnValue_t performI2cFullRequest(uint8_t* reply, size_t replyLen);
+};
+
+#endif /* LINUX_DEVICES_IMTQPOLLINGTASK_H_ */

linux/devices/Max31865RtdPolling.cpp

@@ -1,8 +1,7 @@
-#include "Max31865RtdLowlevelHandler.h"
-
 #include <fsfw/tasks/TaskFactory.h>
 #include <fsfw/timemanager/Stopwatch.h>
 #include <fsfw_hal/linux/spi/ManualCsLockGuard.h>
+#include <linux/devices/Max31865RtdPolling.h>
 
 #define OBSW_RTD_AUTO_MODE 1
 
@@ -17,12 +16,13 @@ static constexpr uint8_t BASE_CFG =
     (MAX31865::ConvMode::NORM_OFF << MAX31865::CfgBitPos::CONV_MODE);
 #endif
 
-Max31865RtdReader::Max31865RtdReader(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF)
+Max31865RtdPolling::Max31865RtdPolling(object_id_t objectId, SpiComIF* lowLevelComIF,
+                                       GpioIF* gpioIF)
     : SystemObject(objectId), rtds(EiveMax31855::NUM_RTDS), comIF(lowLevelComIF), gpioIF(gpioIF) {
   readerMutex = MutexFactory::instance()->createMutex();
 }
 
-ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) {
+ReturnValue_t Max31865RtdPolling::performOperation(uint8_t operationCode) {
   using namespace MAX31865;
   ReturnValue_t result = returnvalue::OK;
   static_cast<void>(result);
@@ -49,17 +49,16 @@ ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) {
   return periodicReadHandling();
 }
 
-bool Max31865RtdReader::rtdIsActive(uint8_t idx) {
+bool Max31865RtdPolling::rtdIsActive(uint8_t idx) {
   if (rtds[idx]->on and rtds[idx]->db.active and rtds[idx]->db.configured) {
     return true;
   }
   return false;
 }
 
-bool Max31865RtdReader::periodicInitHandling() {
+bool Max31865RtdPolling::periodicInitHandling() {
   using namespace MAX31865;
   ReturnValue_t result = returnvalue::OK;
-
   for (auto& rtd : rtds) {
     if (rtd == nullptr) {
       continue;
@@ -70,11 +69,9 @@ bool Max31865RtdReader::periodicInitHandling() {
       return false;
     }
     if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
-      ManualCsLockWrapper mg1(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
-      if (mg1.lockResult != returnvalue::OK or mg1.gpioResult != returnvalue::OK) {
-        sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
-        continue;
-      }
+      // 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
       result = writeCfgReg(rtd->spiCookie, BASE_CFG);
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "writeCfgReg");
@@ -115,7 +112,7 @@ bool Max31865RtdReader::periodicInitHandling() {
   return someRtdUsable;
 }
 
-ReturnValue_t Max31865RtdReader::periodicReadReqHandling() {
+ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
   using namespace MAX31865;
   // Now request one shot config for all active RTDs
   for (auto& rtd : rtds) {
@@ -139,7 +136,7 @@ ReturnValue_t Max31865RtdReader::periodicReadReqHandling() {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::periodicReadHandling() {
+ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
   using namespace MAX31865;
   auto result = returnvalue::OK;
   // Now read the RTD values
@@ -153,11 +150,9 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
       return returnvalue::FAILED;
     }
     if (rtdIsActive(rtd->idx)) {
-      ManualCsLockWrapper mg1(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
-      if (mg1.lockResult != returnvalue::OK or mg1.gpioResult != returnvalue::OK) {
-        sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
-        continue;
-      }
+      // 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
       uint16_t rtdVal = 0;
       bool faultBitSet = false;
       result = writeCfgReg(rtd->spiCookie, BASE_CFG);
@@ -166,6 +161,7 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
         continue;
       }
       result = readRtdVal(rtd->spiCookie, rtdVal, faultBitSet);
+      // sif::debug << "RTD Val: " << rtdVal << std::endl;
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "readRtdVal");
         continue;
@@ -191,7 +187,7 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) {
+ReturnValue_t Max31865RtdPolling::initializeInterface(CookieIF* cookie) {
   if (cookie == nullptr) {
     throw std::invalid_argument("Invalid MAX31865 Reader Cookie");
   }
@@ -211,8 +207,8 @@ ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* sendData,
-                                             size_t sendLen) {
+ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
+                                              size_t sendLen) {
   if (cookie == nullptr) {
     return returnvalue::FAILED;
   }
@@ -308,14 +304,14 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
+ReturnValue_t Max31865RtdPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
 
-ReturnValue_t Max31865RtdReader::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
+ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
-                                                     size_t* size) {
+ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
+                                                      size_t* size) {
   MutexGuard mg(readerMutex);
   if (mg.getLockResult() != returnvalue::OK) {
     // TODO: Emit warning
@@ -338,13 +334,13 @@ ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t**
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::writeCfgReg(SpiCookie* cookie, uint8_t cfg) {
+ReturnValue_t Max31865RtdPolling::writeCfgReg(SpiCookie* cookie, uint8_t cfg) {
   using namespace MAX31865;
   return writeNToReg(cookie, CONFIG, 1, &cfg, nullptr);
 }
 
-ReturnValue_t Max31865RtdReader::writeBiasSel(MAX31865::Bias bias, SpiCookie* cookie,
-                                              uint8_t baseCfg) {
+ReturnValue_t Max31865RtdPolling::writeBiasSel(MAX31865::Bias bias, SpiCookie* cookie,
+                                               uint8_t baseCfg) {
   using namespace MAX31865;
   if (bias == MAX31865::Bias::OFF) {
     baseCfg &= ~(1 << CfgBitPos::BIAS_SEL);
@@ -354,7 +350,7 @@ ReturnValue_t Max31865RtdReader::writeBiasSel(MAX31865::Bias bias, SpiCookie* co
   return writeCfgReg(cookie, baseCfg);
 }
 
-ReturnValue_t Max31865RtdReader::clearFaultStatus(SpiCookie* cookie) {
+ReturnValue_t Max31865RtdPolling::clearFaultStatus(SpiCookie* cookie) {
   using namespace MAX31865;
   // Read back the current configuration to avoid overwriting it when clearing te fault status
   uint8_t currentCfg = 0;
@@ -368,7 +364,7 @@ ReturnValue_t Max31865RtdReader::clearFaultStatus(SpiCookie* cookie) {
   return writeCfgReg(cookie, currentCfg);
 }
 
-ReturnValue_t Max31865RtdReader::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
+ReturnValue_t Max31865RtdPolling::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
   using namespace MAX31865;
   uint8_t* replyPtr = nullptr;
   auto result = readNFromReg(cookie, CONFIG, 1, &replyPtr);
@@ -378,19 +374,19 @@ ReturnValue_t Max31865RtdReader::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::writeLowThreshold(SpiCookie* cookie, uint16_t val) {
+ReturnValue_t Max31865RtdPolling::writeLowThreshold(SpiCookie* cookie, uint16_t val) {
   using namespace MAX31865;
   uint8_t cmd[2] = {static_cast<uint8_t>((val >> 8) & 0xff), static_cast<uint8_t>(val & 0xff)};
   return writeNToReg(cookie, LOW_THRESHOLD, 2, cmd, nullptr);
 }
 
-ReturnValue_t Max31865RtdReader::writeHighThreshold(SpiCookie* cookie, uint16_t val) {
+ReturnValue_t Max31865RtdPolling::writeHighThreshold(SpiCookie* cookie, uint16_t val) {
   using namespace MAX31865;
   uint8_t cmd[2] = {static_cast<uint8_t>((val >> 8) & 0xff), static_cast<uint8_t>(val & 0xff)};
   return writeNToReg(cookie, HIGH_THRESHOLD, 2, cmd, nullptr);
 }
 
-ReturnValue_t Max31865RtdReader::readLowThreshold(SpiCookie* cookie, uint16_t& lowThreshold) {
+ReturnValue_t Max31865RtdPolling::readLowThreshold(SpiCookie* cookie, uint16_t& lowThreshold) {
   using namespace MAX31865;
   uint8_t* replyPtr = nullptr;
   auto result = readNFromReg(cookie, LOW_THRESHOLD, 2, &replyPtr);
@@ -400,7 +396,7 @@ ReturnValue_t Max31865RtdReader::readLowThreshold(SpiCookie* cookie, uint16_t& l
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::readHighThreshold(SpiCookie* cookie, uint16_t& highThreshold) {
+ReturnValue_t Max31865RtdPolling::readHighThreshold(SpiCookie* cookie, uint16_t& highThreshold) {
   using namespace MAX31865;
   uint8_t* replyPtr = nullptr;
   auto result = readNFromReg(cookie, HIGH_THRESHOLD, 2, &replyPtr);
@@ -410,8 +406,8 @@ ReturnValue_t Max31865RtdReader::readHighThreshold(SpiCookie* cookie, uint16_t&
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::writeNToReg(SpiCookie* cookie, uint8_t reg, size_t n, uint8_t* cmd,
-                                             uint8_t** reply) {
+ReturnValue_t Max31865RtdPolling::writeNToReg(SpiCookie* cookie, uint8_t reg, size_t n,
+                                              uint8_t* cmd, uint8_t** reply) {
   using namespace MAX31865;
   if (n > cmdBuf.size() - 1) {
     return returnvalue::FAILED;
@@ -423,7 +419,7 @@ ReturnValue_t Max31865RtdReader::writeNToReg(SpiCookie* cookie, uint8_t reg, siz
   return comIF->sendMessage(cookie, cmdBuf.data(), n + 1);
 }
 
-ReturnValue_t Max31865RtdReader::readRtdVal(SpiCookie* cookie, uint16_t& val, bool& faultBitSet) {
+ReturnValue_t Max31865RtdPolling::readRtdVal(SpiCookie* cookie, uint16_t& val, bool& faultBitSet) {
   using namespace MAX31865;
   uint8_t* replyPtr = nullptr;
   auto result = readNFromReg(cookie, RTD, 2, &replyPtr);
@@ -438,8 +434,8 @@ ReturnValue_t Max31865RtdReader::readRtdVal(SpiCookie* cookie, uint16_t& val, bo
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, size_t n,
-                                              uint8_t** reply) {
+ReturnValue_t Max31865RtdPolling::readNFromReg(SpiCookie* cookie, uint8_t reg, size_t n,
+                                               uint8_t** reply) {
   using namespace MAX31865;
   if (n > 4) {
     return returnvalue::FAILED;
@@ -465,15 +461,15 @@ ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, si
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
-                                                const char* ctx) {
+ReturnValue_t Max31865RtdPolling::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
+                                                 const char* ctx) {
   cookie->db.spiErrorCount.value += 1;
   sif::warning << "Max31865RtdReader::handleSpiError: " << ctx << " | Failed with result " << result
                << std::endl;
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::initialize() {
+ReturnValue_t Max31865RtdPolling::initialize() {
   csLock = comIF->getCsMutex();
   return SystemObject::initialize();
 }

linux/devices/Max31865RtdPolling.h

@@ -35,11 +35,11 @@ struct Max31865ReaderCookie : public CookieIF {
   EiveMax31855::ReadOutStruct db;
 };
 
-class Max31865RtdReader : public SystemObject,
-                          public ExecutableObjectIF,
-                          public DeviceCommunicationIF {
+class Max31865RtdPolling : public SystemObject,
+                           public ExecutableObjectIF,
+                           public DeviceCommunicationIF {
  public:
-  Max31865RtdReader(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF);
+  Max31865RtdPolling(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF);
 
   ReturnValue_t performOperation(uint8_t operationCode) override;
   ReturnValue_t initialize() override;

linux/devices/RwPollingTask.cpp

@@ -162,11 +162,15 @@ ReturnValue_t RwPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requ
 
 ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
   RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
-  {
-    MutexGuard mg(ipcLock);
-    *buffer = rwCookie->replyBuf.data();
-    *size = rwCookie->replyBuf.size();
+  if (rwCookie == nullptr or rwCookie->bufLock == nullptr) {
+    return returnvalue::FAILED;
   }
+  {
+    MutexGuard mg(rwCookie->bufLock);
+    memcpy(rwCookie->exchangeBuf.data(), rwCookie->replyBuf.data(), rwCookie->replyBuf.size());
+  }
+  *buffer = rwCookie->exchangeBuf.data();
+  *size = rwCookie->exchangeBuf.size();
   return returnvalue::OK;
 }
 
@@ -248,6 +252,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf
 #endif
 
   size_t decodedFrameLen = 0;
+  MutexGuard mg(rwCookie.bufLock);
 
   while (decodedFrameLen < maxReplyLen) {
     // First byte already read in
@@ -428,7 +433,12 @@ void RwPollingTask::handleSpecialRequests() {
     }
     uint8_t* replyBuf;
     size_t maxReadLen = idAndIdxToReadBuffer(specialRequestIds[idx], idx, &replyBuf);
-    readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
+    result = readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
+    if (result == returnvalue::OK) {
+      // The first byte is always a flag which shows whether the value was read
+      // properly at least once.
+      replyBuf[0] = true;
+    }
   }
 }
 

linux/devices/RwPollingTask.h

@@ -18,10 +18,14 @@ class RwCookie : public SpiCookie {
   static constexpr size_t REPLY_BUF_LEN = 524;
   RwCookie(uint8_t rwIdx, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize,
            spi::SpiModes spiMode, uint32_t spiSpeed)
-      : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {}
+      : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {
+    bufLock = MutexFactory::instance()->createMutex();
+  }
 
  private:
   std::array<uint8_t, REPLY_BUF_LEN> replyBuf{};
+  std::array<uint8_t, REPLY_BUF_LEN> exchangeBuf{};
+  MutexIF* bufLock;
   bool setSpeed = true;
   int32_t currentRwSpeed = 0;
   uint16_t currentRampTime = 0;
@@ -56,6 +60,13 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev
   static constexpr uint32_t TIMEOUT_MS = 20;
   static constexpr uint8_t MAX_RETRIES_REPLY = 5;
 
+  // DeviceCommunicationIF overrides
+  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 writeAndReadAllRws(DeviceCommandId_t id);
   ReturnValue_t writeOneRwCmd(uint8_t rwIdx, int fd);
   ReturnValue_t readAllRws(DeviceCommandId_t id);
@@ -64,15 +75,6 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev
   ReturnValue_t readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, size_t maxReplyLen);
   void handleSpecialRequests();
 
-  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 openSpi(int flags, int& fd);
   ReturnValue_t pullCsLow(gpioId_t gpioId, GpioIF& gpioIF);
   void prepareSimpleCommand(DeviceCommandId_t id);

linux/devices/ploc/PlocSupervisorHandler.cpp

@@ -1742,18 +1742,21 @@ ReturnValue_t PlocSupervisorHandler::createMramDumpFile() {
   std::string filename = "mram-dump--" + timeStamp + ".bin";
 
 #ifdef XIPHOS_Q7S
-  std::string currentMountPrefix = sdcMan->getCurrentMountPrefix();
+  const char* currentMountPrefix = sdcMan->getCurrentMountPrefix();
 #else
-  std::string currentMountPrefix("/mnt/sd0");
+  const char* currentMountPrefix = "/mnt/sd0";
 #endif /* BOARD_TE0720 == 0 */
+  if (currentMountPrefix == nullptr) {
+    return returnvalue::FAILED;
+  }
 
   // Check if path to PLOC directory exists
-  if (not std::filesystem::exists(std::string(currentMountPrefix + "/" + supervisorFilePath))) {
+  if (not std::filesystem::exists(std::string(currentMountPrefix) + "/" + supervisorFilePath)) {
     sif::warning << "PlocSupervisorHandler::createMramDumpFile: Supervisor path does not exist"
                  << std::endl;
     return result::PATH_DOES_NOT_EXIST;
   }
-  activeMramFile = currentMountPrefix + "/" + supervisorFilePath + "/" + filename;
+  activeMramFile = std::string(currentMountPrefix) + "/" + supervisorFilePath + "/" + filename;
   // Create new file
   std::ofstream file(activeMramFile, std::ios_base::out);
   file.close();

linux/fsfwconfig/CMakeLists.txt

@@ -1,5 +1,4 @@
-target_sources(${OBSW_NAME} PRIVATE ipc/MissionMessageTypes.cpp
-                                    pollingsequence/pollingSequenceFactory.cpp)
+target_sources(${OBSW_NAME} PRIVATE ipc/MissionMessageTypes.cpp)
 
 target_include_directories(${OBSW_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 

linux/fsfwconfig/FSFWConfig.h.in

@@ -42,7 +42,7 @@
 
 //! When using the newlib nano library, C99 support for stdio facilities
 //! will not be provided. This define should be set to 1 if this is the case.
-#define FSFW_NO_C99_IO 	            1
+#define FSFW_NO_C99_IO 	            0
 
 //! Specify whether a special mode store is used for Subsystem components.
 #define FSFW_USE_MODESTORE          0

linux/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 257 translations.
+ * @brief    Auto-generated event translation file. Contains 263 translations.
  * @details
- * Generated on: 2023-02-21 00:24:44
+ * Generated on: 2023-02-24 16:57:00
  */
 #include "translateEvents.h"
 
@@ -90,9 +90,13 @@ const char *CHANGE_OF_SETUP_PARAMETER_STRING = "CHANGE_OF_SETUP_PARAMETER";
 const char *STORE_ERROR_STRING = "STORE_ERROR";
 const char *MSG_QUEUE_ERROR_STRING = "MSG_QUEUE_ERROR";
 const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR";
+const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
+const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
 const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
+const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
+const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -151,6 +155,7 @@ const char *CARRIER_LOCK_STRING = "CARRIER_LOCK";
 const char *BIT_LOCK_PDEC_STRING = "BIT_LOCK_PDEC";
 const char *LOST_CARRIER_LOCK_PDEC_STRING = "LOST_CARRIER_LOCK_PDEC";
 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 *IMAGE_UPLOAD_FAILED_STRING = "IMAGE_UPLOAD_FAILED";
@@ -250,6 +255,7 @@ 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 *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";
@@ -430,12 +436,20 @@ const char *translateEvents(Event event) {
       return MSG_QUEUE_ERROR_STRING;
     case (10802):
       return SERIALIZATION_ERROR_STRING;
+    case (10803):
+      return FILESTORE_ERROR_STRING;
+    case (10804):
+      return FILENAME_TOO_LARGE_ERROR_STRING;
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
       return SAFE_RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
+    case (11203):
+      return MEKF_INVALID_INFO_STRING;
+    case (11204):
+      return MEKF_INVALID_MODE_VIOLATION_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):
@@ -553,8 +567,10 @@ const char *translateEvents(Event event) {
     case (12406):
       return LOST_BIT_LOCK_PDEC_STRING;
     case (12407):
-      return POLL_SYSCALL_ERROR_PDEC_STRING;
+      return TOO_MANY_IRQS_STRING;
     case (12408):
+      return POLL_SYSCALL_ERROR_PDEC_STRING;
+    case (12409):
       return WRITE_SYSCALL_ERROR_PDEC_STRING;
     case (12500):
       return IMAGE_UPLOAD_FAILED_STRING;
@@ -751,18 +767,20 @@ const char *translateEvents(Event event) {
     case (14006):
       return CURRENT_IMAGE_INFO_STRING;
     case (14100):
+      return POSSIBLE_FILE_CORRUPTION_STRING;
+    case (14200):
       return NO_VALID_SENSOR_TEMPERATURE_STRING;
-    case (14101):
+    case (14201):
       return NO_HEALTHY_HEATER_AVAILABLE_STRING;
-    case (14102):
+    case (14202):
       return SYRLINKS_OVERHEATING_STRING;
-    case (14103):
+    case (14203):
       return PLOC_OVERHEATING_STRING;
-    case (14104):
+    case (14204):
       return OBC_OVERHEATING_STRING;
-    case (14105):
+    case (14205):
       return HPA_OVERHEATING_STRING;
-    case (14106):
+    case (14206):
       return PLPCDU_OVERHEATING_STRING;
     default:
       return "UNKNOWN_EVENT";

linux/fsfwconfig/objects/translateObjects.cpp

@@ -1,8 +1,8 @@
 /**
  * @brief  Auto-generated object translation file.
  * @details
- * Contains 152 translations.
- * Generated on: 2023-02-21 00:24:44
+ * Contains 159 translations.
+ * Generated on: 2023-02-24 16:57:00
  */
 #include "translateObjects.h"
 
@@ -37,6 +37,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
 const char *RW4_STRING = "RW4";
 const char *STAR_TRACKER_STRING = "STAR_TRACKER";
 const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER";
+const char *IMTQ_POLLING_STRING = "IMTQ_POLLING";
 const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
 const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
 const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
@@ -109,6 +110,7 @@ const char *PUS_SERVICE_5_EVENT_REPORTING_STRING = "PUS_SERVICE_5_EVENT_REPORTIN
 const char *PUS_SERVICE_8_FUNCTION_MGMT_STRING = "PUS_SERVICE_8_FUNCTION_MGMT";
 const char *PUS_SERVICE_9_TIME_MGMT_STRING = "PUS_SERVICE_9_TIME_MGMT";
 const char *PUS_SERVICE_11_TC_SCHEDULER_STRING = "PUS_SERVICE_11_TC_SCHEDULER";
+const char *PUS_SERVICE_15_TM_STORAGE_STRING = "PUS_SERVICE_15_TM_STORAGE";
 const char *PUS_SERVICE_17_TEST_STRING = "PUS_SERVICE_17_TEST";
 const char *PUS_SERVICE_20_PARAMETERS_STRING = "PUS_SERVICE_20_PARAMETERS";
 const char *PUS_SERVICE_200_MODE_MGMT_STRING = "PUS_SERVICE_200_MODE_MGMT";
@@ -155,6 +157,11 @@ const char *ACS_SUBSYSTEM_STRING = "ACS_SUBSYSTEM";
 const char *PL_SUBSYSTEM_STRING = "PL_SUBSYSTEM";
 const char *TCS_SUBSYSTEM_STRING = "TCS_SUBSYSTEM";
 const char *COM_SUBSYSTEM_STRING = "COM_SUBSYSTEM";
+const char *MISC_TM_STORE_STRING = "MISC_TM_STORE";
+const char *OK_TM_STORE_STRING = "OK_TM_STORE";
+const char *NOT_OK_TM_STORE_STRING = "NOT_OK_TM_STORE";
+const char *HK_TM_STORE_STRING = "HK_TM_STORE";
+const char *CFDP_TM_STORE_STRING = "CFDP_TM_STORE";
 const char *CCSDS_IP_CORE_BRIDGE_STRING = "CCSDS_IP_CORE_BRIDGE";
 const char *THERMAL_TEMP_INSERTER_STRING = "THERMAL_TEMP_INSERTER";
 const char *NO_OBJECT_STRING = "NO_OBJECT";
@@ -223,6 +230,8 @@ const char *translateObject(object_id_t object) {
       return STAR_TRACKER_STRING;
     case 0x44130045:
       return GPS_CONTROLLER_STRING;
+    case 0x44140013:
+      return IMTQ_POLLING_STRING;
     case 0x44140014:
       return IMTQ_HANDLER_STRING;
     case 0x442000A1:
@@ -367,6 +376,8 @@ const char *translateObject(object_id_t object) {
       return PUS_SERVICE_9_TIME_MGMT_STRING;
     case 0x53000011:
       return PUS_SERVICE_11_TC_SCHEDULER_STRING;
+    case 0x53000015:
+      return PUS_SERVICE_15_TM_STORAGE_STRING;
     case 0x53000017:
       return PUS_SERVICE_17_TEST_STRING;
     case 0x53000020:
@@ -459,6 +470,16 @@ const char *translateObject(object_id_t object) {
       return TCS_SUBSYSTEM_STRING;
     case 0x73010004:
       return COM_SUBSYSTEM_STRING;
+    case 0x73020001:
+      return MISC_TM_STORE_STRING;
+    case 0x73020002:
+      return OK_TM_STORE_STRING;
+    case 0x73020003:
+      return NOT_OK_TM_STORE_STRING;
+    case 0x73020004:
+      return HK_TM_STORE_STRING;
+    case 0x73030000:
+      return CFDP_TM_STORE_STRING;
     case 0x73500000:
       return CCSDS_IP_CORE_BRIDGE_STRING;
     case 0x90000003:

linux/ipcore/PdecHandler.cpp

@@ -1,6 +1,7 @@
 #include "PdecHandler.h"
 
 #include <fcntl.h>
+#include <fsfw/tasks/TaskFactory.h>
 #include <poll.h>
 #include <sys/mman.h>
 #include <unistd.h>
@@ -113,7 +114,7 @@ ReturnValue_t PdecHandler::polledOperation() {
         // Requires reconfiguration and reinitialization of PDEC
         triggerEvent(INVALID_FAR);
         state = State::WAIT_FOR_RECOVERY;
-        return result;
+        break;
       }
       state = State::RUNNING;
       break;
@@ -145,8 +146,9 @@ ReturnValue_t PdecHandler::irqOperation() {
 
   // Used to unmask IRQ
   uint32_t info = 1;
-  ssize_t nb = 0;
-  int ret = 0;
+
+  interruptWindowCd.resetTimer();
+
   // Clear interrupts with dummy read before unmasking the interrupt. Use a volatile to prevent
   // read being optimized away.
   volatile uint32_t dummy = *(registerBaseAddress + PDEC_PIR_OFFSET);
@@ -157,7 +159,7 @@ ReturnValue_t PdecHandler::irqOperation() {
     readCommandQueue();
     switch (state) {
       case State::INIT:
-        resetFarStatFlag();
+        result = resetFarStatFlag();
         if (result != returnvalue::OK) {
           // Requires reconfiguration and reinitialization of PDEC
           triggerEvent(INVALID_FAR);
@@ -165,59 +167,19 @@ ReturnValue_t PdecHandler::irqOperation() {
           return result;
         }
         state = State::RUNNING;
+        checkLocks();
         break;
       case State::RUNNING: {
-        nb = write(fd, &info, sizeof(info));
-        if (nb != static_cast<ssize_t>(sizeof(info))) {
-          sif::error << "PdecHandler::irqOperation: Unmasking IRQ failed" << std::endl;
-          triggerEvent(WRITE_SYSCALL_ERROR_PDEC, errno);
-          close(fd);
-          state = State::INIT;
-          return returnvalue::FAILED;
-        }
-        struct pollfd fds = {.fd = fd, .events = POLLIN, .revents = 0};
-        ret = poll(&fds, 1, IRQ_TIMEOUT_MS);
-        if (ret == 0) {
-          // No TCs for timeout period
-          checkLocks();
-          lockCheckCd.resetTimer();
-        } else if (ret >= 1) {
-          nb = read(fd, &info, sizeof(info));
-          if (nb == static_cast<ssize_t>(sizeof(info))) {
-            uint32_t pisr = *(registerBaseAddress + PDEC_PISR_OFFSET);
-            if ((pisr & TC_NEW_MASK) == TC_NEW_MASK) {
-              // handle TC
-              handleNewTc();
-            }
-            if ((pisr & TC_ABORT_MASK) == TC_ABORT_MASK) {
-              tcAbortCounter += 1;
-            }
-            if ((pisr & NEW_FAR_MASK) == NEW_FAR_MASK) {
-              // Read FAR here
-              CURRENT_FAR = readFar();
-              checkFrameAna(CURRENT_FAR);
-            }
-            if (lockCheckCd.hasTimedOut()) {
-              checkLocks();
-              lockCheckCd.resetTimer();
-            }
-            // Clear interrupts with dummy read
-            dummy = *(registerBaseAddress + PDEC_PIR_OFFSET);
-          }
-        } else {
-          sif::error << "PdecHandler::irqOperation: Poll error with errno " << errno << ": "
-                     << strerror(errno) << std::endl;
-          triggerEvent(POLL_SYSCALL_ERROR_PDEC, errno);
-          close(fd);
-          state = State::INIT;
-          return returnvalue::FAILED;
-        }
+        checkAndHandleIrqs(fd, info);
         break;
       }
       case State::WAIT_FOR_RECOVERY:
+        TaskFactory::delayTask(400);
         break;
       default:
+        // Should never happen.
         sif::error << "PdecHandler::performOperation: Invalid state" << std::endl;
+        TaskFactory::delayTask(400);
         break;
     }
   }
@@ -226,6 +188,71 @@ ReturnValue_t PdecHandler::irqOperation() {
   return returnvalue::OK;
 }
 
+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))) {
+    sif::error << "PdecHandler::irqOperation: Unmasking IRQ failed" << std::endl;
+    triggerEvent(WRITE_SYSCALL_ERROR_PDEC, errno);
+    close(fd);
+    state = State::INIT;
+    return returnvalue::FAILED;
+  }
+  struct pollfd fds = {.fd = fd, .events = POLLIN, .revents = 0};
+  int ret = poll(&fds, 1, IRQ_TIMEOUT_MS);
+  if (ret == 0) {
+    // No TCs for timeout period
+    checkLocks();
+    genericCheckCd.resetTimer();
+    resetIrqLimiters();
+  } else if (ret >= 1) {
+    // Interrupt handling.
+    nb = read(fd, &info, sizeof(info));
+    interruptCounter++;
+    if (nb == static_cast<ssize_t>(sizeof(info))) {
+      uint32_t pisr = *(registerBaseAddress + PDEC_PISR_OFFSET);
+      if ((pisr & TC_NEW_MASK) == TC_NEW_MASK) {
+        // handle TC
+        handleNewTc();
+      }
+      if ((pisr & TC_ABORT_MASK) == TC_ABORT_MASK) {
+        tcAbortCounter += 1;
+      }
+      if ((pisr & NEW_FAR_MASK) == NEW_FAR_MASK) {
+        // Read FAR here
+        CURRENT_FAR = readFar();
+        checkFrameAna(CURRENT_FAR);
+      }
+      // Clear interrupts with dummy read. Volatile is important here to prevent
+      // compiler opitmizations in release builds!
+      volatile uint32_t dummy = *(registerBaseAddress + PDEC_PIR_OFFSET);
+      static_cast<void>(dummy);
+
+      if (genericCheckCd.hasTimedOut()) {
+        checkLocks();
+        genericCheckCd.resetTimer();
+        if (interruptWindowCd.hasTimedOut()) {
+          if (interruptCounter >= MAX_ALLOWED_IRQS_PER_WINDOW) {
+            sif::error << "PdecHandler::irqOperation: Possible IRQ storm" << std::endl;
+            triggerEvent(TOO_MANY_IRQS, MAX_ALLOWED_IRQS_PER_WINDOW);
+            resetIrqLimiters();
+            TaskFactory::delayTask(400);
+            return returnvalue::FAILED;
+          }
+          resetIrqLimiters();
+        }
+      }
+    }
+  } else {
+    sif::error << "PdecHandler::irqOperation: Poll error with errno " << errno << ": "
+               << strerror(errno) << std::endl;
+    triggerEvent(POLL_SYSCALL_ERROR_PDEC, errno);
+    close(fd);
+    state = State::INIT;
+    return returnvalue::FAILED;
+  }
+  return returnvalue::OK;
+}
+
 void PdecHandler::readCommandQueue(void) {
   CommandMessage commandMessage;
   ReturnValue_t result = returnvalue::FAILED;
@@ -618,6 +645,11 @@ void PdecHandler::printPdecMon() {
 
 uint32_t PdecHandler::readFar() { return *(registerBaseAddress + PDEC_FAR_OFFSET); }
 
+void PdecHandler::resetIrqLimiters() {
+  interruptWindowCd.resetTimer();
+  interruptCounter = 0;
+}
+
 std::string PdecHandler::getMonStatusString(uint32_t status) {
   switch (status) {
     case TC_CHANNEL_INACTIVE:

linux/ipcore/PdecHandler.h

@@ -87,10 +87,12 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
   static const Event LOST_CARRIER_LOCK_PDEC = MAKE_EVENT(5, severity::INFO);
   //! [EXPORT] : [COMMENT] Lost bit lock
   static const Event LOST_BIT_LOCK_PDEC = MAKE_EVENT(6, severity::INFO);
+  //! [EXPORT] : [COMMENT] Too many IRQs over the time window of one second. P1: Allowed TCs
+  static constexpr Event TOO_MANY_IRQS = MAKE_EVENT(7, severity::MEDIUM);
   static constexpr Event POLL_SYSCALL_ERROR_PDEC =
-      event::makeEvent(SUBSYSTEM_ID, 7, severity::MEDIUM);
-  static constexpr Event WRITE_SYSCALL_ERROR_PDEC =
       event::makeEvent(SUBSYSTEM_ID, 8, severity::MEDIUM);
+  static constexpr Event WRITE_SYSCALL_ERROR_PDEC =
+      event::makeEvent(SUBSYSTEM_ID, 9, severity::MEDIUM);
 
  private:
   static const uint8_t INTERFACE_ID = CLASS_ID::PDEC_HANDLER;
@@ -180,6 +182,8 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
   // discarded
   static const uint8_t MAP_CLK_FREQ = 2;
 
+  static constexpr uint32_t MAX_ALLOWED_IRQS_PER_WINDOW = 800;
+
   enum class FrameAna_t : uint8_t {
     ABANDONED_CLTU,
     FRAME_DIRTY,
@@ -206,13 +210,16 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
 
   static uint32_t CURRENT_FAR;
 
-  Countdown lockCheckCd = Countdown(IRQ_TIMEOUT_MS);
+  Countdown genericCheckCd = Countdown(IRQ_TIMEOUT_MS);
   object_id_t tcDestinationId;
 
   AcceptsTelecommandsIF* tcDestination = nullptr;
 
   LinuxLibgpioIF* gpioComIF = nullptr;
 
+  uint32_t interruptCounter = 0;
+  Countdown interruptWindowCd = Countdown(1000);
+
   /**
    * Reset signal is required to hold PDEC in reset state until the configuration has been
    * written to the appropriate memory space.
@@ -259,6 +266,7 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
 
   ReturnValue_t polledOperation();
   ReturnValue_t irqOperation();
+  ReturnValue_t checkAndHandleIrqs(int fd, uint32_t& info);
 
   uint32_t readFar();
 
@@ -294,6 +302,8 @@ class PdecHandler : public SystemObject, public ExecutableObjectIF, public HasAc
    */
   void checkLocks();
 
+  void resetIrqLimiters();
+
   /**
    * @brief   Analyzes the FramAna field (frame analysis data) of a FAR report.
    *

mission/acsDefs.h

@@ -25,6 +25,10 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
 static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
 //!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained.
 static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
+//!< MEKF was not able to compute a solution.
+static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
+//!< MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.
+static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(4, severity::HIGH);
 
 extern const char* getModeStr(AcsMode mode);
 

mission/comDefs.h

@@ -24,7 +24,7 @@ enum class CcsdsSubmode : uint8_t {
   DATARATE_HIGH = 2,
   DATARATE_DEFAULT = 3
 };
-enum class ParameterId : uint8_t { DATARATE = 0 };
+enum class ParameterId : uint8_t { DATARATE = 0, TRANSMITTER_TIMEOUT = 1 };
 
 }  // namespace com
 

mission/controller/AcsController.cpp

@@ -1,9 +1,8 @@
 #include "AcsController.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>
-
-#include "mission/acsDefs.h"
-#include "mission/config/torquer.h"
+#include <mission/acsDefs.h>
+#include <mission/config/torquer.h>
 
 AcsController::AcsController(object_id_t objectId)
     : ExtendedControllerBase(objectId),
@@ -14,8 +13,6 @@ AcsController::AcsController(object_id_t objectId)
       safeCtrl(&acsParameters),
       detumble(&acsParameters),
       ptgCtrl(&acsParameters),
-      detumbleCounter{0},
-      multipleRwUnavailableCounter{0},
       parameterHelper(this),
       mgmDataRaw(this),
       mgmDataProcessed(this),
@@ -28,6 +25,14 @@ AcsController::AcsController(object_id_t objectId)
       ctrlValData(this),
       actuatorCmdData(this) {}
 
+ReturnValue_t AcsController::initialize() {
+  ReturnValue_t result = parameterHelper.initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return ExtendedControllerBase::initialize();
+}
+
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
   ReturnValue_t result = actionHelper.handleActionMessage(message);
   if (result == returnvalue::OK) {
@@ -40,6 +45,26 @@ ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
   return result;
 }
 
+ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
+                                           const uint8_t *data, size_t size) {
+  switch (actionId) {
+    case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
+      ReturnValue_t result = guidance.solarArrayDeploymentComplete();
+      if (result == returnvalue::FAILED) {
+        return FILE_DELETION_FAILED;
+      }
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
+    case RESET_MEKF: {
+      navigation.resetMekf(&mekfData);
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
+    default: {
+      return HasActionsIF::INVALID_ACTION_ID;
+    }
+  }
+}
+
 MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
 
 ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
@@ -54,6 +79,25 @@ void AcsController::performControlOperation() {
 #if OBSW_THREAD_TRACING == 1
   trace::threadTrace(opCounter, "ACS & TCS PST");
 #endif
+  {
+    PoolReadGuard pg(&mgmDataRaw);
+    if (pg.getReadResult() == returnvalue::OK) {
+      copyMgmData();
+    }
+  }
+  {
+    PoolReadGuard pg(&susDataRaw);
+    if (pg.getReadResult() == returnvalue::OK) {
+      copySusData();
+    }
+  }
+  {
+    PoolReadGuard pg(&gyrDataRaw);
+    if (pg.getReadResult() == returnvalue::OK) {
+      copyGyrData();
+    }
+  }
+
   switch (internalState) {
     case InternalState::STARTUP: {
       initialCountdown.resetTimer();
@@ -89,25 +133,6 @@ void AcsController::performControlOperation() {
     default:
       break;
   }
-
-  {
-    PoolReadGuard pg(&mgmDataRaw);
-    if (pg.getReadResult() == returnvalue::OK) {
-      copyMgmData();
-    }
-  }
-  {
-    PoolReadGuard pg(&susDataRaw);
-    if (pg.getReadResult() == returnvalue::OK) {
-      copySusData();
-    }
-  }
-  {
-    PoolReadGuard pg(&gyrDataRaw);
-    if (pg.getReadResult() == returnvalue::OK) {
-      copyGyrData();
-    }
-  }
 }
 
 void AcsController::performSafe() {
@@ -116,17 +141,24 @@ void AcsController::performSafe() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
-  // give desired satellite rate and sun direction to align
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
+      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_INFO);
+      mekfInvalidFlag = true;
+    }
+  } else {
+    mekfInvalidFlag = false;
+  }
+  // get desired satellite rate and sun direction to align
   double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
   guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
   // if MEKF is working
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
   bool magMomMtqValid = false;
-  if (validMekf == returnvalue::OK) {
+  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(),
@@ -141,24 +173,9 @@ void AcsController::performSafe() {
         sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
   }
 
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
 
-  {
-    PoolReadGuard pg(&ctrlValData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      double unitQuat[4] = {0, 0, 0, 1};
-      std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
-      ctrlValData.tgtQuat.setValid(false);
-      std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
-      ctrlValData.errQuat.setValid(false);
-      ctrlValData.errAng.value = errAng;
-      ctrlValData.errAng.setValid(true);
-      ctrlValData.setValidity(true, false);
-    }
-  }
-
-  //	Detumble check and switch
+  // detumble check and switch
   if (mekfData.satRotRateMekf.isValid() &&
       VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
           acsParameters.detumbleParameter.omegaDetumbleStart) {
@@ -176,20 +193,8 @@ void AcsController::performSafe() {
     triggerEvent(acs::SAFE_RATE_VIOLATION);
   }
 
-  {
-    PoolReadGuard pg(&actuatorCmdData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      int32_t zeroVec[4] = {0, 0, 0, 0};
-      std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
-      actuatorCmdData.rwTargetTorque.setValid(false);
-      std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
-      actuatorCmdData.rwTargetSpeed.setValid(false);
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
-      actuatorCmdData.mtqTargetDipole.setValid(true);
-      actuatorCmdData.setValidity(true, false);
-    }
-  }
-
+  updateCtrlValData(errAng);
+  updateActuatorCmdData(cmdDipolMtqs);
   //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
   //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
   //                   acsParameters.rwHandlingParameters.rampTime);
@@ -201,15 +206,21 @@ void AcsController::performDetumble() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
+      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_INFO);
+      mekfInvalidFlag = true;
+    }
+  } else {
+    mekfInvalidFlag = false;
+  }
   double magMomMtq[3] = {0, 0, 0};
   detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
                    mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
                    mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
 
   if (mekfData.satRotRateMekf.isValid() &&
@@ -229,19 +240,8 @@ void AcsController::performDetumble() {
     triggerEvent(acs::SAFE_RATE_RECOVERY);
   }
 
-  {
-    PoolReadGuard pg(&actuatorCmdData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double));
-      actuatorCmdData.rwTargetTorque.setValid(false);
-      std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
-      actuatorCmdData.rwTargetSpeed.setValid(false);
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
-      actuatorCmdData.mtqTargetDipole.setValid(true);
-      actuatorCmdData.setValidity(true, false);
-    }
-  }
-
+  disableCtrlValData();
+  updateActuatorCmdData(cmdDipolMtqs);
   //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
   //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
   //                   acsParameters.rwHandlingParameters.rampTime);
@@ -253,23 +253,35 @@ void AcsController::performPointingCtrl() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
-  double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
-  double quatRef[4] = {0, 0, 0, 0};
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
+      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_INFO);
+      mekfInvalidFlag = true;
+    }
+    if (mekfInvalidCounter > 4) {
+      triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION);
+    }
+    mekfInvalidCounter++;
+    //    commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration,
+    //    cmdSpeedRws[0],
+    //                     cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
+    //                     acsParameters.rwHandlingParameters.rampTime);
+    return;
+  } else {
+    mekfInvalidFlag = false;
+    mekfInvalidCounter = 0;
+  }
   uint8_t enableAntiStiction = true;
-
-  double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
-         deltaRate[3] = {0, 0, 0};  // ToDo: check if pointer needed
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
+  result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
-    multipleRwUnavailableCounter++;
     if (multipleRwUnavailableCounter > 4) {
       triggerEvent(acs::MULTIPLE_RW_INVALID);
     }
+    multipleRwUnavailableCounter++;
     return;
   } else {
     multipleRwUnavailableCounter = 0;
@@ -278,40 +290,37 @@ void AcsController::performPointingCtrl() {
   double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
   double mgtDpDes[3] = {0, 0, 0};
 
+  // Variables required for guidance
+  double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
+         errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
   switch (submode) {
     case acs::PTG_IDLE:
-      guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
-                          targetQuat, refSatRate);
-      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
-                  4 * sizeof(double));
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
-                     *rwPseudoInv, torquePtgRws);
+      guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
       ptgCtrl.ptgNullspace(
-          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
+          &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
           &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
           &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
       VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
       actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
       ptgCtrl.ptgDesaturation(
-          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
+          &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
-
+      enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET:
-      guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
-                                      refSatRate);
-      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
-                  4 * sizeof(double));
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+      guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
+                                      gpsDataProcessed.gpsVelocity.value, targetQuat,
+                                      targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
+                          acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -324,17 +333,15 @@ void AcsController::performPointingCtrl() {
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET_GS:
-      guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
-                               targetQuat, refSatRate);
-      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
-                  4 * sizeof(double));
-      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+      guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
+                               susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -347,16 +354,18 @@ void AcsController::performPointingCtrl() {
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_NADIR:
-      guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
-                                     refSatRate);
-      std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
-      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
+      guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
+                                           gpsDataProcessed.gpsVelocity.value, targetQuat,
+                                           targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
+                          acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
+      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -369,16 +378,17 @@ void AcsController::performPointingCtrl() {
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_INERTIAL:
-      guidance.inertialQuatPtg(targetQuat, refSatRate);
-      std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
+      std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
                   4 * sizeof(double));
-      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
-      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
-                          deltaRate);
-      ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
+                          acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
+      ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(
           &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
@@ -391,6 +401,7 @@ void AcsController::performPointingCtrl() {
           mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
           &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
           &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
       break;
   }
 
@@ -398,24 +409,14 @@ void AcsController::performPointingCtrl() {
     ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
   }
 
-  int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
   actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
                             sensorValues.rw2Set.currSpeed.value,
                             sensorValues.rw3Set.currSpeed.value,
                             sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
   actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
 
-  {
-    PoolReadGuard pg(&actuatorCmdData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
-      std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
-      actuatorCmdData.setValidity(true, true);
-    }
-  }
-
+  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
+  updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
   //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
   //                   acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
   //                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
@@ -451,6 +452,60 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
   return returnvalue::OK;
 }
 
+void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) {
+  updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole);
+}
+
+void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
+                                          const int32_t *rwTargetSpeed,
+                                          const int16_t *mtqTargetDipole) {
+  PoolReadGuard pg(&actuatorCmdData);
+  if (pg.getReadResult() == returnvalue::OK) {
+    std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
+    std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
+    std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
+    actuatorCmdData.setValidity(true, true);
+  }
+}
+
+void AcsController::updateCtrlValData(double errAng) {
+  PoolReadGuard pg(&ctrlValData);
+  if (pg.getReadResult() == returnvalue::OK) {
+    std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
+    ctrlValData.tgtQuat.setValid(false);
+    std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
+    ctrlValData.errQuat.setValid(false);
+    ctrlValData.errAng.value = errAng;
+    ctrlValData.errAng.setValid(true);
+    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
+    ctrlValData.tgtRotRate.setValid(false);
+    ctrlValData.setValidity(true, false);
+  }
+}
+
+void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
+                                      const double *tgtRotRate) {
+  PoolReadGuard pg(&ctrlValData);
+  if (pg.getReadResult() == returnvalue::OK) {
+    std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
+    std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
+    ctrlValData.errAng.value = errAng;
+    std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
+    ctrlValData.setValidity(true, true);
+  }
+}
+
+void AcsController::disableCtrlValData() {
+  PoolReadGuard pg(&ctrlValData);
+  if (pg.getReadResult() == returnvalue::OK) {
+    std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double));
+    std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double));
+    ctrlValData.errAng.value = 0;
+    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double));
+    ctrlValData.setValidity(false, true);
+  }
+}
+
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
   // MGM Raw
@@ -524,11 +579,13 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   // MEKF
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
+  localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
   poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0});
   // Ctrl Values
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
+  localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
   poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
   // Actuator CMD
   localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
@@ -795,11 +852,3 @@ void AcsController::copyGyrData() {
     }
   }
 }
-
-ReturnValue_t AcsController::initialize() {
-  ReturnValue_t result = parameterHelper.initialize();
-  if (result != returnvalue::OK) {
-    return result;
-  }
-  return ExtendedControllerBase::initialize();
-}

mission/controller/AcsController.h

@@ -1,26 +1,27 @@
 #ifndef MISSION_CONTROLLER_ACSCONTROLLER_H_
 #define MISSION_CONTROLLER_ACSCONTROLLER_H_
 
+#include <eive/objects.h>
 #include <fsfw/controller/ExtendedControllerBase.h>
 #include <fsfw/globalfunctions/math/VectorOperations.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/trace.h>
 
 #include "acs/ActuatorCmd.h"
 #include "acs/Guidance.h"
+#include "acs/MultiplicativeKalmanFilter.h"
 #include "acs/Navigation.h"
 #include "acs/SensorProcessing.h"
 #include "acs/control/Detumble.h"
 #include "acs/control/PtgCtrl.h"
 #include "acs/control/SafeCtrl.h"
 #include "controllerdefinitions/AcsCtrlDefinitions.h"
-#include "eive/objects.h"
-#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
-#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
-#include "mission/devices/devicedefinitions/SusDefinitions.h"
-#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
-#include "mission/trace.h"
 
 class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
  public:
@@ -39,6 +40,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void performPointingCtrl();
 
  private:
+  static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
+  static constexpr double ZERO_VEC[3] = {0, 0, 0};
+  static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0};
+  static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0};
+
   AcsParameters acsParameters;
   SensorProcessing sensorProcessing;
   Navigation navigation;
@@ -49,23 +55,37 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   Detumble detumble;
   PtgCtrl ptgCtrl;
 
-  uint8_t detumbleCounter;
-  uint8_t multipleRwUnavailableCounter;
-
   ParameterHelper parameterHelper;
 
+  uint8_t detumbleCounter = 0;
+  uint8_t multipleRwUnavailableCounter = 0;
+  bool mekfInvalidFlag = false;
+  uint8_t mekfInvalidCounter = 0;
+  int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
+  int16_t cmdDipolMtqs[3] = {0, 0, 0};
+
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;
 #endif
 
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };
-
   InternalState internalState = InternalState::STARTUP;
 
+  /** Device command IDs */
+  static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
+  static const DeviceCommandId_t RESET_MEKF = 0x1;
+
+  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
+  static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
+
   ReturnValue_t initialize() override;
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;
   void performControlOperation() override;
 
+  /* HasActionsIF overrides */
+  ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
+                              const uint8_t* data, size_t size) override;
+
   ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                         LocalDataPoolManager& poolManager) override;
   LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
@@ -79,12 +99,19 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
                                  uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
                                  int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
+  void updateActuatorCmdData(const int16_t* mtqTargetDipole);
+  void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
+                             const int16_t* mtqTargetDipole);
+  void updateCtrlValData(double errAng);
+  void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
+                         const double* tgtRotRate);
+  void disableCtrlValData();
 
   /* ACS Sensor Values */
   ACS::SensorValues sensorValues;
 
   /* ACS Actuation Datasets */
-  IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
+  imtq::DipoleActuationSet dipoleSet = imtq::DipoleActuationSet(objects::IMTQ_HANDLER);
   rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1);
   rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2);
   rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3);
@@ -169,12 +196,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   acsctrl::MekfData mekfData;
   PoolEntry<double> quatMekf = PoolEntry<double>(4);
   PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
+  PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
 
   // Ctrl Values
   acsctrl::CtrlValData ctrlValData;
   PoolEntry<double> tgtQuat = PoolEntry<double>(4);
   PoolEntry<double> errQuat = PoolEntry<double>(4);
   PoolEntry<double> errAng = PoolEntry<double>();
+  PoolEntry<double> tgtRotRate = PoolEntry<double>(3);
 
   // Actuator CMD
   acsctrl::ActuatorCmdData actuatorCmdData;

mission/controller/ThermalController.cpp

@@ -10,7 +10,7 @@
 #include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
 #include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
-#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 #include <objects/systemObjectList.h>
@@ -21,6 +21,7 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater
       sensorTemperatures(this),
       susTemperatures(this),
       deviceTemperatures(this),
+      heaterInfo(this),
       imtqThermalSet(objects::IMTQ_HANDLER, ThermalStateCfg()),
       max31865Set0(objects::RTD_0_IC3_PLOC_HEATSPREADER,
                    EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
@@ -112,108 +113,88 @@ void ThermalController::performControlOperation() {
     deviceTemperatures.commit();
   }
 
+  std::array<HeaterHandler::SwitchState, 8> heaterStates;
+  heaterHandler.getAllSwitchStates(heaterStates);
+  {
+    PoolReadGuard pg(&heaterInfo);
+    std::memcpy(heaterInfo.heaterSwitchState.value, heaterStates.data(), 8);
+    {
+      PoolReadGuard pg2(&currentVecPdu2);
+      if (pg.getReadResult() == returnvalue::OK and pg2.getReadResult() == returnvalue::OK) {
+        heaterInfo.heaterCurrent.value = currentVecPdu2.value[PDU2::Channels::TCS_HEATER_IN];
+      }
+    }
+  }
+
   // performThermalModuleCtrl();
 }
 
 ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                          LocalDataPoolManager& poolManager) {
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_HEATSPREADER,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_MISSIONBOARD,
-                           new PoolEntry<float>({1.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_4K_CAMERA,
-                           new PoolEntry<float>({2.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DAC_HEATSPREADER,
-                           new PoolEntry<float>({3.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_STARTRACKER,
-                           new PoolEntry<float>({4.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_RW1, new PoolEntry<float>({5.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DRO, new PoolEntry<float>({6.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_SCEX, new PoolEntry<float>({7.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_X8, new PoolEntry<float>({8.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_HPA, new PoolEntry<float>({9.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TX_MODUL,
-                           new PoolEntry<float>({10.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MPA, new PoolEntry<float>({11.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_ACU, new PoolEntry<float>({12.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLPCDU_HEATSPREADER,
-                           new PoolEntry<float>({13.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TCS_BOARD,
-                           new PoolEntry<float>({14.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MAGNETTORQUER,
-                           new PoolEntry<float>({15.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_HEATSPREADER, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_MISSIONBOARD, new PoolEntry<float>({1.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_4K_CAMERA, new PoolEntry<float>({2.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_DAC_HEATSPREADER, new PoolEntry<float>({3.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_STARTRACKER, new PoolEntry<float>({4.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_RW1, new PoolEntry<float>({5.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_DRO, new PoolEntry<float>({6.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_SCEX, new PoolEntry<float>({7.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_X8, new PoolEntry<float>({8.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_HPA, new PoolEntry<float>({9.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TX_MODUL, new PoolEntry<float>({10.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_MPA, new PoolEntry<float>({11.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_ACU, new PoolEntry<float>({12.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLPCDU_HEATSPREADER, new PoolEntry<float>({13.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TCS_BOARD, new PoolEntry<float>({14.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_MAGNETTORQUER, new PoolEntry<float>({15.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);
 
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_0_N_LOC_XFYFZM_PT_XF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_6_R_LOC_XFYBZM_PT_XF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_1_N_LOC_XBYFZM_PT_XB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_7_R_LOC_XBYBZM_PT_XB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_2_N_LOC_XFYBZB_PT_YB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_8_R_LOC_XBYBZB_PT_YB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_3_N_LOC_XFYBZF_PT_YF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_9_R_LOC_XBYBZB_PT_YF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                           new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_0_N_LOC_XFYFZM_PT_XF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_6_R_LOC_XFYBZM_PT_XF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_1_N_LOC_XBYFZM_PT_XB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_7_R_LOC_XBYBZM_PT_XB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_2_N_LOC_XFYBZB_PT_YB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_8_R_LOC_XBYBZB_PT_YB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_3_N_LOC_XFYBZF_PT_YF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_9_R_LOC_XBYBZB_PT_YF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_4_N_LOC_XMYFZF_PT_ZF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_10_N_LOC_XMYBZF_PT_ZF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_5_N_LOC_XFYMZB_PT_ZB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_11_R_LOC_XBYMZB_PT_ZB, new PoolEntry<float>({0.0}));
 
-  localDataPoolMap.emplace(thermalControllerDefinitions::COMPONENT_RW, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::COMPONENT_RW, new PoolEntry<float>({0.0}));
 
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_Q7S, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_1,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_2,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_3,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_4,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW1, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW2, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW3, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW4, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_STAR_TRACKER,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_SYRLINKS_POWER_AMPLIFIER,
-                           new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_SYRLINKS_BASEBAND_BOARD,
-                           new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGT, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_ACU, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_PDU1, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_PDU2, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_0_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_1_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_2_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_3_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGM_0_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGM_2_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_ADC_PAYLOAD_PCDU,
-                           new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_Q7S, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_1, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_2, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_3, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_4, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW1, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW2, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW3, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW4, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_STAR_TRACKER, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_POWER_AMPLIFIER, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_BASEBAND_BOARD, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGT, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_ACU, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_PDU1, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_PDU2, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_0_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_1_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_2_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_3_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_0_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_2_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_ADC_PAYLOAD_PCDU, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::HEATER_SWITCH_LIST, &heaterSwitchStates);
+  localDataPoolMap.emplace(tcsCtrl::HEATER_CURRENT, &heaterCurrent);
 
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(sensorTemperatures.getSid(), false, 10.0));
@@ -221,17 +202,21 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo
       subdp::RegularHkPeriodicParams(susTemperatures.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), false, 10.0));
+  poolManager.subscribeForDiagPeriodicPacket(
+      subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), false, 10.0));
   return returnvalue::OK;
 }
 
 LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) {
   switch (sid.ownerSetId) {
-    case thermalControllerDefinitions::SENSOR_TEMPERATURES:
+    case tcsCtrl::SENSOR_TEMPERATURES:
       return &sensorTemperatures;
-    case thermalControllerDefinitions::SUS_TEMPERATURES:
+    case tcsCtrl::SUS_TEMPERATURES:
       return &susTemperatures;
-    case thermalControllerDefinitions::DEVICE_TEMPERATURES:
+    case tcsCtrl::DEVICE_TEMPERATURES:
       return &deviceTemperatures;
+    case tcsCtrl::HEATER_SET:
+      return &heaterInfo;
     default:
       return nullptr;
   }
@@ -785,7 +770,7 @@ void ThermalController::copyDevices() {
   }
 
   {
-    lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, IMTQ::MCU_TEMPERATURE);
+    lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, imtq::MCU_TEMPERATURE);
     PoolReadGuard pg(&tempMgt, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl;

mission/controller/ThermalController.h

@@ -12,6 +12,7 @@
 #include <list>
 
 #include "mission/devices/HeaterHandler.h"
+#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
 #include "mission/trace.h"
 
 /**
@@ -76,9 +77,12 @@ class ThermalController : public ExtendedControllerBase {
 
   HeaterHandler& heaterHandler;
 
-  thermalControllerDefinitions::SensorTemperatures sensorTemperatures;
-  thermalControllerDefinitions::SusTemperatures susTemperatures;
-  thermalControllerDefinitions::DeviceTemperatures deviceTemperatures;
+  tcsCtrl::SensorTemperatures sensorTemperatures;
+  tcsCtrl::SusTemperatures susTemperatures;
+  tcsCtrl::DeviceTemperatures deviceTemperatures;
+  tcsCtrl::HeaterInfo heaterInfo;
+  lp_vec_t<int16_t, 9> currentVecPdu2 =
+      lp_vec_t<int16_t, 9>(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS));
 
   DeviceHandlerThermalSet imtqThermalSet;
 
@@ -160,11 +164,13 @@ class ThermalController : public ExtendedControllerBase {
   std::array<std::pair<bool, double>, 5> sensors;
   uint8_t numSensors = 0;
 
-  PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075IfBrd = PoolEntry<float>(10.0);
+  PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075IfBrd = PoolEntry<float>({10.0});
+  PoolEntry<uint8_t> heaterSwitchStates = PoolEntry<uint8_t>(heater::NUMBER_OF_SWITCHES);
+  PoolEntry<int16_t> heaterCurrent = PoolEntry<int16_t>();
 
   static constexpr dur_millis_t MUTEX_TIMEOUT = 50;
 

mission/controller/acs/AcsParameters.cpp

@@ -342,7 +342,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0x9):  // TargetModeControllerParameters
+    case (0x9):  // IdleModeControllerParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(targetModeControllerParameters.zeta);
+          break;
+        case 0x1:
+          parameterWrapper->set(targetModeControllerParameters.om);
+          break;
+        case 0x2:
+          parameterWrapper->set(targetModeControllerParameters.omMax);
+          break;
+        case 0x3:
+          parameterWrapper->set(targetModeControllerParameters.qiMin);
+          break;
+        case 0x4:
+          parameterWrapper->set(targetModeControllerParameters.gainNullspace);
+          break;
+        case 0x5:
+          parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
+          break;
+        case 0x6:
+          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
+          break;
+        case 0x7:
+          parameterWrapper->set(targetModeControllerParameters.desatOn);
+          break;
+        case 0x8:
+          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          break;
+
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0xA):  // TargetModeControllerParameters
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(targetModeControllerParameters.zeta);
@@ -408,7 +442,61 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0xA):  // NadirModeControllerParameters
+    case (0xB):  // GsTargetModeControllerParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(targetModeControllerParameters.zeta);
+          break;
+        case 0x1:
+          parameterWrapper->set(targetModeControllerParameters.om);
+          break;
+        case 0x2:
+          parameterWrapper->set(targetModeControllerParameters.omMax);
+          break;
+        case 0x3:
+          parameterWrapper->set(targetModeControllerParameters.qiMin);
+          break;
+        case 0x4:
+          parameterWrapper->set(targetModeControllerParameters.gainNullspace);
+          break;
+        case 0x5:
+          parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
+          break;
+        case 0x6:
+          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
+          break;
+        case 0x7:
+          parameterWrapper->set(targetModeControllerParameters.desatOn);
+          break;
+        case 0x8:
+          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          break;
+        case 0x9:
+          parameterWrapper->set(targetModeControllerParameters.refDirection);
+          break;
+        case 0xA:
+          parameterWrapper->set(targetModeControllerParameters.refRotRate);
+          break;
+        case 0xB:
+          parameterWrapper->set(targetModeControllerParameters.quatRef);
+          break;
+        case 0xC:
+          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
+          break;
+        case 0xD:
+          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
+          break;
+        case 0xE:
+          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
+          break;
+        case 0xF:
+          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0xC):  // NadirModeControllerParameters
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(nadirModeControllerParameters.zeta);
@@ -450,7 +538,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0xB):  // InertialModeControllerParameters
+    case (0xD):  // InertialModeControllerParameters
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(inertialModeControllerParameters.zeta);
@@ -492,7 +580,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0xC):  // StrParameters
+    case (0xE):  // StrParameters
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(strParameters.exclusionAngle);
@@ -504,7 +592,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0xD):  // GpsParameters
+    case (0xF):  // GpsParameters
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
@@ -513,7 +601,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0xE):  // SunModelParameters
+    case (0x10):  // SunModelParameters
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(sunModelParameters.domega);
@@ -543,7 +631,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0xF):  // KalmanFilterParameters
+    case (0x11):  // KalmanFilterParameters
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
@@ -567,7 +655,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0x10):  // MagnetorquesParameter
+    case (0x12):  // MagnetorquesParameter
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
@@ -594,7 +682,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           return INVALID_IDENTIFIER_ID;
       }
       break;
-    case (0x11):  // DetumbleParameter
+    case (0x13):  // DetumbleParameter
       switch (parameterId) {
         case 0x0:
           parameterWrapper->set(detumbleParameter.detumblecounter);

mission/controller/acs/AcsParameters.h

@@ -1,8 +1,3 @@
-/*******************************
- *	EIVE Flight Software Framework (FSFW)
- * (c) 2022 IRS, Uni Stuttgart
- *******************************/
-
 #ifndef ACSPARAMETERS_H_
 #define ACSPARAMETERS_H_
 
@@ -841,10 +836,12 @@ class AcsParameters : public HasParametersIF {
     uint8_t enableAntiStiction = true;
   } pointingLawParameters;
 
+  struct IdleModeControllerParameters : PointingLawParameters {
+  } idleModeControllerParameters;
+
   struct TargetModeControllerParameters : PointingLawParameters {
     double refDirection[3] = {-1, 0, 0};  // Antenna
-    double refRotRate[3] = {0, 0, 0};     // Not used atm, do we want an option to
-    // give this as an input- currently en calculation is done
+    double refRotRate[3] = {0, 0, 0};
     double quatRef[4] = {0, 0, 0, 1};
     int8_t timeElapsedMax = 10;  // rot rate calculations
 
@@ -860,9 +857,20 @@ class AcsParameters : public HasParametersIF {
     double blindRotRate = 1 * M_PI / 180;
   } targetModeControllerParameters;
 
+  struct GsTargetModeControllerParameters : PointingLawParameters {
+    double refDirection[3] = {-1, 0, 0};  // Antenna
+    int8_t timeElapsedMax = 10;           // rot rate calculations
+
+    // Default is Stuttgart GS
+    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
+    double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
+    double altitudeTgt = 500;                     // [m]
+  } gsTargetModeControllerParameters;
+
   struct NadirModeControllerParameters : PointingLawParameters {
     double refDirection[3] = {-1, 0, 0};  // Antenna
     double quatRef[4] = {0, 0, 0, 1};
+    double refRotRate[3] = {0, 0, 0};
     int8_t timeElapsedMax = 10;  // rot rate calculations
   } nadirModeControllerParameters;
 

mission/controller/acs/Guidance.cpp

@@ -1,10 +1,3 @@
-/*
- * Guidance.cpp
- *
- *  Created on: 6 Jun 2022
- *      Author: Robin Marquardt
- */
-
 #include "Guidance.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>
@@ -19,74 +12,50 @@
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
 
-Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
+Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
 
 Guidance::~Guidance() {}
 
-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::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
-                3 * sizeof(double));
-  } else {
-    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
-                3 * sizeof(double));
-  }
-  std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
-              3 * sizeof(double));
-}
-
-void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                                       acsctrl::SusDataProcessed *susDataProcessed,
-                                       acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
-                                       double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3],
+                                       double sunDirI[3], double refDirB[3], double quatBI[4],
+                                       double targetQuat[4], double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
   //-------------------------------------------------------------------------------------
-  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
-  // fixed/centered frame)
-  double targetCart[3] = {0, 0, 0};
+  // transform longitude, latitude and altitude to ECEF
+  double targetE[3] = {0, 0, 0};
 
   MathOperations<double>::cartesianFromLatLongAlt(
       acsParameters.targetModeControllerParameters.latitudeTgt,
       acsParameters.targetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
 
-  //	Position of the satellite in the earth/fixed frame via GPS
-  double posSatE[3] = {0, 0, 0};
-  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
-  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
-  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
-                                                  sensorValues->gpsSet.altitude.value, posSatE);
-
-  //	Target direction in the ECEF frame
+  // target direction in the ECEF frame
   double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
+  VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
 
-  //	Transformation between ECEF and IJK frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+  // transformation between ECEF and ECI frame
+  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
 
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
 
-  //	Transformation between ECEF and Body frame
-  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  // transformation between ECEF and Body frame
+  double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double quatBJ[4] = {0, 0, 0, 0};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
 
-  QuaternionOperations::toDcm(quatBJ, dcmBJ);
-  MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
+  QuaternionOperations::toDcm(quatBI, dcmBI);
+  MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
 
-  //	Target Direction in the body frame
+  // target Direction in the body frame
   double targetDirB[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
 
-  //	rotation quaternion from two vectors
+  // rotation quaternion from two vectors
   double refDir[3] = {0, 0, 0};
   refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
   refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
@@ -106,15 +75,13 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
   VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
 
   //-------------------------------------------------------------------------------------
-  //	Calculation of reference rotation rate
+  // calculation of reference rotation rate
   //-------------------------------------------------------------------------------------
-  double velSatE[3] = {0, 0, 0};
-  std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
   double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
-  //	Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
+  // velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
   MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
   double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MatrixOperations<double>::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3);
+  MatrixOperations<double>::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3);
   MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
   VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
 
@@ -124,21 +91,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
   double satRateDir[3] = {0, 0, 0};
   VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
   VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
-  VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3);
+  VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3);
 
   //-------------------------------------------------------------------------------------
   //	Calculation of reference rotation rate in case of star tracker blinding
   //-------------------------------------------------------------------------------------
   if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
     double sunDirB[3] = {0, 0, 0};
-
-    if (susDataProcessed->sunIjkModel.isValid()) {
-      double sunDirJ[3] = {0, 0, 0};
-      std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
-      MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
-    } else {
-      std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
-    }
+    MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
 
     double exclAngle = acsParameters.strParameters.exclusionAngle,
            blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
@@ -148,18 +108,13 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
 
     if (!(strBlindAvoidFlag)) {
       double critSightAngle = blindStart * exclAngle;
-
       if (sightAngleSun < critSightAngle) {
         strBlindAvoidFlag = true;
       }
-
-    }
-
-    else {
+    } else {
       if (sightAngleSun < blindEnd * exclAngle) {
         double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
         double blindRefRate[3] = {0, 0, 0};
-
         if (sunDirB[1] < 0) {
           blindRefRate[0] = normBlindRefRate;
           blindRefRate[1] = 0;
@@ -169,21 +124,353 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
           blindRefRate[1] = 0;
           blindRefRate[2] = 0;
         }
-
-        VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
-
+        VectorOperations<double>::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3);
       } else {
         strBlindAvoidFlag = false;
       }
     }
   }
+  // revert calculated quaternion from qBX to qIX
+  double quatIB[4] = {0, 0, 0, 1};
+  QuaternionOperations::inverse(quatBI, quatIB);
+  QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
 }
 
-void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
-                               double *refSatRate) {
+void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
+                                      double targetQuat[4], double targetSatRotRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for target pointing
+  //-------------------------------------------------------------------------------------
+  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
+  double targetE[3] = {0, 0, 0};
+  MathOperations<double>::cartesianFromLatLongAlt(
+      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
+
+  // transformation between ECEF and ECI frame
+  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
+
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
+
+  // target direction in the ECI frame
+  double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
+  VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
+
+  // x-axis aligned with target direction
+  // this aligns with the camera, E- and S-band antennas
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirI, xAxis, 3);
+
+  // transform velocity into inertial frame
+  double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
+  VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
+
+  // orbital normal vector of target and velocity vector
+  double orbitalNormalI[3] = {0, 0, 0};
+  VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
+  VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
+
+  // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
+  // resolution
+  double yAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+
+  // z-axis completes RHS
+  double zAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(xAxis, yAxis, zAxis);
+
+  // join transformation matrix
+  double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                        {xAxis[1], yAxis[1], zAxis[1]},
+                        {xAxis[2], yAxis[2], zAxis[2]}};
+  QuaternionOperations::fromDcm(dcmIX, targetQuat);
+
+  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
+}
+
+void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
+                               double targetQuat[4], double targetSatRotRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for ground station pointing
+  //-------------------------------------------------------------------------------------
+  // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
+  double groundStationE[3] = {0, 0, 0};
+
+  MathOperations<double>::cartesianFromLatLongAlt(
+      acsParameters.gsTargetModeControllerParameters.latitudeTgt,
+      acsParameters.gsTargetModeControllerParameters.longitudeTgt,
+      acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
+
+  // transformation between ECEF and ECI frame
+  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
+
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
+
+  // target direction in the ECI frame
+  double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
+  VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
+
+  // negative x-axis aligned with target direction
+  // this aligns with the camera, E- and S-band antennas
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
+  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+
+  // get sun vector model in ECI
+  VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
+
+  // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
+  // z = sPerpenticular =  s - sParallel = s - (x*s)/norm(x)^2 * x
+  double xDotS = VectorOperations<double>::dot(xAxis, sunDirI);
+  xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
+  double sunParallel[3], zAxis[3];
+  VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
+  VectorOperations<double>::subtract(sunDirI, sunParallel, zAxis, 3);
+  VectorOperations<double>::normalize(zAxis, zAxis, 3);
+
+  // y-axis completes RHS
+  double yAxis[3];
+  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+
+  // join transformation matrix
+  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                         {xAxis[1], yAxis[1], zAxis[1]},
+                         {xAxis[2], yAxis[2], zAxis[2]}};
+  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
+
+  int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
+  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
+}
+
+void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion to sun
+  //-------------------------------------------------------------------------------------
+  //  positive z-Axis of EIVE in direction of sun
+  double zAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(sunDirI, zAxis, 3);
+
+  //  assign helper vector (north pole inertial)
+  double helperVec[3] = {0, 0, 1};
+
+  //  construct y-axis from helper vector and z-axis
+  double yAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxis, helperVec, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+
+  //  x-axis completes RHS
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(yAxis, zAxis, xAxis);
+  VectorOperations<double>::normalize(xAxis, xAxis, 3);
+
+  // join transformation matrix
+  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                         {xAxis[1], yAxis[1], zAxis[1]},
+                         {xAxis[2], yAxis[2], zAxis[2]}};
+  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
+
+  //----------------------------------------------------------------------------
+  //	Calculation of reference rotation rate
+  //----------------------------------------------------------------------------
+  refSatRate[0] = 0;
+  refSatRate[1] = 0;
+  refSatRate[2] = 0;
+}
+
+void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
+                                            double targetQuat[4], double refDirB[3],
+                                            double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for Nadir pointing
+  //-------------------------------------------------------------------------------------
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
+
+  // transformation between ECEF and ECI frame
+  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
+
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
+
+  // transformation between ECEF and Body frame
+  double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  QuaternionOperations::toDcm(quatBI, dcmBI);
+  MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
+
+  // target Direction in the body frame
+  double targetDirB[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
+
+  // rotation quaternion from two vectors
+  double refDir[3] = {0, 0, 0};
+  refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
+  refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
+  refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
+  double noramlizedTargetDirB[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
+  VectorOperations<double>::normalize(refDir, refDir, 3);
+  double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
+  double normRefDir = VectorOperations<double>::norm(refDir, 3);
+  double crossDir[3] = {0, 0, 0};
+  double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
+  VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
+  targetQuat[0] = crossDir[0];
+  targetQuat[1] = crossDir[1];
+  targetQuat[2] = crossDir[2];
+  targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
+  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
+
   //-------------------------------------------------------------------------------------
   //	Calculation of reference rotation rate
   //-------------------------------------------------------------------------------------
+  refSatRate[0] = 0;
+  refSatRate[1] = 0;
+  refSatRate[2] = 0;
+
+  // revert calculated quaternion from qBX to qIX
+  double quatIB[4] = {0, 0, 0, 1};
+  QuaternionOperations::inverse(quatBI, quatIB);
+  QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
+}
+
+void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
+                                           double targetQuat[4], double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for Nadir pointing
+  //-------------------------------------------------------------------------------------
+  // transformation between ECEF and ECI frame
+  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
+
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
+
+  // satellite position in inertial reference frame
+  double posSatI[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
+
+  // negative x-axis aligned with position vector
+  // this aligns with the camera, E- and S-band antennas
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(posSatI, xAxis, 3);
+  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+
+  // make z-Axis parallel to major part of camera resolution
+  double zAxis[3] = {0, 0, 0};
+  double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
+  VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
+  VectorOperations<double>::cross(xAxis, velocityI, zAxis);
+  VectorOperations<double>::normalize(zAxis, zAxis, 3);
+
+  // y-Axis completes RHS
+  double yAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
+
+  // join transformation matrix
+  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                         {xAxis[1], yAxis[1], zAxis[1]},
+                         {xAxis[2], yAxis[2], zAxis[2]}};
+  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
+
+  int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
+  targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
+}
+
+void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
+                          double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
+                          double errorQuat[4], double errorSatRotRate[3], double errorAngle) {
+  // First calculate error quaternion between current and target orientation
+  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
+  // Last calculate add rotation from reference quaternion
+  QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
+  // Keep scalar part of quaternion positive
+  if (errorQuat[3] < 0) {
+    VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
+  }
+  // Calculate error angle
+  errorAngle = QuaternionOperations::getAngle(errorQuat, true);
+
+  // Only give back error satellite rotational rate if orientation has already been aquired
+  if (errorAngle < 2. / 180. * M_PI) {
+    // First combine the target and reference satellite rotational rates
+    double combinedRefSatRotRate[3] = {0, 0, 0};
+    VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
+    // Then substract the combined required satellite rotational rates from the actual rate
+    VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate,
+                                       3);
+  } else {
+    // If orientation has not been aquired yet set satellite rotational rate to zero
+    errorSatRotRate = 0;
+  }
+
+  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
+  // under 150 arcsec ??
+}
+
+void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
+                          double targetSatRotRate[3], double errorQuat[4],
+                          double errorSatRotRate[3], double errorAngle) {
+  // First calculate error quaternion between current and target orientation
+  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
+  // Keep scalar part of quaternion positive
+  if (errorQuat[3] < 0) {
+    VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
+  }
+  // Calculate error angle
+  errorAngle = QuaternionOperations::getAngle(errorQuat, true);
+
+  // Only give back error satellite rotational rate if orientation has already been aquired
+  if (errorAngle < 2. / 180. * M_PI) {
+    // Then substract the combined required satellite rotational rates from the actual rate
+    VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
+  } else {
+    // If orientation has not been aquired yet set satellite rotational rate to zero
+    errorSatRotRate = 0;
+  }
+
+  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
+  // under 150 arcsec ??
+}
+
+void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+                                  double *refSatRate) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target rotation rate
+  //-------------------------------------------------------------------------------------
   double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
                        (timeSavedQuaternion.tv_sec +
                         timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
@@ -221,395 +508,6 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn
   savedQuaternion[3] = quatInertialTarget[3];
 }
 
-void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
-                                      acsctrl::GpsDataProcessed *gpsDataProcessed,
-                                      acsctrl::MekfData *mekfData, timeval now,
-                                      double targetQuat[4], double refSatRate[3]) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion for target pointing
-  //-------------------------------------------------------------------------------------
-  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
-  // fixed/centered frame)
-  double targetCart[3] = {0, 0, 0};
-
-  MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.targetModeControllerParameters.latitudeTgt,
-      acsParameters.targetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
-  //	Position of the satellite in the earth/fixed frame via GPS
-  double posSatE[3] = {0, 0, 0};
-  std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
-
-  //	Transformation between ECEF and IJK frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
-
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
-
-  //	Target Direction and position vector in the inertial frame
-  double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
-
-  // negative x-Axis aligned with target (Camera/E-band transmitter position)
-  double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
-  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
-
-  // Transform velocity into inertial frame
-  double velocityE[3];
-  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
-  double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
-  VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
-
-  // orbital normal vector
-  double orbitalNormalJ[3] = {0, 0, 0};
-  VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
-  VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
-
-  // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
-  double yAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
-  VectorOperations<double>::normalize(yAxis, yAxis, 3);
-
-  // z-Axis completes RHS
-  double zAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(xAxis, yAxis, zAxis);
-
-  // Complete transformation matrix
-  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
-                         {xAxis[1], yAxis[1], zAxis[1]},
-                         {xAxis[2], yAxis[2], zAxis[2]}};
-  double quatInertialTarget[4] = {0, 0, 0, 0};
-  QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
-
-  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
-  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
-
-  // Transform in system relative to satellite frame
-  double quatBJ[4] = {0, 0, 0, 0};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
-}
-
-void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                               acsctrl::SusDataProcessed *susDataProcessed,
-                               acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
-                               double targetQuat[4], double refSatRate[3]) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion for ground station pointing
-  //-------------------------------------------------------------------------------------
-  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
-  // fixed/centered frame)
-  double groundStationCart[3] = {0, 0, 0};
-
-  MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters.targetModeControllerParameters.latitudeTgt,
-      acsParameters.targetModeControllerParameters.longitudeTgt,
-      acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
-  //	Position of the satellite in the earth/fixed frame via GPS
-  double posSatE[3] = {0, 0, 0};
-  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
-  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
-  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
-                                                  sensorValues->gpsSet.altitude.value, posSatE);
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
-
-  //	Transformation between ECEF and IJK frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
-
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
-
-  //	Target Direction and position vector in the inertial frame
-  double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
-
-  // negative x-Axis aligned with target (Camera/E-band transmitter position)
-  double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
-  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
-
-  // get Sun Vector Model in ECI
-  double sunJ[3];
-  std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
-  VectorOperations<double>::normalize(sunJ, sunJ, 3);
-
-  // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
-  // z = sPerpenticular =  s - sParallel = s - (x*s)/norm(x)^2 * x
-  double xDotS = VectorOperations<double>::dot(xAxis, sunJ);
-  xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
-  double sunParallel[3], zAxis[3];
-  VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
-  VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
-  VectorOperations<double>::normalize(zAxis, zAxis, 3);
-
-  // calculate y-axis
-  double yAxis[3];
-  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
-  VectorOperations<double>::normalize(yAxis, yAxis, 3);
-
-  // Complete transformation matrix
-  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
-                         {xAxis[1], yAxis[1], zAxis[1]},
-                         {xAxis[2], yAxis[2], zAxis[2]}};
-  double quatInertialTarget[4] = {0, 0, 0, 0};
-  QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
-
-  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
-  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
-
-  // Transform in system relative to satellite frame
-  double quatBJ[4] = {0, 0, 0, 0};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
-}
-
-void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                          acsctrl::SusDataProcessed *susDataProcessed,
-                          acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
-                          double targetQuat[4], double refSatRate[3]) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion to sun
-  //-------------------------------------------------------------------------------------
-  double quatBJ[4] = {0, 0, 0, 0};
-  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  QuaternionOperations::toDcm(quatBJ, dcmBJ);
-
-  double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
-  if (susDataProcessed->sunIjkModel.isValid()) {
-    std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
-    MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
-  } else if (susDataProcessed->susVecTot.isValid()) {
-    std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
-  } else {
-    return;
-  }
-
-  //	Transformation between ECEF and IJK frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
-
-  // positive z-Axis of EIVE in direction of sun
-  double zAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(sunDirB, zAxis, 3);
-
-  // Assign helper vector (north pole inertial)
-  double helperVec[3] = {0, 0, 1};
-
-  //
-  double yAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxis, helperVec, yAxis);
-  VectorOperations<double>::normalize(yAxis, yAxis, 3);
-
-  //
-  double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(yAxis, zAxis, xAxis);
-  VectorOperations<double>::normalize(xAxis, xAxis, 3);
-
-  // Transformation matrix to Sun, no further transforamtions, reference is already
-  // the EIVE body frame
-  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
-                         {xAxis[1], yAxis[1], zAxis[1]},
-                         {xAxis[2], yAxis[2], zAxis[2]}};
-  double quatSun[4] = {0, 0, 0, 0};
-  QuaternionOperations::fromDcm(dcmTgt, quatSun);
-
-  targetQuat[0] = quatSun[0];
-  targetQuat[1] = quatSun[1];
-  targetQuat[2] = quatSun[2];
-  targetQuat[3] = quatSun[3];
-
-  //----------------------------------------------------------------------------
-  //	Calculation of reference rotation rate
-  //----------------------------------------------------------------------------
-  refSatRate[0] = 0;
-  refSatRate[1] = 0;
-  refSatRate[2] = 0;
-}
-
-void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                                      timeval now, double targetQuat[4],
-                                      double refSatRate[3]) {  // old version of Nadir Pointing
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion for Nadir pointing
-  //-------------------------------------------------------------------------------------
-  //	Position of the satellite in the earth/fixed frame via GPS
-  double posSatE[3] = {0, 0, 0};
-  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
-  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
-  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
-                                                  sensorValues->gpsSet.altitude.value, posSatE);
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
-
-  //	Transformation between ECEF and IJK frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
-
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
-
-  //	Transformation between ECEF and Body frame
-  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double quatBJ[4] = {0, 0, 0, 0};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  QuaternionOperations::toDcm(quatBJ, dcmBJ);
-  MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
-
-  //	Target Direction in the body frame
-  double targetDirB[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
-
-  //	rotation quaternion from two vectors
-  double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
-  double noramlizedTargetDirB[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
-  VectorOperations<double>::normalize(refDir, refDir, 3);
-  double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
-  double normRefDir = VectorOperations<double>::norm(refDir, 3);
-  double crossDir[3] = {0, 0, 0};
-  double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
-  VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
-  targetQuat[0] = crossDir[0];
-  targetQuat[1] = crossDir[1];
-  targetQuat[2] = crossDir[2];
-  targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
-  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
-
-  //-------------------------------------------------------------------------------------
-  //	Calculation of reference rotation rate
-  //-------------------------------------------------------------------------------------
-  refSatRate[0] = 0;
-  refSatRate[1] = 0;
-  refSatRate[2] = 0;
-}
-
-void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
-                                     acsctrl::GpsDataProcessed *gpsDataProcessed,
-                                     acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
-                                     double refSatRate[3]) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion for Nadir pointing
-  //-------------------------------------------------------------------------------------
-  //	Position of the satellite in the earth/fixed frame via GPS
-  double posSatE[3] = {0, 0, 0};
-  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
-  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
-  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
-                                                  sensorValues->gpsSet.altitude.value, posSatE);
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
-
-  //	Transformation between ECEF and IJK frame
-  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
-
-  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
-
-  //	Target Direction in the body frame
-  double targetDirJ[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
-
-  // negative x-Axis aligned with target (Camera position)
-  double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
-  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
-
-  // z-Axis parallel to long side of picture resolution
-  double zAxis[3] = {0, 0, 0}, velocityE[3];
-  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
-  double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
-  VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
-  VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
-  VectorOperations<double>::normalize(zAxis, zAxis, 3);
-
-  // y-Axis completes RHS
-  double yAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
-
-  // Complete transformation matrix
-  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
-                         {xAxis[1], yAxis[1], zAxis[1]},
-                         {xAxis[2], yAxis[2], zAxis[2]}};
-  double quatInertialTarget[4] = {0, 0, 0, 0};
-  QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
-
-  int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
-  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
-
-  // Transform in system relative to satellite frame
-  double quatBJ[4] = {0, 0, 0, 0};
-  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
-  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
-}
-
-void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
-  std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
-              4 * sizeof(double));
-  std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
-              3 * sizeof(double));
-}
-
-void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
-                          double refSatRate[3], double quatErrorComplete[4], double quatError[3],
-                          double deltaRate[3]) {
-  double satRate[3] = {0, 0, 0};
-  std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
-  VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
-  // valid checks ?
-  double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
-                               {-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]},
-                               {quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
-                               {quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
-
-  MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
-
-  if (quatErrorComplete[3] < 0) {
-    quatErrorComplete[3] *= -1;
-  }
-
-  quatError[0] = quatErrorComplete[0];
-  quatError[1] = quatErrorComplete[1];
-  quatError[2] = quatErrorComplete[2];
-
-  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
-  // under 150 arcsec ??
-}
-
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
                                                 double *rwPseudoInv) {
   bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
@@ -640,3 +538,32 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
     return returnvalue::FAILED;
   }
 }
+
+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::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
+                3 * sizeof(double));
+  } else {
+    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
+                3 * sizeof(double));
+  }
+  std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
+              3 * sizeof(double));
+}
+
+ReturnValue_t Guidance::solarArrayDeploymentComplete() {
+  if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) {
+    std::remove(SD_0_SKEWED_PTG_FILE);
+    if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE)) {
+      return returnvalue::FAILED;
+    }
+  }
+  if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {
+    std::remove(SD_1_SKEWED_PTG_FILE);
+    if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {
+      return returnvalue::FAILED;
+    }
+  }
+  return returnvalue::OK;
+}

mission/controller/acs/Guidance.h

@@ -1,10 +1,3 @@
-/*
- * Guidance.h
- *
- *  Created on: 6 Jun 2022
- *      Author: Robin Marquardt
- */
-
 #ifndef GUIDANCE_H_
 #define GUIDANCE_H_
 
@@ -20,52 +13,44 @@ class Guidance {
   virtual ~Guidance();
 
   void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
+  ReturnValue_t solarArrayDeploymentComplete();
 
   // Function to get the target quaternion and refence rotation rate from gps position and
   // position of the ground station
-  void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
-                              acsctrl::GpsDataProcessed *gpsDataProcessed,
-                              acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
-                              double refSatRate[3]);
-  void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                       acsctrl::SusDataProcessed *susDataProcessed,
-                       acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
-                       double targetQuat[4], double refSatRate[3]);
-  void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                               acsctrl::SusDataProcessed *susDataProcessed,
-                               acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
-                               double targetQuat[4], double refSatRate[3]);
+  void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
+                               double refDirB[3], double quatBI[4], double targetQuat[4],
+                               double targetSatRotRate[3]);
+  void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
+                              double targetSatRotRate[3]);
+  void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
+                       double targetSatRotRate[3]);
 
   // Function to get the target quaternion and refence rotation rate for sun pointing after ground
   // station
-  void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                  acsctrl::SusDataProcessed *susDataProcessed,
-                  acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4],
-                  double refSatRate[3]);
+  void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);
 
   // Function to get the target quaternion and refence rotation rate from gps position for Nadir
   // pointing
-  void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
-                             acsctrl::GpsDataProcessed *gpsDataProcessed,
-                             acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
-                             double refSatRate[3]);
-  void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                              timeval now, double targetQuat[4], double refSatRate[3]);
+  void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
+                                    double targetQuat[4], double refDirB[3], double refSatRate[3]);
+  void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
+                                   double targetQuat[4], double refSatRate[3]);
 
-  // Function to get the target quaternion and refence rotation rate from parameters for inertial
-  // pointing
-  void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);
+  // @note: Calculates the error quaternion between the current orientation and the target
+  // quaternion, considering a reference quaternion. Additionally the difference between the  actual
+  // and a desired satellite rotational rate is calculated, again considering a reference rotational
+  // rate. Lastly gives back the error angle of the error quaternion.
+  void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
+                  double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
+                  double errorQuat[4], double errorSatRotRate[3], double errorAngle);
+  void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
+                  double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
+                  double errorAngle);
 
-  // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
-  // desired
-  void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
-                  double refSatRate[3], double quatErrorComplete[4], double quatError[3],
-                  double deltaRate[3]);
+  void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+                          double *targetSatRotRate);
 
-  void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
-                       double *refSatRate);
-
-  //	@note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
+  // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
   // reation wheel 	maybe can be done in "commanding.h"
   ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
 

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -14,7 +14,7 @@
 
 /*Initialisation of values for parameters in constructor*/
 MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
-    : initialQuaternion{0.5, 0.5, 0.5, 0.5},
+    : initialQuaternion{0, 0, 0, 1},
       initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                               {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} {
   loadAcsParameters(acsParameters_);
@@ -27,12 +27,10 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
   kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
 }
 
-void MultiplicativeKalmanFilter::reset() {}
-
-void MultiplicativeKalmanFilter::init(
+ReturnValue_t MultiplicativeKalmanFilter::init(
     const double *magneticField_, const bool validMagField_, const double *sunDir_,
     const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-    const bool validMagModel) {  // valids for "model measurements"?
+    const bool validMagModel, acsctrl::MekfData *mekfData) {  // valids for "model measurements"?
   // check for valid mag/sun
   if (validMagField_ && validSS && validSSModel && validMagModel) {
     validInit = true;
@@ -190,9 +188,13 @@ void MultiplicativeKalmanFilter::init(
     initialCovarianceMatrix[5][3] = initGyroCov[2][0];
     initialCovarianceMatrix[5][4] = initGyroCov[2][1];
     initialCovarianceMatrix[5][5] = initGyroCov[2][2];
+    updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
+    return MEKF_INITIALIZED;
   } else {
     // no initialisation possible, no valid measurements
     validInit = false;
+    updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
+    return MEKF_UNINITIALIZED;
   }
 }
 
@@ -208,33 +210,13 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   // Check for GYR Measurements
   int MDF = 0;  // Matrix Dimension Factor
   if (!validGYRs_) {
-    {
-      PoolReadGuard pg(mekfData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-        double zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
-        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
-        mekfData->setValidity(false, true);
-      }
-    }
-    validMekf = false;
-    return KALMAN_NO_GYR_MEAS;
+    updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
+    return MEKF_NO_GYR_DATA;
   }
   // Check for Model Calculations
   else if (!validSSModel || !validMagModel) {
-    {
-      PoolReadGuard pg(mekfData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-        double zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
-        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
-        mekfData->setValidity(false, true);
-      }
-    }
-    validMekf = false;
-    return KALMAN_NO_MODEL;
+    updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
+    return MEKF_NO_MODEL_VECTORS;
   }
   // Check Measurements available from SS, MAG, STR
   if (validSS && validMagField_ && validSTR_) {
@@ -260,17 +242,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
     MDF = 3;
   } else {
     sensorsAvail = 8;  // no measurements
-    validMekf = false;
-    {
-      PoolReadGuard pg(mekfData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-        double zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
-        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
-        mekfData->setValidity(false, true);
-      }
-    }
+    updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA);
     return returnvalue::FAILED;
   }
 
@@ -881,18 +853,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   double invResidualCov[MDF][MDF] = {{0}};
   int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
   if (inversionFailed) {
-    {
-      PoolReadGuard pg(mekfData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-        double zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
-        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
-        mekfData->setValidity(false, true);
-      }
-    }
-    validMekf = false;
-    return KALMAN_INVERSION_FAILED;  // RETURN VALUE ? -- Like: Kalman Inversion Failed
+    updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
+    return MEKF_COVARIANCE_INVERSION_FAILED;  // RETURN VALUE ? -- Like: Kalman Inversion Failed
   }
 
   // [K = P * H' / (H * P * H' + R)]
@@ -1121,20 +1083,47 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
 
   MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
-  validMekf = true;
 
-  // Discrete Time Step
+  updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
+  return MEKF_RUNNING;
+}
 
-  // Check for new data in measurement -> SensorProcessing ?
+ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
+  double resetQuaternion[4] = {0, 0, 0, 1};
+  double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
+                                        {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
+  std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
+  std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
+  updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
+  return MEKF_UNINITIALIZED;
+}
 
+void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
+                                                          MekfStatus mekfStatus) {
   {
     PoolReadGuard pg(mekfData);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double));
-      std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double));
+      double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
+      double zeroVec[3] = {0.0, 0.0, 0.0};
+      std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
+      mekfData->quatMekf.setValid(false);
+      std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
+      mekfData->satRotRateMekf.setValid(false);
+      mekfData->mekfStatus = mekfStatus;
+      mekfData->setValidity(true, false);
+    }
+  }
+}
+
+void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus,
+                                               double quat[4], double satRotRate[3]) {
+  {
+    PoolReadGuard pg(mekfData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double));
+      std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double));
+      mekfData->mekfStatus = mekfStatus;
       mekfData->setValidity(true, true);
     }
   }
-
-  return returnvalue::OK;
 }

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -1,28 +1,20 @@
-/*
- * MultiplicativeKalmanFilter.h
- *
- *  Created on: 4 Feb 2022
- *      Author: Robin Marquardt
- *
- *      @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
- *      means of the spacecraft attitude sensors
- *
- *      @note: A description of the used algorithms can be found in the bachelor thesis of Robin
- * Marquardt
- *      https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
- */
-
 #ifndef MULTIPLICATIVEKALMANFILTER_H_
 #define MULTIPLICATIVEKALMANFILTER_H_
 
-#include <stdint.h>  //uint8_t
-#include <time.h>    /*purpose, timeval ?*/
+#include <stdint.h>
 
 #include "../controllerdefinitions/AcsCtrlDefinitions.h"
 #include "AcsParameters.h"
 #include "eive/resultClassIds.h"
 
 class MultiplicativeKalmanFilter {
+  /* @brief: This class handles the calculation of an estimated quaternion and the gyro bias by
+   *           means of the spacecraft attitude sensors
+   *
+   * @note: A description of the used algorithms can be found in the bachelor thesis of Robin
+   *        Marquardt
+   *        https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=500811
+   */
  public:
   /* @brief: Constructor
    * @param: acsParameters_   Pointer to object which defines the ACS configuration parameters
@@ -30,18 +22,19 @@ class MultiplicativeKalmanFilter {
   MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
   virtual ~MultiplicativeKalmanFilter();
 
-  void reset();  // NOT YET DEFINED - should only reset all mekf variables
+  ReturnValue_t reset(acsctrl::MekfData *mekfData);
 
   /* @brief: init() - This function initializes the Kalman Filter and will provide the first
    * quaternion through the QUEST algorithm
    * @param:  magneticField_  magnetic field vector in the body frame
-   * 			sunDir_ 		sun direction vector in the body frame
-   * 			sunDirJ 		sun direction vector in the ECI frame
-   * 			magFieldJ 		magnetic field vector in the ECI frame
+   * 		  sunDir_ 		  sun direction vector in the body frame
+   * 		  sunDirJ 		  sun direction vector in the ECI frame
+   * 		  magFieldJ 	  magnetic field vector in the ECI frame
    */
-  void init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
-            const bool validSS, const double *sunDirJ, const bool validSSModel,
-            const double *magFieldJ, const bool validMagModel);
+  ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
+                     const bool validSS, const double *sunDirJ, const bool validSSModel,
+                     const double *magFieldJ, const bool validMagModel,
+                     acsctrl::MekfData *mekfData);
 
   /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
    * for the current step after the initalization
@@ -63,11 +56,26 @@ class MultiplicativeKalmanFilter {
                         const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
                         const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
 
+  enum MekfStatus : uint8_t {
+    UNINITIALIZED = 0,
+    NO_GYR_DATA = 1,
+    NO_MODEL_VECTORS = 2,
+    NO_SUS_MGM_STR_DATA = 3,
+    COVARIANCE_INVERSION_FAILED = 4,
+    INITIALIZED = 10,
+    RUNNING = 11,
+  };
+
   // resetting Mekf
-  static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
-  static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1);
-  static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2);
-  static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3);
+  static constexpr uint8_t IF_MEKF_ID = CLASS_ID::ACS_MEKF;
+  static constexpr ReturnValue_t MEKF_UNINITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 2);
+  static constexpr ReturnValue_t MEKF_NO_GYR_DATA = returnvalue::makeCode(IF_MEKF_ID, 3);
+  static constexpr ReturnValue_t MEKF_NO_MODEL_VECTORS = returnvalue::makeCode(IF_MEKF_ID, 4);
+  static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
+  static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
+      returnvalue::makeCode(IF_MEKF_ID, 6);
+  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
+  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
 
  private:
   /*Parameters*/
@@ -80,16 +88,17 @@ class MultiplicativeKalmanFilter {
   double initialQuaternion[4];          /*after reset?QUEST*/
   double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
   double propagatedQuaternion[4];       /*Filter Quaternion for next step*/
-  bool validMekf;
   uint8_t sensorsAvail;
 
   /*Outputs*/
   double quatBJ[4];  /* Output Quaternion */
   double biasGYR[3]; /*Between measured and estimated sat Rate*/
                      /*Parameter INIT*/
-  // double alpha, gamma, beta;
   /*Functions*/
   void loadAcsParameters(AcsParameters *acsParameters_);
+  void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
+  void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
+                     double satRotRate[3]);
 };
 
 #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

mission/controller/acs/Navigation.cpp

@@ -1,10 +1,3 @@
-/*
- * Navigation.cpp
- *
- *  Created on: 23 May 2022
- *      Author: Robin Marquardt
- */
-
 #include "Navigation.h"
 
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
@@ -21,37 +14,36 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt
 
 Navigation::~Navigation() {}
 
-void Navigation::useMekf(ACS::SensorValues *sensorValues,
-                         acsctrl::GyrDataProcessed *gyrDataProcessed,
-                         acsctrl::MgmDataProcessed *mgmDataProcessed,
-                         acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
-                         ReturnValue_t *mekfValid) {
-  double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
+ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
+                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
+                                  acsctrl::SusDataProcessed *susDataProcessed,
+                                  acsctrl::MekfData *mekfData) {
+  double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
-  bool quatJBValid = sensorValues->strSet.caliQx.isValid() &&
+  bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
                      sensorValues->strSet.caliQy.isValid() &&
                      sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
 
-  if (kalmanInit) {
-    *mekfValid = multiplicativeKalmanFilter.mekfEst(
-        quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value,
+  if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
+    mekfStatus = multiplicativeKalmanFilter.init(
+        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
+        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
+        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
+        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
+    return mekfStatus;
+  } else {
+    mekfStatus = multiplicativeKalmanFilter.mekfEst(
+        quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
         gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
         mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
         susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
         susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
-        mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime,
-        mekfData);  // VALIDS FOR QUAT AND RATE ??
-  } else {
-    multiplicativeKalmanFilter.init(
-        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
-        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
-        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
-        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid());
-    kalmanInit = true;
-    *mekfValid = returnvalue::OK;
-
-    //		Maybe we need feedback from kalmanfilter to identify if there was a problem with the
-    // init of kalman filter 		where does this class know from that kalman filter was not
-    // initialized ?
+        mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
+    return mekfStatus;
   }
 }
+
+void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
+  mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
+}

mission/controller/acs/Navigation.h

@@ -1,10 +1,3 @@
-/*
- * Navigation.h
- *
- *  Created on: 19 Apr 2022
- *      Author: Robin Marquardt
- */
-
 #ifndef NAVIGATION_H_
 #define NAVIGATION_H_
 
@@ -16,20 +9,20 @@
 
 class Navigation {
  public:
-  Navigation(AcsParameters *acsParameters_);  // Input mode ?
+  Navigation(AcsParameters *acsParameters_);
   virtual ~Navigation();
 
-  void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed,
-               acsctrl::MgmDataProcessed *mgmDataProcessed,
-               acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
-               ReturnValue_t *mekfValid);
-  void processSensorData();
+  ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
+                        acsctrl::GyrDataProcessed *gyrDataProcessed,
+                        acsctrl::MgmDataProcessed *mgmDataProcessed,
+                        acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData);
+  void resetMekf(acsctrl::MekfData *mekfData);
 
  protected:
  private:
   MultiplicativeKalmanFilter multiplicativeKalmanFilter;
   AcsParameters acsParameters;
-  bool kalmanInit = false;
+  ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
 };
 
 #endif /* ACS_NAVIGATION_H_ */

mission/controller/acs/SensorProcessing.cpp

@@ -1,10 +1,3 @@
-/*
- * SensorProcessing.cpp
- *
- *  Created on: 7 Mar 2022
- *      Author: Robin Marquardt
- */
-
 #include "SensorProcessing.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>

mission/controller/acs/SensorProcessing.h

@@ -1,7 +1,3 @@
-/*******************************
- *	EIVE Flight Software
- * (c) 2022 IRS, Uni Stuttgart
- *******************************/
 #ifndef SENSORPROCESSING_H_
 #define SENSORPROCESSING_H_
 

mission/controller/acs/SensorValues.cpp

@@ -1,9 +1,3 @@
-/*
- * SensorValues.cpp
- *
- *  Created on: 30 Mar 2022
- *      Author: rooob
- */
 #include "SensorValues.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>

mission/controller/acs/SensorValues.h

@@ -1,6 +1,7 @@
 #ifndef SENSORVALUES_H_
 #define SENSORVALUES_H_
 
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 
 #include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
@@ -10,7 +11,6 @@
 #include "mission/devices/devicedefinitions/GPSDefinitions.h"
 #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
-#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
 
 namespace ACS {
 
@@ -35,7 +35,8 @@ class SensorValues {
       MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
   RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
       RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
-  IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_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),

mission/controller/acs/SusConverter.cpp

@@ -1,16 +1,9 @@
-/*
- * SusConverter.cpp
- *
- *  Created on: 17.01.2022
- *      Author: Timon Schwarz
- */
-
 #include "SusConverter.h"
 
 #include <fsfw/datapoollocal/LocalPoolVariable.h>
 #include <fsfw/datapoollocal/LocalPoolVector.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <math.h>  //for atan2
+#include <math.h>
 
 #include <iostream>
 
@@ -63,8 +56,7 @@ void SusConverter::calcAngle(const uint16_t susChannel[6]) {
 }
 
 void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
-  uint8_t index;
-  float k, l;
+  uint8_t index, k, l;
 
   // while loop iterates above all calibration cells to use the different calibration functions in
   // each cell
@@ -75,10 +67,10 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB
     while (l < 3) {
       l++;
       // if-condition to check in which cell the data point has to be
-      if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) &&
-           alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) &&
-          (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) &&
-           alphaBetaRaw[1] < ((completeCellWidth * (l / 3)) - halfCellWidth))) {
+      if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) &&
+           alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) &&
+          (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) &&
+           alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) {
         index = (3 * (k - 1) + l) - 1;  // calculate the index of the datapoint for the right cell
         alphaBetaCalibrated[0] =
             coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] +

mission/controller/acs/SusConverter.h

@@ -1,10 +1,3 @@
-/*
- * SusConverter.h
- *
- *  Created on: Sep 22, 2022
- *      Author: marius
- */
-
 #ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
 #define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
 
@@ -28,8 +21,6 @@ class SusConverter {
 
  private:
   float alphaBetaRaw[2];          //[°]
-                                  //  float coeffAlpha[9][10];
-                                  //  float coeffBeta[9][10];
   float alphaBetaCalibrated[2];   //[°]
   float sunVectorSensorFrame[3];  //[-]
 

mission/controller/acs/control/PtgCtrl.cpp

@@ -27,7 +27,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
 }
 
 void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
-                     const double *qError, const double *deltaRate, const double *rwPseudoInv,
+                     const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
                      double *torqueRws) {
   //------------------------------------------------------------------------------------------------
   // Compute gain matrix K and P matrix
@@ -37,6 +37,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   double qErrorMin = pointingLawParameters->qiMin;
   double omMax = pointingLawParameters->omMax;
 
+  double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
+
   double cInt = 2 * om * zeta;
   double kInt = 2 * pow(om, 2);
 

mission/controller/acs/util/MathOperations.h

@@ -300,6 +300,7 @@ class MathOperations {
   }
 
   static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
+    /* do not use this. takes 300ms */
     float det = 0;
     T1 matrix[size][size], submatrix[size - 1][size - 1];
     for (uint8_t row = 0; row < size; row++) {
@@ -329,9 +330,7 @@ class MathOperations {
   }
 
   static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
-    if (MathOperations<T1>::matrixDeterminant(inputMatrix, size) == 0) {
-      return 1;  // Matrix is singular and not invertible
-    }
+    // Stopwatch stopwatch;
     T1 matrix[size][size], identity[size][size];
     // reformat array to matrix
     for (uint8_t row = 0; row < size; row++) {
@@ -346,7 +345,6 @@ class MathOperations {
     }
     // gauss-jordan algo
     // sort matrix such as no diag entry shall be 0
-    // should not be needed as such a matrix has a det=0
     for (uint8_t row = 0; row < size; row++) {
       if (matrix[row][row] == 0.0) {
         bool swaped = false;

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -91,10 +91,12 @@ enum PoolIds : lp_id_t {
   // MEKF
   SAT_ROT_RATE_MEKF,
   QUAT_MEKF,
+  MEKF_STATUS,
   // Ctrl Values
   TGT_QUAT,
   ERROR_QUAT,
   ERROR_ANG,
+  TGT_ROT_RATE,
   // Actuator Cmd
   RW_TARGET_TORQUE,
   RW_TARGET_SPEED,
@@ -108,8 +110,8 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
 static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
 static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
 static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
-static constexpr uint8_t MEKF_SET_ENTRIES = 2;
-static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
+static constexpr uint8_t MEKF_SET_ENTRIES = 3;
+static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
 
 /**
@@ -238,6 +240,7 @@ class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
 
   lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
   lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);
+  lp_var_t<uint8_t> mekfStatus = lp_var_t<uint8_t>(sid.objectId, MEKF_STATUS, this);
 
  private:
 };
@@ -249,6 +252,7 @@ class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
   lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
   lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
   lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
+  lp_vec_t<double, 3> tgtRotRate = lp_vec_t<double, 3>(sid.objectId, TGT_ROT_RATE, this);
 
  private:
 };

mission/controller/controllerdefinitions/ThermalControllerDefinitions.h

@@ -4,13 +4,16 @@
 #include <fsfw/datapoollocal/LocalPoolVariable.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
 
-namespace thermalControllerDefinitions {
+#include "devices/heaterSwitcherList.h"
 
-enum SetIds : uint32_t {
-  SENSOR_TEMPERATURES,
-  DEVICE_TEMPERATURES,
-  SUS_TEMPERATURES,
-  COMPONENT_TEMPERATURES
+namespace tcsCtrl {
+
+enum SetId : uint32_t {
+  SENSOR_TEMPERATURES = 0,
+  DEVICE_TEMPERATURES = 1,
+  SUS_TEMPERATURES = 2,
+  COMPONENT_TEMPERATURES = 3,
+  HEATER_SET = 4,
 };
 
 enum PoolIds : lp_id_t {
@@ -75,7 +78,10 @@ enum PoolIds : lp_id_t {
   TEMP_GYRO_3_SIDE_B,
   TEMP_MGM_0_SIDE_A,
   TEMP_MGM_2_SIDE_B,
-  TEMP_ADC_PAYLOAD_PCDU
+  TEMP_ADC_PAYLOAD_PCDU,
+
+  HEATER_SWITCH_LIST,
+  HEATER_CURRENT
 };
 
 static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 25;
@@ -202,6 +208,17 @@ class SusTemperatures : public StaticLocalDataSet<ENTRIES_SUS_TEMPERATURE_SET> {
       lp_var_t<float>(sid.objectId, PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, this);
 };
 
-}  // namespace thermalControllerDefinitions
+class HeaterInfo : public StaticLocalDataSet<3> {
+ public:
+  HeaterInfo(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, HEATER_SET) {}
+  HeaterInfo(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, HEATER_SET)) {}
+
+  lp_vec_t<uint8_t, heater::NUMBER_OF_SWITCHES> heaterSwitchState =
+      lp_vec_t<uint8_t, heater::NUMBER_OF_SWITCHES>(sid.objectId, PoolIds::HEATER_SWITCH_LIST,
+                                                    this);
+  lp_var_t<int16_t> heaterCurrent = lp_var_t<int16_t>(sid.objectId, PoolIds::HEATER_CURRENT, this);
+};
+
+}  // namespace tcsCtrl
 
 #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_THERMALCONTROLLERDEFINITIONS_H_ */

mission/core/CMakeLists.txt

@@ -1 +1,2 @@
-target_sources(${LIB_EIVE_MISSION} PRIVATE GenericFactory.cpp scheduling.cpp)
+target_sources(${LIB_EIVE_MISSION} PRIVATE GenericFactory.cpp scheduling.cpp
+                                           pollingSeqTables.cpp)

mission/core/GenericFactory.cpp

@@ -20,6 +20,7 @@
 #include <fsfw/pus/Service8FunctionManagement.h>
 #include <fsfw/pus/Service9TimeManagement.h>
 #include <fsfw/storagemanager/PoolManager.h>
+#include <fsfw/subsystem/SubsystemBase.h>
 #include <fsfw/tcdistribution/CcsdsDistributor.h>
 #include <fsfw/tcdistribution/PusDistributor.h>
 #include <fsfw/timemanager/CdsShortTimeStamper.h>
@@ -57,6 +58,7 @@
 // TCP server includes
 #include "fsfw/osal/common/TcpTmTcBridge.h"
 #include "fsfw/osal/common/TcpTmTcServer.h"
+#include "mission/tmtc/Service15TmStorage.h"
 #endif
 #endif
 
@@ -82,7 +84,7 @@ EiveFaultHandler EIVE_FAULT_HANDLER;
 }  // namespace cfdp
 
 void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFunnel** pusFunnel,
-                                          CfdpTmFunnel** cfdpFunnel) {
+                                          CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan) {
   // Framework objects
   new EventManager(objects::EVENT_MANAGER);
   auto healthTable = new HealthTable(objects::HEALTH_TABLE);
@@ -94,6 +96,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
   auto* timeStamper = new CdsShortTimeStamper(objects::TIME_STAMPER);
   StorageManagerIF* tcStore;
   StorageManagerIF* tmStore;
+  StorageManagerIF* ipcStore;
   {
     PoolManager::LocalPoolConfig poolCfg = {{250, 16},  {250, 32},   {250, 64},
                                             {150, 128}, {120, 1024}, {120, 2048}};
@@ -108,8 +111,8 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
 
   {
     PoolManager::LocalPoolConfig poolCfg = {{300, 16},  {200, 32}, {150, 64},  {150, 128},
-                                            {100, 256}, {50, 512}, {50, 1024}, {50, 2048}};
-    new PoolManager(objects::IPC_STORE, poolCfg);
+                                            {100, 256}, {50, 512}, {50, 1024}, {10, 2048}};
+    ipcStore = new PoolManager(objects::IPC_STORE, poolCfg);
   }
 
 #if OBSW_ADD_TCPIP_SERVERS == 1
@@ -136,9 +139,11 @@ 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);
 
-  *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmStore, 50);
-  *pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, *timeStamper, *tmStore,
-                               config::MAX_PUS_FUNNEL_QUEUE_DEPTH);
+  PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, *tmStore, *ipcStore, 50);
+  *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, config::EIVE_CFDP_APID);
+  PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, *tmStore, *ipcStore,
+                                      config::MAX_PUS_FUNNEL_QUEUE_DEPTH);
+  *pusFunnel = new PusTmFunnel(pusFunnelCfg, *timeStamper, sdcMan);
 #if OBSW_ADD_TCPIP_SERVERS == 1
 #if OBSW_ADD_TMTC_UDP_SERVER == 1
   (*cfdpFunnel)->addDestination("UDP Server", *udpBridge, 0);
@@ -170,6 +175,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
   new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(
       PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11),
       ccsdsDistrib);
+  new Service15TmStorage(objects::PUS_SERVICE_15_TM_STORAGE, config::EIVE_PUS_APID, 10);
   new Service17Test(
       PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17));
   new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, config::EIVE_PUS_APID,
@@ -219,6 +225,7 @@ void ObjectFactory::createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF&
   }});
   heaterHandler = new HeaterHandler(objects::HEATER_HANDLER, &gpioIF, helper, &pwrSwitcher,
                                     pcdu::Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V);
+  heaterHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM);
 }
 
 void ObjectFactory::createThermalController(HeaterHandler& heaterHandler) {
@@ -230,7 +237,7 @@ void ObjectFactory::createRwAssy(PowerSwitchIF& pwrSwitcher, power::Switch_t the
                                  std::array<object_id_t, 4> rwIds) {
   RwHelper rwHelper(rwIds);
   auto* rwAss = new RwAssembly(objects::RW_ASS, &pwrSwitcher, theSwitch, rwHelper);
-  for (uint8_t idx = 0; idx < rwIds.size(); idx++) {
+  for (size_t idx = 0; idx < rwIds.size(); idx++) {
     ReturnValue_t result = rws[idx]->connectModeTreeParent(*rwAss);
     if (result != returnvalue::OK) {
       sif::error << "Connecting RW " << static_cast<int>(idx) << " to RW assembly failed"
@@ -249,7 +256,7 @@ void ObjectFactory::createSusAssy(PowerSwitchIF& pwrSwitcher,
       objects::SUS_6_R_LOC_XFYBZM_PT_XF,  objects::SUS_7_R_LOC_XBYBZM_PT_XB,
       objects::SUS_8_R_LOC_XBYBZB_PT_YB,  objects::SUS_9_R_LOC_XBYBZB_PT_YF,
       objects::SUS_10_N_LOC_XMYBZF_PT_ZF, objects::SUS_11_R_LOC_XBYMZB_PT_ZB};
-  SusAssHelper susAssHelper = SusAssHelper(susIds);
+  auto susAssHelper = SusAssHelper(susIds);
   auto susAss = new SusAssembly(objects::SUS_BOARD_ASS, &pwrSwitcher, susAssHelper);
   for (auto& sus : suses) {
     if (sus != nullptr) {
@@ -285,8 +292,8 @@ void ObjectFactory::createAcsBoardAssy(PowerSwitchIF& pwrSwitcher,
 
 TcsBoardAssembly* ObjectFactory::createTcsBoardAssy(PowerSwitchIF& pwrSwitcher) {
   TcsBoardHelper helper(RTD_INFOS);
-  TcsBoardAssembly* tcsBoardAss = new TcsBoardAssembly(
-      objects::TCS_BOARD_ASS, &pwrSwitcher, pcdu::Switches::PDU1_CH0_TCS_BOARD_3V3, helper);
+  auto* tcsBoardAss = new TcsBoardAssembly(objects::TCS_BOARD_ASS, &pwrSwitcher,
+                                           pcdu::Switches::PDU1_CH0_TCS_BOARD_3V3, helper);
   tcsBoardAss->connectModeTreeParent(satsystem::tcs::SUBSYSTEM);
   return tcsBoardAss;
 }

mission/core/GenericFactory.h

@@ -2,6 +2,7 @@
 #define MISSION_CORE_GENERICFACTORY_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
+#include <mission/memory/SdCardMountedIF.h>
 
 #include "fsfw/objectmanager/SystemObjectIF.h"
 #include "fsfw/power/PowerSwitchIF.h"
@@ -37,7 +38,7 @@ const std::array<std::pair<object_id_t, std::string>, EiveMax31855::NUM_RTDS> RT
 namespace ObjectFactory {
 
 void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel,
-                           CfdpTmFunnel** cfdpFunnel);
+                           CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan);
 void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher,
                                    HeaterHandler*& heaterHandler);
 

mission/core/pollingSeqTables.cpp

@@ -1,9 +1,10 @@
-#include "pollingSequenceFactory.h"
+#include "pollingSeqTables.h"
 
 #include <fsfw/devicehandlers/DeviceHandlerIF.h>
 #include <fsfw/objectmanager/ObjectManagerIF.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
 #include <fsfw/tasks/FixedTimeslotTaskIF.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 
 #include "OBSWConfig.h"
 #include "eive/definitions.h"
@@ -60,8 +61,8 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
                         DeviceHandlerIF::PERFORM_OPERATION);
   thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
   thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.25, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.25, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::GET_READ);
 #endif
   // These are actually part of another bus, but this works, so keep it like this for now
 #if OBSW_ADD_TMP_DEVICES == 1
@@ -585,102 +586,80 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
   if (cfg.scheduleImtq) {
     // This is the MTM measurement cycle
     thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::PERFORM_OPERATION);
+                          imtq::ComStep::DHB_OP);
     thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
+                          imtq::ComStep::START_MEASURE_SEND);
     thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+                          imtq::ComStep::START_MEASURE_GET);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                          imtq::ComStep::READ_MEASURE_SEND);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                          imtq::ComStep::READ_MEASURE_GET);
   }
 
-  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_3_PERIOD, 0);
+  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_4_PERIOD, 0);
 
   if (cfg.scheduleImtq) {
     // This is the torquing cycle.
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
+                          imtq::ComStep::START_ACTUATE_SEND);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
+                          imtq::ComStep::START_ACTUATE_GET);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
+                          imtq::ComStep::READ_ACTUATE_SEND);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::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_4_PERIOD,
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
 
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-
     thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
+                          DeviceHandlerIF::SEND_WRITE);
     thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
+                          DeviceHandlerIF::SEND_WRITE);
     thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
+                          DeviceHandlerIF::SEND_WRITE);
     thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
+                          DeviceHandlerIF::SEND_WRITE);
 
     thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+                          DeviceHandlerIF::GET_WRITE);
     thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+                          DeviceHandlerIF::GET_WRITE);
     thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+                          DeviceHandlerIF::GET_WRITE);
     thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
+                          DeviceHandlerIF::GET_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::GET_READ);
   }
 
-  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0);
+  thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::acs::SCHED_BLOCK_RTD_PERIOD, 0);
 
   return returnvalue::OK;
 }

mission/devices/GyroADIS1650XHandler.cpp

@@ -385,6 +385,7 @@ ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool
 
   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(

mission/devices/GyroADIS1650XHandler.h

@@ -63,6 +63,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
   InternalState internalState = InternalState::STARTUP;
   bool commandExecuted = false;
 
+  PoolEntry<uint16_t> rangMdl = PoolEntry<uint16_t>();
+
   void prepareReadCommand(uint8_t *regList, size_t len);
 
   BurstModes getBurstMode();

mission/devices/HeaterHandler.cpp

@@ -8,11 +8,13 @@
 #include <stdexcept>
 
 #include "devices/powerSwitcherList.h"
+#include "fsfw/subsystem/helper.h"
 
 HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, HeaterHelper helper,
                              PowerSwitchIF* mainLineSwitcher_, power::Switch_t mainLineSwitch_)
     : SystemObject(setObjectId_),
       helper(helper),
+      modeHelper(this),
       gpioInterface(gpioInterface_),
       mainLineSwitcher(mainLineSwitcher_),
       mainLineSwitch(mainLineSwitch_),
@@ -28,8 +30,8 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, H
   if (mainLineSwitcher == nullptr) {
     throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
   }
-  heaterMutex = MutexFactory::instance()->createMutex();
-  if (heaterMutex == nullptr) {
+  heaterHealthAndStateMutex = MutexFactory::instance()->createMutex();
+  if (heaterHealthAndStateMutex == nullptr) {
     throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
   }
   auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
@@ -46,6 +48,13 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
       heater.first->performOperation(0);
     }
     handleSwitchHandling();
+    if (waitForSwitchOff) {
+      if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
+        waitForSwitchOff = false;
+        mode = MODE_OFF;
+        modeHelper.modeChanged(mode, submode);
+      }
+    }
   } catch (const std::out_of_range& e) {
     sif::warning << "HeaterHandler::performOperation: "
                     "Out of range error | "
@@ -76,6 +85,10 @@ ReturnValue_t HeaterHandler::initialize() {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
 
+  result = modeHelper.initialize();
+  if (result != returnvalue::OK) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
   return returnvalue::OK;
 }
 
@@ -95,11 +108,16 @@ void HeaterHandler::readCommandQueue() {
       break;
     } else if (result != returnvalue::OK) {
       sif::warning << "HeaterHandler::readCommandQueue: Message reception error" << std::endl;
+      break;
     }
     result = actionHelper.handleActionMessage(&command);
     if (result == returnvalue::OK) {
       continue;
     }
+    result = modeHelper.handleModeCommand(&command);
+    if (result == returnvalue::OK) {
+      continue;
+    }
   } while (result == returnvalue::OK);
 }
 
@@ -124,7 +142,11 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
   auto action = static_cast<SwitchAction>(data[1]);
   // Always accepts OFF commands
   if (action == SwitchAction::SET_SWITCH_ON) {
-    HasHealthIF::HealthState health = heater.healthDevice->getHealth();
+    HasHealthIF::HealthState health;
+    {
+      MutexGuard mg(heaterHealthAndStateMutex);
+      health = heater.healthDevice->getHealth();
+    }
     if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
         health == HasHealthIF::NEEDS_RECOVERY) {
       return HasHealthIF::OBJECT_NOT_HEALTHY;
@@ -218,6 +240,9 @@ void HeaterHandler::handleSwitchHandling() {
 void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
   ReturnValue_t result = returnvalue::OK;
   auto& heater = heaterVec.at(heaterIdx);
+  if (waitForSwitchOff) {
+    waitForSwitchOff = false;
+  }
   /* Check if command waits for main switch being set on and whether the timeout has expired */
   if (heater.waitMainSwitchOn && heater.mainSwitchCountdown.hasTimedOut()) {
     // TODO - This requires the initiation of an FDIR procedure
@@ -244,11 +269,16 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
         triggerEvent(GPIO_PULL_HIGH_FAILED, result);
       } else {
         triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
-        heater.switchState = ON;
+        {
+          MutexGuard mg(heaterHealthAndStateMutex);
+          heater.switchState = ON;
+        }
       }
     } else {
       triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
     }
+    mode = HasModesIF::MODE_ON;
+    modeHelper.modeChanged(mode, submode);
     // There is no need to send action finish replies if the sender was the
     // HeaterHandler itself
     if (heater.replyQueue != commandQueue->getId()) {
@@ -289,15 +319,15 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
                  << " low" << std::endl;
       triggerEvent(GPIO_PULL_LOW_FAILED, result);
     } else {
-      result = heaterMutex->lockMutex();
-      heater.switchState = OFF;
-      if (result == returnvalue::OK) {
-        heaterMutex->unlockMutex();
+      {
+        MutexGuard mg(heaterHealthAndStateMutex);
+        heater.switchState = OFF;
       }
       triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
       // When all switches are off, also main line switch will be turned off
       if (allSwitchesOff()) {
         mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+        waitForSwitchOff = true;
       }
     }
   } else {
@@ -316,7 +346,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
 }
 
 HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const {
-  MutexGuard mg(heaterMutex);
+  MutexGuard mg(heaterHealthAndStateMutex);
   return heaterVec.at(switchNr).switchState;
 }
 
@@ -329,9 +359,57 @@ ReturnValue_t HeaterHandler::switchHeater(heater::Switchers heater, SwitchState
   return returnvalue::FAILED;
 }
 
+void HeaterHandler::announceMode(bool recursive) {
+  triggerEvent(MODE_INFO, mode, submode);
+
+  std::array<SwitchState, 8> states;
+  getAllSwitchStates(states);
+  for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+    if (states[idx] == ON) {
+      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_ON, 0);
+    } else {
+      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_OFF,
+                                   0);
+    }
+  }
+}
+
+void HeaterHandler::getMode(Mode_t* mode, Submode_t* submode) {
+  if (!mode || !submode) {
+    return;
+  }
+  *mode = this->mode;
+  *submode = this->submode;
+}
+
+const HasHealthIF* HeaterHandler::getOptHealthIF() const { return nullptr; }
+
+const HasModesIF& HeaterHandler::getModeIF() const { return *this; }
+
+ReturnValue_t HeaterHandler::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
+}
+
+ModeTreeChildIF& HeaterHandler::getModeTreeChildIF() { return *this; }
+
+object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectId(); }
+
+ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& statesBuf) {
+  {
+    MutexGuard mg(heaterHealthAndStateMutex);
+    if (mg.getLockResult() != returnvalue::OK) {
+      return returnvalue::FAILED;
+    }
+    for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+      statesBuf[idx] = heaterVec[idx].switchState;
+    }
+  }
+  return returnvalue::OK;
+}
+
 bool HeaterHandler::allSwitchesOff() {
   bool allSwitchesOrd = false;
-  MutexGuard mg(heaterMutex);
+  MutexGuard mg(heaterHealthAndStateMutex);
   /* 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;
@@ -364,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(heaterMutex);
+    MutexGuard mg(heaterHealthAndStateMutex);
     return healthDev->getHealth();
   }
   return HasHealthIF::HealthState::FAULTY;

mission/devices/HeaterHandler.h

@@ -10,6 +10,8 @@
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/power/PowerSwitchIF.h>
 #include <fsfw/returnvalues/returnvalue.h>
+#include <fsfw/subsystem/ModeTreeChildIF.h>
+#include <fsfw/subsystem/ModeTreeConnectionIF.h>
 #include <fsfw/tasks/ExecutableObjectIF.h>
 #include <fsfw/timemanager/Countdown.h>
 #include <fsfw_hal/common/gpio/GpioIF.h>
@@ -40,6 +42,9 @@ struct HeaterHelper {
  */
 class HeaterHandler : public ExecutableObjectIF,
                       public PowerSwitchIF,
+                      public HasModesIF,
+                      public ModeTreeChildIF,
+                      public ModeTreeConnectionIF,
                       public SystemObject,
                       public HasActionsIF {
   friend class ThermalController;
@@ -54,6 +59,7 @@ class HeaterHandler : public ExecutableObjectIF,
   static const ReturnValue_t COMMAND_ALREADY_WAITING = MAKE_RETURN_CODE(0xA5);
 
   enum CmdSourceParam : uint8_t { INTERNAL = 0, EXTERNAL = 1 };
+  enum SwitchState : uint8_t { ON = 1, OFF = 0 };
 
   /** Device command IDs */
   static const DeviceCommandId_t SWITCH_HEATER = 0x0;
@@ -61,10 +67,12 @@ class HeaterHandler : public ExecutableObjectIF,
   HeaterHandler(object_id_t setObjectId, GpioIF* gpioInterface_, HeaterHelper helper,
                 PowerSwitchIF* mainLineSwitcherObjectId, power::Switch_t mainLineSwitch);
 
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
+  ReturnValue_t getAllSwitchStates(std::array<SwitchState, 8>& statesBuf);
+
   virtual ~HeaterHandler();
 
  protected:
-  enum SwitchState : bool { ON = true, OFF = false };
   enum SwitchAction : uint8_t { SET_SWITCH_OFF, SET_SWITCH_ON, NONE };
 
   ReturnValue_t switchHeater(heater::Switchers heater, SwitchState switchState);
@@ -128,12 +136,14 @@ class HeaterHandler : public ExecutableObjectIF,
 
   HeaterMap heaterVec = {};
 
-  MutexIF* heaterMutex = nullptr;
+  MutexIF* heaterHealthAndStateMutex = nullptr;
 
   HeaterHelper helper;
+  ModeHelper modeHelper;
 
   /** Size of command queue */
   size_t cmdQueueSize = 20;
+  bool waitForSwitchOff = true;
 
   GpioIF* gpioInterface = nullptr;
 
@@ -152,6 +162,9 @@ class HeaterHandler : public ExecutableObjectIF,
 
   StorageManagerIF* ipcStore = nullptr;
 
+  Mode_t mode = HasModesIF::MODE_OFF;
+  Submode_t submode = 0;
+
   void readCommandQueue();
 
   /**
@@ -172,6 +185,16 @@ class HeaterHandler : public ExecutableObjectIF,
    */
   void setInitialSwitchStates();
 
+  // HasModesIF implementation
+  void announceMode(bool recursive) override;
+  void getMode(Mode_t* mode, Submode_t* submode) override;
+
+  // Mode Tree helper overrides
+  object_id_t getObjectId() const override;
+  const HasHealthIF* getOptHealthIF() const override;
+  const HasModesIF& getModeIF() const override;
+  ModeTreeChildIF& getModeTreeChildIF() override;
+
   void handleSwitchOnCommand(heater::Switchers heaterIdx);
 
   void handleSwitchOffCommand(heater::Switchers heaterIdx);

mission/devices/ImtqHandler.h

@@ -2,7 +2,7 @@
 #define MISSION_DEVICES_IMTQHANDLER_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <string.h>
 
 #include "events/subsystemIdRanges.h"
@@ -23,7 +23,6 @@ class ImtqHandler : public DeviceHandlerBase {
 
   void setPollingMode(NormalPollingMode pollMode);
 
-  void doSendRead() override;
   /**
    * @brief   Sets mode to MODE_NORMAL. Can be used for debugging.
    */
@@ -32,6 +31,7 @@ class ImtqHandler : public DeviceHandlerBase {
   void setDebugMode(bool enable);
 
  protected:
+  ReturnValue_t performOperation(uint8_t opCode);
   void doStartUp() override;
   void doShutDown() override;
   ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
@@ -42,7 +42,6 @@ class ImtqHandler : public DeviceHandlerBase {
   ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId,
                              size_t* foundLen) override;
   ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override;
-  void setNormalDatapoolEntriesInvalid() override;
   virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -50,19 +49,6 @@ class ImtqHandler : public DeviceHandlerBase {
   ReturnValue_t getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) override;
 
  private:
-  static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
-
-  static const ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0xA0);
-  static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(0xA1);
-  static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(0xA2);
-  static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
-  static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(0xA4);
-  static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(0xA5);
-  static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(0xA6);
-  //! [EXPORT] : [COMMENT] The status reply to a self test command was received but no self test
-  //! command has been sent. This should normally never happen.
-  static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
-
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::IMTQ_HANDLER;
 
   //! [EXPORT] : [COMMENT] Get self test result returns I2C failure
@@ -97,49 +83,43 @@ class ImtqHandler : public DeviceHandlerBase {
   //! link between IMTQ and OBC.
   static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);
 
-  IMTQ::EngHkDataset engHkDataset;
-  IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet;
-  IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet;
-  IMTQ::DipoleActuationSet dipoleSet;
-  IMTQ::PosXSelfTestSet posXselfTestDataset;
-  IMTQ::NegXSelfTestSet negXselfTestDataset;
-  IMTQ::PosYSelfTestSet posYselfTestDataset;
-  IMTQ::NegYSelfTestSet negYselfTestDataset;
-  IMTQ::PosZSelfTestSet posZselfTestDataset;
-  IMTQ::NegZSelfTestSet negZselfTestDataset;
+  imtq::StatusDataset statusSet;
+  imtq::DipoleActuationSet dipoleSet;
+  imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
+  imtq::HkDatasetNoTorque hkDatasetNoTorque;
+
+  imtq::RawMtmMeasurementWithTorque rawMtmWithTorque;
+  imtq::HkDatasetWithTorque hkDatasetWithTorque;
+
+  imtq::CalibratedMtmMeasurementSet calMtmMeasurementSet;
+  imtq::PosXSelfTestSet posXselfTestDataset;
+  imtq::NegXSelfTestSet negXselfTestDataset;
+  imtq::PosYSelfTestSet posYselfTestDataset;
+  imtq::NegYSelfTestSet negYselfTestDataset;
+  imtq::PosZSelfTestSet posZselfTestDataset;
+  imtq::NegZSelfTestSet negZselfTestDataset;
 
   NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
 
+  PoolEntry<uint8_t> statusMode = PoolEntry<uint8_t>({0});
+  PoolEntry<uint8_t> statusError = PoolEntry<uint8_t>({0});
+  PoolEntry<uint8_t> statusConfig = PoolEntry<uint8_t>({0});
+  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);
-  // Hardcoded to default integration time of 10 ms.
-  // SHOULDDO: Support for other integration times
-  Countdown integrationTimeCd = Countdown(10);
-
   power::Switch_t switcher = power::NO_SWITCH;
 
-  uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE];
+  uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
   bool goToNormalMode = false;
   bool debugMode = false;
+  bool specialRequestActive = false;
+  bool firstReplyCycle = true;
 
-  enum class CommunicationStep {
-    GET_ENG_HK_DATA,
-    START_MTM_MEASUREMENT,
-    GET_CAL_MTM_MEASUREMENT,
-    GET_RAW_MTM_MEASUREMENT,
-    DIPOLE_ACTUATION
-  };
-
-  CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
-
-  enum class StartupStep { NONE, COMMAND_SELF_TEST, GET_SELF_TEST_RESULT };
-
-  StartupStep startupStep = StartupStep::COMMAND_SELF_TEST;
-
-  bool selfTestPerformed = false;
+  imtq::RequestType requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
 
   /**
    * @brief   In case of a status reply to a single axis self test command, this function
@@ -155,7 +135,7 @@ class ImtqHandler : public DeviceHandlerBase {
    *
    * @return  The return code derived from the received status byte.
    */
-  ReturnValue_t parseStatusByte(const uint8_t* packet);
+  ReturnValue_t parseStatusByte(imtq::CC::CC command, const uint8_t* packet);
 
   /**
    * @brief   This function fills the engineering housekeeping dataset with the received data.
@@ -163,23 +143,9 @@ class ImtqHandler : public DeviceHandlerBase {
    * @param packet    Pointer to the received data.
    *
    */
-  void fillEngHkDataset(const uint8_t* packet);
+  void fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet);
 
-  /**
-   * @brief   This function sends a command reply to the requesting queue.
-   *
-   * @param data  Pointer to the data to send.
-   * @param dataSize  Size of the data to send.
-   * @param relplyId  Reply id which will be inserted at the beginning of the action message.
-   */
-  void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId);
-
-  /**
-   * @brief   This function handles the reply containing the commanded dipole.
-   *
-   * @param packet    Pointer to the reply data.
-   */
-  void handleGetCommandedDipoleReply(const uint8_t* packet);
+  void fillSystemStateIntoDataset(const uint8_t* packet);
 
   /**
    * @brief   This function parses the reply containing the calibrated MTM measurement and writes
@@ -212,7 +178,7 @@ class ImtqHandler : public DeviceHandlerBase {
   void handlePositiveZSelfTestReply(const uint8_t* packet);
   void handleNegativeZSelfTestReply(const uint8_t* packet);
 
-  ReturnValue_t buildDipoleActuationCommand();
+  // ReturnValue_t buildDipoleActuationCommand();
   /**
    * @brief   This function checks the error byte of a self test measurement.
    *

mission/devices/PayloadPcduHandler.cpp

@@ -517,6 +517,9 @@ void PayloadPcduHandler::checkJsonFileInit() {
   if (not jsonFileInitComplete) {
     auto activeSd = sdcMan->getActiveSdCard();
     if (activeSd and sdcMan->isSdCardUsable(activeSd.value())) {
+      if (sdcMan->getCurrentMountPrefix() == nullptr) {
+        return;
+      }
       params.initialize(sdcMan->getCurrentMountPrefix());
       jsonFileInitComplete = true;
     }