Merge branch 'main' into auto-switch-image-feature

.gitignore

@@ -22,7 +22,3 @@ __pycache__
 !/.idea/cmake.xml
 
 generators/*.db
-
-# Clangd LSP
-/compile_commands.json
-/.cache

CHANGELOG.md

@@ -16,206 +16,12 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
-# [v7.7.0] 2024-02-29
-
-- Bumped `eive-tmtc` to v6.1.0
-- Bumped `eive-fsfw`
-
-## Fixed
-
-- PLOC SUPV sets: Added missing `PoolReadGuard` instantiations when reading boot status report
-  and latchup status report.
-- PLOC SUPV latchup report could not be handled previously.
-- Bugfix in PLOC SUPV latchup report parsing.
-- Bugfix in PLOC MPSoC HK set: Set and variables were not set valid.
-- The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode
-  and is triggered by the `AcsController` now.
-- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
-- Fixed calculation of desaturation torque for faulty RWs.
-- Fixed bugs within the `MEKF` and simplified the code.
-
-## Changed
-
-- `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes
-- QUEST and STR rates are now allowed per default
-- Changed PTG Strat priorities to favor STR before MEKF.
-- Increased message queue depth and maximum number of handled messages per cycle for
-  `PusServiceBase` based classes (especially PUS scheduler).
-- `MathOperations` functions were moved to their appropriate classes within the `eive-fsfw`
-- Changed pointing strategy for target groundstation mode to prevent blinding of the STR. This
-  also limits the rotation for the reference target quaternion to prevent spikes in required
-  rotation rates.
-- Updated QUEST and Sun Vector Params to new values.
-- Removed the satellites's angular momentum from desaturation calculation.
-- Bumped internal `sagittactl` library to v11.11.
-
 ## Added
 
-- Updated STR handler to unlock and allow using the secondary firmware slot.
-- STR handling for new BlobStats TM set.
-- Added new action command to update the standard deviations within the `MEKF` from the
-  `AcsParameters`.
-
-# [v7.6.1] 2024-02-05
-
-## Changed
-
-- Guidance now uses the coordinate functions from the FSFW.
-- Idle should now point the STR away from the earth
-
-## Fixed
-
-- Fixed bugs in `Guidance::comparePtg` and corrected overloading
-- Detumbling State Machine is now robust to commanded mode changes.
-
-# [v7.6.0] 2024-01-30
-
-- Bumped `eive-tmtc` to v5.13.0
-- Bumped `eive-fsfw`
-
-## Added
-
-- Added new parameter for MPSoC which allows to skip SUPV commanding.
-
-## Changed
-
-- Increased allowed mode transition time for PLOC SUPV.
-- Detumbling can now be triggered from all modes of the `AcsController`. In case the
-  current mode is a higher pointing mode, the STR will be set to faulty, to trigger a
-  transition to safe first. Then, in a second step, a transition to detumble is triggered.
-
-## Fixed
-
-- If the PCDU handler fails reading data from the IPC store, it will
-  not try to do a deserialization anymore.
-- All action commands sent by the PLOC SUPV to itself will have no sender now.
-- RW speed commands get reset to 0 RPM, if the `AcsController` has changed its mode
-  to Safe
-- Antistiction for RWs will set commanded speed to 0 RPM, if a wheel is detected as not
-  working
-- Removed parameter to disable antistiction, as deactivating it would result in the
-  `AcsController` being allowed sending invalid speed commands to the RW Handler, which
-  would then trigger FDIR and turning off the functioning device
-- `RwHandler` returnvalues would use the `INTERFACE_ID` of the `DeviceHandlerBase`
-- The `AcsController` will reset its stored guidance values on mode change and lost
-  orientation.
-- The nullspace controller will only be used if all RWs are available.
-- Calculation of required rotation rate in pointing modes has been fixed to actual
-  calculation of rotation rate from two quaternions.
-- Fixed alignment matrix and pseudo inverses of RWs, to match the wrong definition of
-  positive rotation.
-
-# [v7.5.5] 2024-01-22
-
-## Fixed
-
-- Calculation of error quaternion was done with inverse of the required target quaternion.
-
-# [v7.5.4] 2024-01-16
-
-## Fixed
-
-- Pointing strategy now actually uses fused rotation rate source instead of its valid flag.
-- All datasets now get updated during pointing mode, even if the strategy is a fault one.
-
-# [v7.5.3] 2023-12-19
-
-## Fixed
-
-- Set STR quaternions to invalid in device handler if the solution is not trustworthy.
-
-# [v7.5.2] 2023-12-14
-
-## Fixed
-
-- Fixed faulty scaling within the QUEST algorithm.
-
-# [v7.5.1] 2023-12-13
-
-- `eive-tmtc` v5.12.1
-
-## Changed
-
-- Increased the maximum number of scheduled telecommands from 500 to 4000. Merry Christmas!
-
-## Fixed
-
-- Faulty mapping of input values for QUEST algorithm.
-- Fixed validity check for QUEST algorithm.
-
-# [v7.5.0] 2023-12-06
-
-- `eive-tmtc` v5.12.0
-
-## Changed
-
-- ACS-Board default side changed to B-Side
-- The TLE uploaded now gets stored in a file on the filesystem. It will always be stored on
-  the current active SD Card. After a reboot, the TLE will be read from the filesystem.
-  A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even
-  though it is there.
-- Added action cmd to read the currently stored TLE.
-- Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate
-  the time difference.
-- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing
-  on to the relevant mode functions. It handles all telemetry relevant functions, which were
-  always called, regardless of the mode.
- 
-## Added
-
-- Higher ACS modes can now be entered without a running `MEKF`. Higher modes will collect their
-  quaternion and rotational rate depending on the available sources.
-- `QUEST` attitude estimation was added to the `AcsController`.
-- The fused rotational rate can now be estimated from `QUEST` and the `STR`.
-
-# [v7.4.1] 2023-12-06
-
-## Fixed
-
-- Schedule SCEX again. Scheduling was removed accidentaly when Payload Task was converted to a PST.
-- SCEX transition was previously 0 seconds.. which did not lead to bugs? In any case it is 5
-  seconds now.
-
-# [v7.4.0] 2023-11-30
-
-- `eive-tmtc` v5.11.0
-
-## Changed
-
-- Rewrote the PLOC Supervisor Handler, which is now based on a new device handler base class.
-  Added ADC and Logging Counters telemetry set support.
-
-## Fixed
-
-- Increase allowed time for PTME writers to finish partial transfers. A duration of 200 ms was
-  not sufficient for cases where 3 writers write concurrently.
-- Fixed state issue for PTME writer object where the writer was not reset properly after a timeout
-  of a partial transfer. This was a major bug blocking the whole VC if it occured.
-- STR config path was previously hardcoded to `/mnt/sd0/startracker/flight-config.json`.
-  A new abstraction was introduces which now uses the active SD card to build the correct
-  config path when initializing the star tracker.
-
-## Added
-
-- PL PCDU: Add command to enable and disable channel order checks.
-- Added new PUS 15 subservice `DELETE_BY_TIME_RANGE` which allows to also specify a deletion
-  start time when deleting packets from the persistent TM store.
-- Introduced a new `RELOAD_JSON_CFG_FILE` command for the STR to reload the JSON configuration
-  data based on the current output of the config file path getter function. A reboot of the
-  device is still necessary to load the configuration to the STR.
-
-# [v7.3.0] 2023-11-07
-
-## Changed
-
-- Changed PDEC addresses depending on which firmware version is used. It is suspected that
-  the previous addresses were invalid and not properly covered by the Linux memory protection.
-  The OBSW will use the old addresses for older FW versions.
-- Reverted some STR ComIF behaviour back to an older software version.
-
-## Added
-
-- Always add PLOC MPSoC and PLOC SUPV components for the EM as well.
+- Added a new safety mechanism where the ProASIC scratch buffer can be used to trigger an
+  auto-boot to another image. The auto-boot is currently implemented as a one-shot mechanism:
+  The key-value pair which triggers the auto-boot will be removed from the scratch buffer.
+  See more information [here](https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/OBC_Auto_Switch_Image)
 
 # [v7.2.0] 2023-10-27
 
@@ -885,7 +691,7 @@ This is the version which will fly on the satellite for the initial launch phase
   This gives other tasks some time to register the SD cards being unusable, and therefore provides
   a way for them to perform any re-initialization tasks necessary after SD card switches.
 - TCS controller now only has an OFF mode and an ON mode
-- The TCS controller pauses operations related to the TCS board assembly (reading sensors and
+- The TCS controller pauses operations related to the TCS board assembly (reading sensors and 
   the primary control loop) while a TCS board recovery is on-going.
 - Allow specifying custom OBSW update filename. This allowed keeping a cleaner file structure
   where each update has a name including the version
@@ -1950,8 +1756,8 @@ Syrlinks PR: PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/353
 - Syrlinks Handler: Read RX frequency shift as 24 bit signed number now. Also include
   validity handling for datasets.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/350
-- `GyroADIS1650XHandler`: Changed calculation of angular rate to be sensitivity based instead of
-  max. range based, as previous fix still left an margin of error between ADIS16505 sensors
+- `GyroADIS1650XHandler`: Changed calculation of angular rate to be sensitivity based instead of 
+  max. range based, as previous fix still left an margin of error between ADIS16505 sensors 
   and L3GD20 sensors.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/346
 

CMakeLists.txt

@@ -10,7 +10,7 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 7)
-set(OBSW_VERSION_MINOR 7)
+set(OBSW_VERSION_MINOR 2)
 set(OBSW_VERSION_REVISION 0)
 
 # set(CMAKE_VERBOSE TRUE)
@@ -64,7 +64,7 @@ include(EiveHelpers)
 option(EIVE_ADD_ETL_LIB "Add ETL library" ON)
 option(EIVE_ADD_JSON_LIB "Add JSON library" ON)
 
-set(OBSW_MAX_SCHEDULED_TCS 4000)
+set(OBSW_MAX_SCHEDULED_TCS 500)
 
 if(EIVE_Q7S_EM)
   set(OBSW_Q7S_EM
@@ -126,13 +126,13 @@ set(OBSW_ADD_HEATERS
     1
     CACHE STRING "Add TCS heaters")
 set(OBSW_ADD_PLOC_SUPERVISOR
-    1
+    ${INIT_VAL}
     CACHE STRING "Add PLOC supervisor handler")
 set(OBSW_ADD_SA_DEPL
     ${INIT_VAL}
     CACHE STRING "Add SA deployment handler")
 set(OBSW_ADD_PLOC_MPSOC
-    1
+    ${INIT_VAL}
     CACHE STRING "Add MPSoC handler")
 set(OBSW_ADD_ACS_CTRL
     ${INIT_VAL}

bsp_hosted/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 320 translations.
+ * @brief    Auto-generated event translation file. Contains 315 translations.
  * @details
- * Generated on: 2024-02-29 13:15:00
+ * Generated on: 2023-10-27 14:24:05
  */
 #include "translateEvents.h"
 
@@ -94,17 +94,14 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
 const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
-const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
+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_RECOVERY_STRING = "MEKF_RECOVERY";
 const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
-const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
+const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
 const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
-const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
-const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
-const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
 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";
@@ -160,8 +157,6 @@ const char *SUPV_EXE_FAILURE_STRING = "SUPV_EXE_FAILURE";
 const char *SUPV_CRC_FAILURE_EVENT_STRING = "SUPV_CRC_FAILURE_EVENT";
 const char *SUPV_HELPER_EXECUTING_STRING = "SUPV_HELPER_EXECUTING";
 const char *SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUTDOWN_BUILD_FAILED";
-const char *SUPV_ACK_UNKNOWN_COMMAND_STRING = "SUPV_ACK_UNKNOWN_COMMAND";
-const char *SUPV_EXE_ACK_UNKNOWN_COMMAND_STRING = "SUPV_EXE_ACK_UNKNOWN_COMMAND";
 const char *SANITIZATION_FAILED_STRING = "SANITIZATION_FAILED";
 const char *MOUNTED_SD_CARD_STRING = "MOUNTED_SD_CARD";
 const char *SEND_MRAM_DUMP_FAILED_STRING = "SEND_MRAM_DUMP_FAILED";
@@ -507,7 +502,7 @@ const char *translateEvents(Event event) {
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
-      return RATE_RECOVERY_STRING;
+      return SAFE_RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
     case (11203):
@@ -517,17 +512,11 @@ const char *translateEvents(Event event) {
     case (11205):
       return MEKF_AUTOMATIC_RESET_STRING;
     case (11206):
-      return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
+      return MEKF_INVALID_MODE_VIOLATION_STRING;
     case (11207):
       return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11208):
       return TLE_TOO_OLD_STRING;
-    case (11209):
-      return TLE_FILE_READ_FAILED_STRING;
-    case (11210):
-      return PTG_RATE_VIOLATION_STRING;
-    case (11211):
-      return DETUMBLE_TRANSITION_FAILED_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):
@@ -638,10 +627,6 @@ const char *translateEvents(Event event) {
       return SUPV_HELPER_EXECUTING_STRING;
     case (12008):
       return SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING;
-    case (12009):
-      return SUPV_ACK_UNKNOWN_COMMAND_STRING;
-    case (12010):
-      return SUPV_EXE_ACK_UNKNOWN_COMMAND_STRING;
     case (12100):
       return SANITIZATION_FAILED_STRING;
     case (12101):

bsp_hosted/fsfwconfig/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 175 translations.
- * Generated on: 2024-02-29 13:15:00
+ * Generated on: 2023-10-27 14:24:05
  */
 #include "translateObjects.h"
 

bsp_hosted/objectFactory.cpp

@@ -38,9 +38,9 @@
 
 #include "devices/gpioIds.h"
 #include "fsfw_hal/linux/gpio/Gpio.h"
-#include "linux/payload/FreshSupvHandler.h"
 #include "linux/payload/PlocMpsocHandler.h"
 #include "linux/payload/PlocMpsocSpecialComHelper.h"
+#include "linux/payload/PlocSupervisorHandler.h"
 #include "linux/payload/PlocSupvUartMan.h"
 #include "test/gpio/DummyGpioIF.h"
 #endif
@@ -97,11 +97,10 @@ void ObjectFactory::produce(void* args) {
       new SerialCookie(objects::PLOC_SUPERVISOR_HANDLER, plocSupvString, uart::PLOC_SUPV_BAUD,
                        supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL);
   supervisorCookie->setNoFixedSizeReply();
-  new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER);
-  DhbConfig dhbConf(objects::PLOC_SUPERVISOR_HANDLER);
-  auto* supvHandler =
-      new FreshSupvHandler(dhbConf, supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, dummyGpioIF),
-                           dummySwitcher, power::PDU1_CH6_PLOC_12V);
+  auto supvHelper = new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER);
+  new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, supervisorCookie,
+                            Gpio(gpioIds::ENABLE_SUPV_UART, dummyGpioIF), pcdu::PDU1_CH6_PLOC_12V,
+                            *supvHelper);
 #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */
 #endif
 

bsp_q7s/CMakeLists.txt

@@ -25,4 +25,3 @@ add_subdirectory(memory)
 add_subdirectory(callbacks)
 add_subdirectory(xadc)
 add_subdirectory(fs)
-add_subdirectory(acs)

bsp_q7s/acs/CMakeLists.txt

@@ -1 +0,0 @@
-# target_sources(${OBSW_NAME} PUBLIC <Source File List>)

bsp_q7s/acs/StrConfigPathGetter.h

@@ -1,23 +0,0 @@
-#include <optional>
-
-#include "bsp_q7s/fs/SdCardManager.h"
-#include "mission/acs/str/strHelpers.h"
-
-class StrConfigPathGetter : public startracker::SdCardConfigPathGetter {
- public:
-  StrConfigPathGetter(SdCardManager& sdcMan) : sdcMan(sdcMan) {}
-
-  std::optional<std::string> getCfgPath() override {
-    if (!sdcMan.isSdCardUsable(std::nullopt)) {
-      return std::nullopt;
-    }
-    if (sdcMan.getActiveSdCard() == sd::SdCard::SLOT_1) {
-      return std::string("/mnt/sd1/startracker/flight-config.json");
-    } else {
-      return std::string("/mnt/sd0/startracker/flight-config.json");
-    }
-  }
-
- private:
-  SdCardManager& sdcMan;
-};

bsp_q7s/boardconfig/busConf.h

@@ -18,8 +18,7 @@ static constexpr char I2C_Q7_EIVE[] = "/dev/i2c_q7";
 
 static constexpr char UART_GNSS_DEV[] = "/dev/gps0";
 static constexpr char UART_PLOC_MPSOC_DEV[] = "/dev/ul_plmpsoc";
-static constexpr char UART_PLOC_SUPERVISOR_DEV_FALLBACK[] = "/dev/ttyUL4";
-static constexpr char UART_PLOC_SUPERVISOR_DEV[] = "/dev/ploc_supv";
+static constexpr char UART_PLOC_SUPERVSIOR_DEV[] = "/dev/ploc_supv";
 static constexpr char UART_SYRLINKS_DEV[] = "/dev/ul_syrlinks";
 static constexpr char UART_STAR_TRACKER_DEV[] = "/dev/ul_str";
 static constexpr char UART_SCEX_DEV[] = "/dev/scex";

bsp_q7s/core/CoreController.cpp

@@ -359,6 +359,29 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_
     case (OBSW_UPDATE_FROM_TMP): {
       return executeSwUpdate(SwUpdateSources::TMP_DIR, data, size);
     }
+    case (ENABLE_AUTO_SWITCH): {
+      if (size < 2) {
+        return HasActionsIF::INVALID_PARAMETERS;
+      }
+      uint8_t chip = data[0];
+      uint8_t copy = data[1];
+      if (chip > 1 or copy > 1) {
+        return HasActionsIF::INVALID_PARAMETERS;
+      }
+      std::string value = std::to_string(chip) + std::to_string(copy);
+      ReturnValue_t result = scratch::writeString(scratch::AUTO_SWITCH_IMAGE, value);
+      if (result == returnvalue::OK) {
+        return EXECUTION_FINISHED;
+      }
+      return result;
+    }
+    case (DISABLE_AUTO_SWITCH): {
+      ReturnValue_t result = scratch::clearValue(scratch::AUTO_SWITCH_IMAGE);
+      if (result != returnvalue::OK and result != scratch::KEY_NOT_FOUND) {
+        return result;
+      }
+      return EXECUTION_FINISHED;
+    }
     case (SWITCH_TO_SD_0): {
       if (not startSdStateMachine(sd::SdCard::SLOT_0, SdCfgMode::COLD_REDUNDANT, commandedBy,
                                   actionId)) {

bsp_q7s/em/emObjectFactory.cpp

@@ -152,7 +152,7 @@ void ObjectFactory::produce(void* args) {
 #endif
 
 #if OBSW_ADD_STAR_TRACKER == 1
-  createStrComponents(pwrSwitcher, *SdCardManager::instance());
+  createStrComponents(pwrSwitcher);
 #endif /* OBSW_ADD_STAR_TRACKER == 1 */
 
 #if OBSW_ADD_PL_PCDU == 1
@@ -163,8 +163,8 @@ void ObjectFactory::produce(void* args) {
 #if OBSW_ADD_CCSDS_IP_CORES == 1
   CcsdsIpCoreHandler* ipCoreHandler = nullptr;
   CcsdsComponentArgs ccsdsArgs(*gpioComIF, *ipcStore, *tmStore, stores, *pusFunnel, *cfdpFunnel,
-                               &ipCoreHandler, 0, 0);
-  createCcsdsIpComponentsWrapper(ccsdsArgs);
+                               &ipCoreHandler);
+  createCcsdsIpComponentsAddTmRouting(ccsdsArgs);
 #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */
 
   /* Test Task */
@@ -175,7 +175,7 @@ void ObjectFactory::produce(void* args) {
   createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false,
                        power::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V);
 #endif
-  createAcsController(true, enableHkSets, *SdCardManager::instance());
+  createAcsController(true, enableHkSets);
   HeaterHandler* heaterHandler;
   createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler);
   createThermalController(*heaterHandler, true);

bsp_q7s/fmObjectFactory.cpp

@@ -109,14 +109,14 @@ void ObjectFactory::produce(void* args) {
   createPayloadComponents(gpioComIF, *pwrSwitcher);
 
 #if OBSW_ADD_STAR_TRACKER == 1
-  createStrComponents(pwrSwitcher, *SdCardManager::instance());
+  createStrComponents(pwrSwitcher);
 #endif /* OBSW_ADD_STAR_TRACKER == 1 */
 
 #if OBSW_ADD_CCSDS_IP_CORES == 1
   CcsdsIpCoreHandler* ipCoreHandler = nullptr;
   CcsdsComponentArgs ccsdsArgs(*gpioComIF, *ipcStore, *tmStore, stores, *pusFunnel, *cfdpFunnel,
-                               &ipCoreHandler, 0, 0);
-  createCcsdsIpComponentsWrapper(ccsdsArgs);
+                               &ipCoreHandler);
+  createCcsdsIpComponentsAddTmRouting(ccsdsArgs);
 #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */
 
 #if OBSW_ADD_SCEX_DEVICE == 1
@@ -130,6 +130,6 @@ void ObjectFactory::produce(void* args) {
 
   createMiscComponents();
   createThermalController(*heaterHandler, false);
-  createAcsController(true, enableHkSets, *SdCardManager::instance());
+  createAcsController(true, enableHkSets);
   satsystem::init(false);
 }

bsp_q7s/memory/scratchApi.h

@@ -19,6 +19,7 @@ namespace scratch {
 
 static constexpr char PREFERED_SDC_KEY[] = "PREFSD";
 static constexpr char ALLOC_FAILURE_COUNT[] = "ALLOCERR";
+static constexpr char AUTO_SWITCH_IMAGE[] = "ASWI";
 
 static constexpr uint8_t INTERFACE_ID = CLASS_ID::SCRATCH_BUFFER;
 static constexpr ReturnValue_t KEY_NOT_FOUND = returnvalue::makeCode(INTERFACE_ID, 0);
@@ -76,7 +77,6 @@ ReturnValue_t readToFile(std::string name, std::ifstream& file, std::string& fil
   int result = std::system(oss.str().c_str());
   if (result != 0) {
     if (WEXITSTATUS(result) == 1) {
-      sif::warning << "scratch::readToFile: Key " << name << " does not exist" << std::endl;
       // Could not find value
       std::remove(filename.c_str());
       return KEY_NOT_FOUND;

bsp_q7s/objectFactory.cpp

@@ -13,6 +13,7 @@
 #include <linux/payload/PlocMemoryDumper.h>
 #include <linux/payload/PlocMpsocHandler.h>
 #include <linux/payload/PlocMpsocSpecialComHelper.h>
+#include <linux/payload/PlocSupervisorHandler.h>
 #include <linux/payload/ScexUartReader.h>
 #include <linux/payload/plocMpsocHelpers.h>
 #include <linux/power/CspComIF.h>
@@ -36,10 +37,11 @@
 #include <cstring>
 
 #include "OBSWConfig.h"
-#include "bsp_q7s/acs/StrConfigPathGetter.h"
 #include "bsp_q7s/boardtest/Q7STestTask.h"
 #include "bsp_q7s/callbacks/gnssCallback.h"
+#include "bsp_q7s/callbacks/pcduSwitchCb.h"
 #include "bsp_q7s/callbacks/q7sGpioCallbacks.h"
+#include "bsp_q7s/callbacks/rwSpiCallback.h"
 #include "busConf.h"
 #include "ccsdsConfig.h"
 #include "devConf.h"
@@ -58,7 +60,6 @@
 #include "linux/ipcore/PdecHandler.h"
 #include "linux/ipcore/Ptme.h"
 #include "linux/ipcore/PtmeConfig.h"
-#include "linux/payload/FreshSupvHandler.h"
 #include "mission/config/configfile.h"
 #include "mission/system/acs/AcsBoardFdir.h"
 #include "mission/system/acs/AcsSubsystem.h"
@@ -67,11 +68,11 @@
 #include "mission/system/acs/acsModeTree.h"
 #include "mission/system/com/SyrlinksFdir.h"
 #include "mission/system/com/comModeTree.h"
-#include "mission/system/payloadModeTree.h"
 #include "mission/system/power/GomspacePowerFdir.h"
 #include "mission/system/tcs/RtdFdir.h"
 #include "mission/system/tcs/TcsBoardAssembly.h"
 #include "mission/system/tcs/tcsModeTree.h"
+#include "mission/system/tree/payloadModeTree.h"
 #include "mission/tmtc/tmFilters.h"
 #include "mission/utility/GlobalConfigHandler.h"
 #include "tmtc/pusIds.h"
@@ -611,11 +612,11 @@ void ObjectFactory::createSyrlinksComponents(PowerSwitchIF* pwrSwitcher) {
 #endif
 }
 
-void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF& pwrSwitcher) {
+void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF& pwrSwitch) {
   using namespace gpio;
   std::stringstream consumer;
   auto* camSwitcher =
-      new CamSwitcher(objects::CAM_SWITCHER, pwrSwitcher, power::PDU2_CH8_PAYLOAD_CAMERA);
+      new CamSwitcher(objects::CAM_SWITCHER, pwrSwitch, power::PDU2_CH8_PAYLOAD_CAMERA);
   camSwitcher->connectModeTreeParent(satsystem::payload::SUBSYSTEM);
 #if OBSW_ADD_PLOC_MPSOC == 1
   consumer << "0x" << std::hex << objects::PLOC_MPSOC_HANDLER;
@@ -641,19 +642,15 @@ void ObjectFactory::createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwit
   auto supvGpioCookie = new GpioCookie;
   supvGpioCookie->addGpio(gpioIds::ENABLE_SUPV_UART, gpioConfigSupv);
   gpioComIF->addGpios(supvGpioCookie);
-  const char* plocSupvDev = q7s::UART_PLOC_SUPERVISOR_DEV;
-  if (not std::filesystem::exists(plocSupvDev)) {
-    plocSupvDev = q7s::UART_PLOC_SUPERVISOR_DEV_FALLBACK;
-  }
-  auto supervisorCookie =
-      new SerialCookie(objects::PLOC_SUPERVISOR_HANDLER, plocSupvDev, serial::PLOC_SUPV_BAUD,
-                       supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL);
+  auto supervisorCookie = new SerialCookie(objects::PLOC_SUPERVISOR_HANDLER,
+                                           q7s::UART_PLOC_SUPERVSIOR_DEV, serial::PLOC_SUPV_BAUD,
+                                           supv::MAX_PACKET_SIZE * 20, UartModes::NON_CANONICAL);
   supervisorCookie->setNoFixedSizeReply();
-  new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER);
-  DhbConfig dhbConf(objects::PLOC_SUPERVISOR_HANDLER);
-  auto* supvHandler =
-      new FreshSupvHandler(dhbConf, supervisorCookie, Gpio(gpioIds::ENABLE_SUPV_UART, gpioComIF),
-                           pwrSwitcher, power::PDU1_CH6_PLOC_12V);
+  auto supvHelper = new PlocSupvUartManager(objects::PLOC_SUPERVISOR_HELPER);
+  auto* supvHandler = new PlocSupervisorHandler(objects::PLOC_SUPERVISOR_HANDLER, supervisorCookie,
+                                                Gpio(gpioIds::ENABLE_SUPV_UART, gpioComIF),
+                                                power::PDU1_CH6_PLOC_12V, *supvHelper);
+  supvHandler->setPowerSwitcher(&pwrSwitch);
   supvHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM);
 #endif /* OBSW_ADD_PLOC_SUPERVISOR == 1 */
   static_cast<void>(consumer);
@@ -839,7 +836,7 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(CcsdsComponentArgs& args) {
   uioNames.registers = q7s::UIO_PDEC_REGISTERS;
   uioNames.irq = q7s::UIO_PDEC_IRQ;
   new PdecHandler(objects::PDEC_HANDLER, objects::CCSDS_HANDLER, &args.gpioComIF,
-                  gpioIds::PDEC_RESET, uioNames, args.pdecCfgMemBaseAddr, args.pdecRamBaseAddr);
+                  gpioIds::PDEC_RESET, uioNames);
   GpioCookie* gpioRS485Chip = new GpioCookie;
   gpio = new GpiodRegularByLineName(q7s::gpioNames::RS485_EN_TX_CLOCK, "RS485 Transceiver",
                                     Direction::OUT, Levels::LOW);
@@ -934,7 +931,7 @@ void ObjectFactory::createTestComponents(LinuxLibgpioIF* gpioComIF) {
 #endif
 }
 
-void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher, SdCardManager& sdcMan) {
+void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) {
   auto* strAssy = new StrAssembly(objects::STR_ASSY);
   strAssy->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
   auto* starTrackerCookie =
@@ -948,10 +945,9 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher, SdCardManage
     sif::error << "No valid Star Tracker parameter JSON file" << std::endl;
   }
   auto strFdir = new StrFdir(objects::STAR_TRACKER);
-  auto cfgGetter = new StrConfigPathGetter(sdcMan);
   auto starTracker =
       new StarTrackerHandler(objects::STAR_TRACKER, objects::STR_COM_IF, starTrackerCookie,
-                             strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter, sdcMan);
+                             paramJsonFile, strComIF, power::PDU1_CH2_STAR_TRACKER_5V);
   starTracker->setPowerSwitcher(pwrSwitcher);
   starTracker->connectModeTreeParent(*strAssy);
   starTracker->setCustomFdir(strFdir);
@@ -1066,13 +1062,7 @@ ReturnValue_t ObjectFactory::readFirmwareVersion() {
   return returnvalue::OK;
 }
 
-ReturnValue_t ObjectFactory::createCcsdsIpComponentsWrapper(CcsdsComponentArgs& ccsdsArgs) {
-  ccsdsArgs.pdecCfgMemBaseAddr = config::pdec::PDEC_CONFIG_BASE_ADDR;
-  ccsdsArgs.pdecRamBaseAddr = config::pdec::PDEC_RAM_ADDR;
-  if (core::FW_VERSION_MAJOR < 6) {
-    ccsdsArgs.pdecCfgMemBaseAddr = config::pdec::PDEC_CONFIG_BASE_ADDR_LEGACY;
-    ccsdsArgs.pdecRamBaseAddr = config::pdec::PDEC_RAM_ADDR_LEGACY;
-  }
+ReturnValue_t ObjectFactory::createCcsdsIpComponentsAddTmRouting(CcsdsComponentArgs& ccsdsArgs) {
   ReturnValue_t result = createCcsdsComponents(ccsdsArgs);
 #if OBSW_TM_TO_PTME == 1
   if (ccsdsArgs.normalLiveTmDest != MessageQueueIF::NO_QUEUE) {

bsp_q7s/objectFactory.h

@@ -15,8 +15,6 @@
 #include <atomic>
 #include <string>
 
-#include "bsp_q7s/fs/SdCardManager.h"
-
 class LinuxLibgpioIF;
 class SerialComIF;
 class SpiComIF;
@@ -33,17 +31,14 @@ namespace ObjectFactory {
 struct CcsdsComponentArgs {
   CcsdsComponentArgs(LinuxLibgpioIF& gpioIF, StorageManagerIF& ipcStore, StorageManagerIF& tmStore,
                      PersistentTmStores& stores, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel,
-                     CcsdsIpCoreHandler** ipCoreHandler, uint32_t pdecCfgMemBaseAddr,
-                     uint32_t pdecRamBaseAddr)
+                     CcsdsIpCoreHandler** ipCoreHandler)
       : gpioComIF(gpioIF),
         ipcStore(ipcStore),
         tmStore(tmStore),
         stores(stores),
         pusFunnel(pusFunnel),
         cfdpFunnel(cfdpFunnel),
-        ipCoreHandler(ipCoreHandler),
-        pdecCfgMemBaseAddr(pdecCfgMemBaseAddr),
-        pdecRamBaseAddr(pdecRamBaseAddr) {}
+        ipCoreHandler(ipCoreHandler) {}
   LinuxLibgpioIF& gpioComIF;
   StorageManagerIF& ipcStore;
   StorageManagerIF& tmStore;
@@ -51,8 +46,6 @@ struct CcsdsComponentArgs {
   PusTmFunnel& pusFunnel;
   CfdpTmFunnel& cfdpFunnel;
   CcsdsIpCoreHandler** ipCoreHandler;
-  uint32_t pdecCfgMemBaseAddr;
-  uint32_t pdecRamBaseAddr;
   MessageQueueId_t normalLiveTmDest = MessageQueueIF::NO_QUEUE;
   MessageQueueId_t cfdpLiveTmDest = MessageQueueIF::NO_QUEUE;
 };
@@ -77,12 +70,12 @@ void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTa
                             HeaterHandler*& heaterHandler);
 void createImtqComponents(PowerSwitchIF* pwrSwitcher, bool enableHkSets, const char* i2cDev);
 void createBpxBatteryComponent(bool enableHkSets, const char* i2cDev);
-void createStrComponents(PowerSwitchIF* pwrSwitcher, SdCardManager& sdcMan);
+void createStrComponents(PowerSwitchIF* pwrSwitcher);
 void createSolarArrayDeploymentComponents(PowerSwitchIF& pwrSwitcher, GpioIF& gpioIF);
 void createSyrlinksComponents(PowerSwitchIF* pwrSwitcher);
 void createPayloadComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF& pwrSwitcher);
 void createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF* pwrSwitcher);
-ReturnValue_t createCcsdsIpComponentsWrapper(CcsdsComponentArgs& args);
+ReturnValue_t createCcsdsIpComponentsAddTmRouting(CcsdsComponentArgs& args);
 ReturnValue_t createCcsdsComponents(CcsdsComponentArgs& args);
 ReturnValue_t readFirmwareVersion();
 void createMiscComponents();

bsp_q7s/obsw.cpp

@@ -1,5 +1,6 @@
 #include "obsw.h"
 
+#include <libxiphos.h>
 #include <pwd.h>
 #include <sys/types.h>
 #include <unistd.h>
@@ -13,6 +14,7 @@
 #include "commonConfig.h"
 #include "fsfw/tasks/TaskFactory.h"
 #include "fsfw/version.h"
+#include "memory/scratchApi.h"
 #include "mission/acs/defs.h"
 #include "mission/com/defs.h"
 #include "mission/system/systemTree.h"
@@ -50,6 +52,8 @@ int obsw::obsw(int argc, char* argv[]) {
   }
 #endif
 
+  autoSwitchHandling();
+
   // Delay the boot if applicable.
   bootDelayHandling();
 
@@ -82,6 +86,33 @@ int obsw::obsw(int argc, char* argv[]) {
   return 0;
 }
 
+void obsw::autoSwitchHandling() {
+  std::string autoSwitchTarget;
+  auto switchToTarget = [&](xsc_libnor_chip_t chip, xsc_libnor_copy_t copy) {
+    sif::warning << "Detected ASWI=" << autoSwitchTarget
+                 << " in ProASIC scratch buffer, auto-switching to image " << int(chip) << " "
+                 << int(copy) << std::endl;
+    scratch::clearValue(scratch::AUTO_SWITCH_IMAGE);
+    // A bit of delay to ensure printout works..
+    TaskFactory::delayTask(500);
+    xsc_boot_copy(chip, copy);
+  };
+  if (scratch::readString(scratch::AUTO_SWITCH_IMAGE, autoSwitchTarget) == returnvalue::OK) {
+    if (autoSwitchTarget == "00") {
+      switchToTarget(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_NOMINAL);
+    } else if (autoSwitchTarget == "01") {
+      switchToTarget(XSC_LIBNOR_CHIP_0, XSC_LIBNOR_COPY_GOLD);
+    } else if (autoSwitchTarget == "10") {
+      switchToTarget(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_NOMINAL);
+    } else if (autoSwitchTarget == "11") {
+      switchToTarget(XSC_LIBNOR_CHIP_1, XSC_LIBNOR_COPY_GOLD);
+    } else {
+      sif::warning << "Invalid Auto Switch Image (ASWI) value detected: " << autoSwitchTarget
+                   << std::endl;
+    }
+  }
+}
+
 void obsw::bootDelayHandling() {
   const char* homedir = nullptr;
   homedir = getenv("HOME");

bsp_q7s/obsw.h

@@ -5,6 +5,12 @@ namespace obsw {
 
 int obsw(int argc, char* argv[]);
 
+/**
+ * This is a safety mechanism where the ProASIC scratch buffer can be used to trigger an
+ * auto-boot to another image. The auto-boot is currently implemented as a one-shot mechanism:
+ * The key-value pair which triggers the auto-boot will be removed from the scratch buffer.
+ */
+void autoSwitchHandling();
 void bootDelayHandling();
 void commandEiveSystemToSafe();
 void commandComSubsystemRxOnly();

bsp_q7s/scheduling.cpp

@@ -383,9 +383,11 @@ void scheduling::initTasks() {
   }
 #endif /* OBSW_ADD_PLOC_SUPERVISOR */
 
-  FixedTimeslotTaskIF* plTask = factory->createFixedTimeslotTask(
-      "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.5, missedDeadlineFunc);
-  pst::pstPayload(plTask);
+  PeriodicTaskIF* plTask = factory->createPeriodicTask(
+      "PL_TASK", 25, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.5, missedDeadlineFunc, &RR_SCHEDULING);
+  plTask->addComponent(objects::CAM_SWITCHER);
+  scheduling::addMpsocSupvHandlers(plTask);
+  scheduling::scheduleScexDev(plTask);
 
 #if OBSW_ADD_SCEX_DEVICE == 1
   PeriodicTaskIF* scexReaderTask;

cmake/scripts/q7s/q7s-make-debug.sh

@@ -24,8 +24,6 @@ if [ ! -z "${EIVE_Q7S_EM}" ]; then
     build_defs="EIVE_Q7S_EM=ON"
 fi
 
-build_defs="${build_defs} CMAKE_EXPORT_COMPILE_COMMANDS=ON" 
-
 os_fsfw="linux"
 tgt_bsp="arm/q7s"
 build_dir="cmake-build-debug-q7s"

cmake/scripts/q7s/q7s-make-release.sh

@@ -24,8 +24,6 @@ if [ ! -z "${EIVE_Q7S_EM}" ]; then
     build_defs="EIVE_Q7S_EM=ON"
 fi
 
-build_defs="${build_defs} CMAKE_EXPORT_COMPILE_COMMANDS=ON" 
-
 os_fsfw="linux"
 tgt_bsp="arm/q7s"
 build_dir="cmake-build-release-q7s"

common/config/eive/definitions.h

@@ -115,18 +115,6 @@ static constexpr float SCHED_BLOCK_10_PERIOD =
 
 }  // namespace spiSched
 
-namespace pdec {
-
-// Pre FW v6.0.0
-static constexpr uint32_t PDEC_CONFIG_BASE_ADDR_LEGACY = 0x24000000;
-static constexpr uint32_t PDEC_RAM_ADDR_LEGACY = 0x26000000;
-
-// Post FW v6.0.0
-static constexpr uint32_t PDEC_CONFIG_BASE_ADDR = 0x4000000;
-static constexpr uint32_t PDEC_RAM_ADDR = 0x7000000;
-
-}  // namespace pdec
-
 }  // namespace config
 
 #endif /* COMMON_CONFIG_DEFINITIONS_H_ */

dummies/helperFactory.cpp

@@ -40,8 +40,8 @@
 #include "mission/genericFactory.h"
 #include "mission/system/acs/acsModeTree.h"
 #include "mission/system/com/comModeTree.h"
-#include "mission/system/payloadModeTree.h"
 #include "mission/system/tcs/tcsModeTree.h"
+#include "mission/system/tree/payloadModeTree.h"
 #include "mission/tcs/defs.h"
 
 void dummy::createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitcher, GpioIF* gpioIF,

generators/bsp_hosted_events.csv

@@ -88,17 +88,14 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 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
 10805;0x2a35;HANDLING_CFDP_REQUEST_FAILED;LOW;CFDP request handling failed. P2: Returncode.;fsfw/src/fsfw/cfdp/handler/defs.h
 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;The limits for the rotation in safe mode were violated.;mission/acs/defs.h
-11201;0x2bc1;RATE_RECOVERY;MEDIUM;The system has recovered from a rate rotation violation.;mission/acs/defs.h
+11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;The system has recovered from a safe rate rotation violation.;mission/acs/defs.h
 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;Multiple RWs are invalid, uncommandable and therefore higher ACS modes cannot be maintained.;mission/acs/defs.h
 11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h
 11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h
 11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h
-11206;0x2bc6;PTG_CTRL_NO_ATTITUDE_INFORMATION;HIGH;For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode.;mission/acs/defs.h
+11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h
 11207;0x2bc7;SAFE_MODE_CONTROLLER_FAILURE;HIGH;The ACS safe mode controller was not able to compute a solution and has failed. P1: Missing information about magnetic field, P2: Missing information about rotational rate;mission/acs/defs.h
 11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h
-11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h
-11210;0x2bca;PTG_RATE_VIOLATION;MEDIUM;The limits for the rotation in pointing mode were violated.;mission/acs/defs.h
-11211;0x2bcb;DETUMBLE_TRANSITION_FAILED;HIGH;The detumble transition has failed. //! P1: Last detumble state before failure.;mission/acs/defs.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/power/defs.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/power/defs.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h
@@ -146,16 +143,14 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 11901;0x2e7d;BOOTING_FIRMWARE_FAILED_EVENT;LOW;Failed to boot firmware;mission/acs/str/StarTrackerHandler.h
 11902;0x2e7e;BOOTING_BOOTLOADER_FAILED_EVENT;LOW;Failed to boot star tracker into bootloader mode;mission/acs/str/StarTrackerHandler.h
 11903;0x2e7f;COM_ERROR_REPLY_RECEIVED;LOW;Received COM error. P1:  Communication Error ID (datasheet p32);mission/acs/str/StarTrackerHandler.h
-12001;0x2ee1;SUPV_MEMORY_READ_RPT_CRC_FAILURE;LOW;PLOC supervisor crc failure in telemetry packet;linux/payload/plocSupvDefs.h
-12002;0x2ee2;SUPV_UNKNOWN_TM;LOW;Unhandled event. P1: APID, P2: Service ID;linux/payload/plocSupvDefs.h
-12003;0x2ee3;SUPV_UNINIMPLEMENTED_TM;LOW;No description;linux/payload/plocSupvDefs.h
-12004;0x2ee4;SUPV_ACK_FAILURE;LOW;PLOC supervisor received acknowledgment failure report;linux/payload/plocSupvDefs.h
-12005;0x2ee5;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report P1: ID of command for which the execution failed P2: Status code sent by the supervisor handler;linux/payload/plocSupvDefs.h
-12006;0x2ee6;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;linux/payload/plocSupvDefs.h
-12007;0x2ee7;SUPV_HELPER_EXECUTING;LOW;Supervisor helper currently executing a command;linux/payload/plocSupvDefs.h
-12008;0x2ee8;SUPV_MPSOC_SHUTDOWN_BUILD_FAILED;LOW;Failed to build the command to shutdown the MPSoC;linux/payload/plocSupvDefs.h
-12009;0x2ee9;SUPV_ACK_UNKNOWN_COMMAND;LOW;Received ACK, but no related command is unknown or has not been sent  by this software instance. P1: Module APID. P2: Service ID.;linux/payload/plocSupvDefs.h
-12010;0x2eea;SUPV_EXE_ACK_UNKNOWN_COMMAND;LOW;Received ACK EXE, but no related command is unknown or has not been sent  by this software instance. P1: Module APID. P2: Service ID.;linux/payload/plocSupvDefs.h
+12001;0x2ee1;SUPV_MEMORY_READ_RPT_CRC_FAILURE;LOW;PLOC supervisor crc failure in telemetry packet;linux/payload/PlocSupervisorHandler.h
+12002;0x2ee2;SUPV_UNKNOWN_TM;LOW;Unhandled event. P1: APID, P2: Service ID;linux/payload/PlocSupervisorHandler.h
+12003;0x2ee3;SUPV_UNINIMPLEMENTED_TM;LOW;No description;linux/payload/PlocSupervisorHandler.h
+12004;0x2ee4;SUPV_ACK_FAILURE;LOW;PLOC supervisor received acknowledgment failure report;linux/payload/PlocSupervisorHandler.h
+12005;0x2ee5;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report P1: ID of command for which the execution failed P2: Status code sent by the supervisor handler;linux/payload/PlocSupervisorHandler.h
+12006;0x2ee6;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;linux/payload/PlocSupervisorHandler.h
+12007;0x2ee7;SUPV_HELPER_EXECUTING;LOW;Supervisor helper currently executing a command;linux/payload/PlocSupervisorHandler.h
+12008;0x2ee8;SUPV_MPSOC_SHUTDOWN_BUILD_FAILED;LOW;Failed to build the command to shutdown the MPSoC;linux/payload/PlocSupervisorHandler.h
 12100;0x2f44;SANITIZATION_FAILED;LOW;No description;bsp_q7s/fs/SdCardManager.h
 12101;0x2f45;MOUNTED_SD_CARD;INFO;No description;bsp_q7s/fs/SdCardManager.h
 12300;0x300c;SEND_MRAM_DUMP_FAILED;LOW;Failed to send mram dump command to supervisor handler P1: Return value of commandAction function P2: Start address of MRAM to dump with this command;linux/payload/PlocMemoryDumper.h

generators/bsp_hosted_returnvalues.csv

@@ -387,7 +387,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x4304;PUS11_InvalidRelativeTime;No description;4;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
 0x4305;PUS11_ContainedTcTooSmall;No description;5;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
 0x4306;PUS11_ContainedTcCrcMissmatch;No description;6;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
-0x4307;PUS11_MapIsFull;No description;7;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
 0x4400;FILS_GenericFileError;No description;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h
 0x4401;FILS_GenericDirError;No description;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h
 0x4402;FILS_FilesystemInactive;No description;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h
@@ -454,12 +453,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x5208;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/acs/imtqHelpers.h
 0x5209;IMTQ_StartupCfgError;No description;9;IMTQ_HANDLER;mission/acs/imtqHelpers.h
 0x520a;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.;10;IMTQ_HANDLER;mission/acs/imtqHelpers.h
-0x53a0;RWHA_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;RW_HANDLER;mission/acs/RwHandler.h
-0x53a1;RWHA_InvalidRampTime;Action Message with invalid ramp time was received.;161;RW_HANDLER;mission/acs/RwHandler.h
-0x53a2;RWHA_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;RW_HANDLER;mission/acs/RwHandler.h
-0x53a3;RWHA_ExecutionFailed;Command execution failed;163;RW_HANDLER;mission/acs/RwHandler.h
-0x53a4;RWHA_CrcError;Reaction wheel reply has invalid crc;164;RW_HANDLER;mission/acs/RwHandler.h
-0x53a5;RWHA_ValueNotRead;No description;165;RW_HANDLER;mission/acs/RwHandler.h
 0x53b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/acs/rwHelpers.h
 0x53b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/acs/rwHelpers.h
 0x53b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/acs/rwHelpers.h
@@ -493,8 +486,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x54b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;mission/acs/str/StarTrackerHandler.h
 0x54b7;STRH_StartrackerNotRunningFirmware;Star tracker must be in firmware mode to run this command;183;STR_HANDLER;mission/acs/str/StarTrackerHandler.h
 0x54b8;STRH_StartrackerNotRunningBootloader;Star tracker must be in bootloader mode to run this command;184;STR_HANDLER;mission/acs/str/StarTrackerHandler.h
-0x59a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/acs/archive/LegacySusHandler.h
-0x59a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/acs/archive/LegacySusHandler.h
+0x59a0;SUSS_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a1;SUSS_InvalidRampTime;Action Message with invalid ramp time was received.;161;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a2;SUSS_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a3;SUSS_ExecutionFailed;Command execution failed;163;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a4;SUSS_CrcError;Reaction wheel reply has invalid crc;164;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a5;SUSS_ValueNotRead;No description;165;SUS_HANDLER;mission/acs/RwHandler.h
 0x5e00;GOMS_PacketTooLong;No description;0;GOM_SPACE_HANDLER;mission/power/GomspaceDeviceHandler.h
 0x5e01;GOMS_InvalidTableId;No description;1;GOM_SPACE_HANDLER;mission/power/GomspaceDeviceHandler.h
 0x5e02;GOMS_InvalidAddress;No description;2;GOM_SPACE_HANDLER;mission/power/GomspaceDeviceHandler.h
@@ -511,11 +508,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x67a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h
 0x67a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h
 0x67a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h
-0x6aa0;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;160;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
-0x6aa1;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;161;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
-0x6aa2;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;162;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
-0x6aa3;ACSCTRL_SingleRwUnavailable;A single RW has failed.;163;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
-0x6aa4;ACSCTRL_MultipleRwUnavailable;Multiple RWs have failed.;164;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
+0x6a00;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;0;ACS_CTRL;mission/controller/AcsController.h
 0x6b02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6b03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6b04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -529,5 +522,4 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x6f00;TMS_IsBusy;No description;0;TM_SINK;mission/tmtc/DirectTmSinkIF.h
 0x6f01;TMS_PartiallyWritten;No description;1;TM_SINK;mission/tmtc/DirectTmSinkIF.h
 0x6f02;TMS_NoWriteActive;No description;2;TM_SINK;mission/tmtc/DirectTmSinkIF.h
-0x6f03;TMS_Timeout;No description;3;TM_SINK;mission/tmtc/DirectTmSinkIF.h
 0x7000;VCS_ChannelDoesNotExist;No description;0;VIRTUAL_CHANNEL;mission/com/VirtualChannel.h

generators/bsp_q7s_events.csv

@@ -88,17 +88,14 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 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
 10805;0x2a35;HANDLING_CFDP_REQUEST_FAILED;LOW;CFDP request handling failed. P2: Returncode.;fsfw/src/fsfw/cfdp/handler/defs.h
 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;The limits for the rotation in safe mode were violated.;mission/acs/defs.h
-11201;0x2bc1;RATE_RECOVERY;MEDIUM;The system has recovered from a rate rotation violation.;mission/acs/defs.h
+11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;The system has recovered from a safe rate rotation violation.;mission/acs/defs.h
 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;Multiple RWs are invalid, uncommandable and therefore higher ACS modes cannot be maintained.;mission/acs/defs.h
 11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h
 11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h
 11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h
-11206;0x2bc6;PTG_CTRL_NO_ATTITUDE_INFORMATION;HIGH;For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode.;mission/acs/defs.h
+11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h
 11207;0x2bc7;SAFE_MODE_CONTROLLER_FAILURE;HIGH;The ACS safe mode controller was not able to compute a solution and has failed. P1: Missing information about magnetic field, P2: Missing information about rotational rate;mission/acs/defs.h
 11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h
-11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h
-11210;0x2bca;PTG_RATE_VIOLATION;MEDIUM;The limits for the rotation in pointing mode were violated.;mission/acs/defs.h
-11211;0x2bcb;DETUMBLE_TRANSITION_FAILED;HIGH;The detumble transition has failed. //! P1: Last detumble state before failure.;mission/acs/defs.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/power/defs.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/power/defs.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h
@@ -146,16 +143,14 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 11901;0x2e7d;BOOTING_FIRMWARE_FAILED_EVENT;LOW;Failed to boot firmware;mission/acs/str/StarTrackerHandler.h
 11902;0x2e7e;BOOTING_BOOTLOADER_FAILED_EVENT;LOW;Failed to boot star tracker into bootloader mode;mission/acs/str/StarTrackerHandler.h
 11903;0x2e7f;COM_ERROR_REPLY_RECEIVED;LOW;Received COM error. P1:  Communication Error ID (datasheet p32);mission/acs/str/StarTrackerHandler.h
-12001;0x2ee1;SUPV_MEMORY_READ_RPT_CRC_FAILURE;LOW;PLOC supervisor crc failure in telemetry packet;linux/payload/plocSupvDefs.h
-12002;0x2ee2;SUPV_UNKNOWN_TM;LOW;Unhandled event. P1: APID, P2: Service ID;linux/payload/plocSupvDefs.h
-12003;0x2ee3;SUPV_UNINIMPLEMENTED_TM;LOW;No description;linux/payload/plocSupvDefs.h
-12004;0x2ee4;SUPV_ACK_FAILURE;LOW;PLOC supervisor received acknowledgment failure report;linux/payload/plocSupvDefs.h
-12005;0x2ee5;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report P1: ID of command for which the execution failed P2: Status code sent by the supervisor handler;linux/payload/plocSupvDefs.h
-12006;0x2ee6;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;linux/payload/plocSupvDefs.h
-12007;0x2ee7;SUPV_HELPER_EXECUTING;LOW;Supervisor helper currently executing a command;linux/payload/plocSupvDefs.h
-12008;0x2ee8;SUPV_MPSOC_SHUTDOWN_BUILD_FAILED;LOW;Failed to build the command to shutdown the MPSoC;linux/payload/plocSupvDefs.h
-12009;0x2ee9;SUPV_ACK_UNKNOWN_COMMAND;LOW;Received ACK, but no related command is unknown or has not been sent  by this software instance. P1: Module APID. P2: Service ID.;linux/payload/plocSupvDefs.h
-12010;0x2eea;SUPV_EXE_ACK_UNKNOWN_COMMAND;LOW;Received ACK EXE, but no related command is unknown or has not been sent  by this software instance. P1: Module APID. P2: Service ID.;linux/payload/plocSupvDefs.h
+12001;0x2ee1;SUPV_MEMORY_READ_RPT_CRC_FAILURE;LOW;PLOC supervisor crc failure in telemetry packet;linux/payload/PlocSupervisorHandler.h
+12002;0x2ee2;SUPV_UNKNOWN_TM;LOW;Unhandled event. P1: APID, P2: Service ID;linux/payload/PlocSupervisorHandler.h
+12003;0x2ee3;SUPV_UNINIMPLEMENTED_TM;LOW;No description;linux/payload/PlocSupervisorHandler.h
+12004;0x2ee4;SUPV_ACK_FAILURE;LOW;PLOC supervisor received acknowledgment failure report;linux/payload/PlocSupervisorHandler.h
+12005;0x2ee5;SUPV_EXE_FAILURE;LOW;PLOC received execution failure report P1: ID of command for which the execution failed P2: Status code sent by the supervisor handler;linux/payload/PlocSupervisorHandler.h
+12006;0x2ee6;SUPV_CRC_FAILURE_EVENT;LOW;PLOC supervisor reply has invalid crc;linux/payload/PlocSupervisorHandler.h
+12007;0x2ee7;SUPV_HELPER_EXECUTING;LOW;Supervisor helper currently executing a command;linux/payload/PlocSupervisorHandler.h
+12008;0x2ee8;SUPV_MPSOC_SHUTDOWN_BUILD_FAILED;LOW;Failed to build the command to shutdown the MPSoC;linux/payload/PlocSupervisorHandler.h
 12100;0x2f44;SANITIZATION_FAILED;LOW;No description;bsp_q7s/fs/SdCardManager.h
 12101;0x2f45;MOUNTED_SD_CARD;INFO;No description;bsp_q7s/fs/SdCardManager.h
 12300;0x300c;SEND_MRAM_DUMP_FAILED;LOW;Failed to send mram dump command to supervisor handler P1: Return value of commandAction function P2: Start address of MRAM to dump with this command;linux/payload/PlocMemoryDumper.h

generators/bsp_q7s_returnvalues.csv

@@ -387,7 +387,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x4304;PUS11_InvalidRelativeTime;No description;4;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
 0x4305;PUS11_ContainedTcTooSmall;No description;5;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
 0x4306;PUS11_ContainedTcCrcMissmatch;No description;6;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
-0x4307;PUS11_MapIsFull;No description;7;PUS_SERVICE_11;fsfw/src/fsfw/pus/Service11TelecommandScheduling.h
 0x4400;FILS_GenericFileError;No description;0;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h
 0x4401;FILS_GenericDirError;No description;1;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h
 0x4402;FILS_FilesystemInactive;No description;2;FILE_SYSTEM;fsfw/src/fsfw/filesystem/HasFileSystemIF.h
@@ -454,12 +453,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x5208;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/acs/imtqHelpers.h
 0x5209;IMTQ_StartupCfgError;No description;9;IMTQ_HANDLER;mission/acs/imtqHelpers.h
 0x520a;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.;10;IMTQ_HANDLER;mission/acs/imtqHelpers.h
-0x53a0;RWHA_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;RW_HANDLER;mission/acs/RwHandler.h
-0x53a1;RWHA_InvalidRampTime;Action Message with invalid ramp time was received.;161;RW_HANDLER;mission/acs/RwHandler.h
-0x53a2;RWHA_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;RW_HANDLER;mission/acs/RwHandler.h
-0x53a3;RWHA_ExecutionFailed;Command execution failed;163;RW_HANDLER;mission/acs/RwHandler.h
-0x53a4;RWHA_CrcError;Reaction wheel reply has invalid crc;164;RW_HANDLER;mission/acs/RwHandler.h
-0x53a5;RWHA_ValueNotRead;No description;165;RW_HANDLER;mission/acs/RwHandler.h
 0x53b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/acs/rwHelpers.h
 0x53b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/acs/rwHelpers.h
 0x53b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/acs/rwHelpers.h
@@ -505,8 +498,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x58a1;PLSPVhLP_ProcessTerminated;Process has been terminated by command;161;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h
 0x58a2;PLSPVhLP_PathNotExists;Received command with invalid pathname;162;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h
 0x58a3;PLSPVhLP_EventBufferReplyInvalidApid;Expected event buffer TM but received space packet with other APID;163;PLOC_SUPV_HELPER;linux/payload/PlocSupvUartMan.h
-0x59a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/acs/archive/LegacySusHandler.h
-0x59a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/acs/archive/LegacySusHandler.h
+0x59a0;SUSS_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a1;SUSS_InvalidRampTime;Action Message with invalid ramp time was received.;161;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a2;SUSS_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a3;SUSS_ExecutionFailed;Command execution failed;163;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a4;SUSS_CrcError;Reaction wheel reply has invalid crc;164;SUS_HANDLER;mission/acs/RwHandler.h
+0x59a5;SUSS_ValueNotRead;No description;165;SUS_HANDLER;mission/acs/RwHandler.h
 0x5aa0;IPCI_PapbBusy;No description;160;CCSDS_IP_CORE_BRIDGE;linux/ipcore/PapbVcInterface.h
 0x5ba0;PTME_UnknownVcId;No description;160;PTME;linux/ipcore/Ptme.h
 0x5d01;STRHLP_SdNotMounted;SD card specified in path string not mounted;1;STR_HELPER;linux/acs/StrComHandler.h
@@ -595,11 +592,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x69b5;SPVRTVIF_SupvHelperExecuting;Supervisor helper task ist currently executing a command (wait until helper tas has finished or interrupt by sending the terminate command);181;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h
 0x69c0;SPVRTVIF_BufTooSmall;No description;192;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h
 0x69c1;SPVRTVIF_NoReplyTimeout;No description;193;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h
-0x6aa0;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;160;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
-0x6aa1;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;161;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
-0x6aa2;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;162;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
-0x6aa3;ACSCTRL_SingleRwUnavailable;A single RW has failed.;163;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
-0x6aa4;ACSCTRL_MultipleRwUnavailable;Multiple RWs have failed.;164;ACS_CTRL;mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
+0x6a00;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;0;ACS_CTRL;mission/controller/AcsController.h
 0x6b02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6b03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6b04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -624,6 +617,5 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x6f00;TMS_IsBusy;No description;0;TM_SINK;mission/tmtc/DirectTmSinkIF.h
 0x6f01;TMS_PartiallyWritten;No description;1;TM_SINK;mission/tmtc/DirectTmSinkIF.h
 0x6f02;TMS_NoWriteActive;No description;2;TM_SINK;mission/tmtc/DirectTmSinkIF.h
-0x6f03;TMS_Timeout;No description;3;TM_SINK;mission/tmtc/DirectTmSinkIF.h
 0x7000;VCS_ChannelDoesNotExist;No description;0;VIRTUAL_CHANNEL;mission/com/VirtualChannel.h
 0x7200;SCBU_KeyNotFound;No description;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h

generators/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 320 translations.
+ * @brief    Auto-generated event translation file. Contains 315 translations.
  * @details
- * Generated on: 2024-02-29 13:15:00
+ * Generated on: 2023-10-27 14:24:05
  */
 #include "translateEvents.h"
 
@@ -94,17 +94,14 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
 const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
-const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
+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_RECOVERY_STRING = "MEKF_RECOVERY";
 const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
-const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
+const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
 const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
-const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
-const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
-const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
 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";
@@ -160,8 +157,6 @@ const char *SUPV_EXE_FAILURE_STRING = "SUPV_EXE_FAILURE";
 const char *SUPV_CRC_FAILURE_EVENT_STRING = "SUPV_CRC_FAILURE_EVENT";
 const char *SUPV_HELPER_EXECUTING_STRING = "SUPV_HELPER_EXECUTING";
 const char *SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUTDOWN_BUILD_FAILED";
-const char *SUPV_ACK_UNKNOWN_COMMAND_STRING = "SUPV_ACK_UNKNOWN_COMMAND";
-const char *SUPV_EXE_ACK_UNKNOWN_COMMAND_STRING = "SUPV_EXE_ACK_UNKNOWN_COMMAND";
 const char *SANITIZATION_FAILED_STRING = "SANITIZATION_FAILED";
 const char *MOUNTED_SD_CARD_STRING = "MOUNTED_SD_CARD";
 const char *SEND_MRAM_DUMP_FAILED_STRING = "SEND_MRAM_DUMP_FAILED";
@@ -507,7 +502,7 @@ const char *translateEvents(Event event) {
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
-      return RATE_RECOVERY_STRING;
+      return SAFE_RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
     case (11203):
@@ -517,17 +512,11 @@ const char *translateEvents(Event event) {
     case (11205):
       return MEKF_AUTOMATIC_RESET_STRING;
     case (11206):
-      return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
+      return MEKF_INVALID_MODE_VIOLATION_STRING;
     case (11207):
       return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11208):
       return TLE_TOO_OLD_STRING;
-    case (11209):
-      return TLE_FILE_READ_FAILED_STRING;
-    case (11210):
-      return PTG_RATE_VIOLATION_STRING;
-    case (11211):
-      return DETUMBLE_TRANSITION_FAILED_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):
@@ -638,10 +627,6 @@ const char *translateEvents(Event event) {
       return SUPV_HELPER_EXECUTING_STRING;
     case (12008):
       return SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING;
-    case (12009):
-      return SUPV_ACK_UNKNOWN_COMMAND_STRING;
-    case (12010):
-      return SUPV_EXE_ACK_UNKNOWN_COMMAND_STRING;
     case (12100):
       return SANITIZATION_FAILED_STRING;
     case (12101):

generators/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 179 translations.
- * Generated on: 2024-02-29 13:15:00
+ * Generated on: 2023-10-27 14:24:05
  */
 #include "translateObjects.h"
 

linux/ObjectFactory.cpp

@@ -24,10 +24,14 @@
 
 #include "OBSWConfig.h"
 #include "devConf.h"
+#include "devices/addresses.h"
 #include "devices/gpioIds.h"
+#include "eive/definitions.h"
 #include "mission/system/acs/acsModeTree.h"
-#include "mission/system/payloadModeTree.h"
 #include "mission/system/power/epsModeTree.h"
+#include "mission/system/tcs/tcsModeTree.h"
+#include "mission/system/tree/payloadModeTree.h"
+#include "mission/tcs/defs.h"
 
 void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiComIF,
                                               PowerSwitchIF& pwrSwitcher, std::string spiDev,
@@ -331,9 +335,8 @@ void ObjectFactory::createScexComponents(std::string uartDev, PowerSwitchIF* pwr
   scexHandler->connectModeTreeParent(satsystem::payload::SUBSYSTEM);
 }
 
-AcsController* ObjectFactory::createAcsController(bool connectSubsystem, bool enableHkSets,
-                                                  SdCardMountedIF& mountedIF) {
-  auto acsCtrl = new AcsController(objects::ACS_CONTROLLER, enableHkSets, mountedIF);
+AcsController* ObjectFactory::createAcsController(bool connectSubsystem, bool enableHkSets) {
+  auto acsCtrl = new AcsController(objects::ACS_CONTROLLER, enableHkSets);
   if (connectSubsystem) {
     acsCtrl->connectModeTreeParent(satsystem::acs::ACS_SUBSYSTEM);
   }

linux/ObjectFactory.h

@@ -31,8 +31,7 @@ void createScexComponents(std::string uartDev, PowerSwitchIF* pwrSwitcher,
 
 void gpioChecker(ReturnValue_t result, std::string output);
 
-AcsController* createAcsController(bool connectSubsystem, bool enableHkSets,
-                                   SdCardMountedIF& mountedIF);
+AcsController* createAcsController(bool connectSubsystem, bool enableHkSets);
 PowerController* createPowerController(bool connectSubsystem, bool enableHkSets);
 
 }  // namespace ObjectFactory

linux/acs/StrComHandler.cpp

@@ -54,7 +54,7 @@ ReturnValue_t StrComHandler::performOperation(uint8_t operationCode) {
     switch (state) {
       case InternalState::POLL_ONE_REPLY: {
         // Stopwatch watch;
-        replyTimeout.setTimeout(400);
+        replyTimeout.setTimeout(200);
         readOneReply(static_cast<uint32_t>(state));
         {
           MutexGuard mg(lock);
@@ -175,8 +175,7 @@ void StrComHandler::setDownloadImageName(std::string filename) {
 
 void StrComHandler::setFlashReadFilename(std::string filename) { flashRead.filename = filename; }
 
-ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname,
-                                                 startracker::FirmwareTarget target) {
+ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) {
   {
     MutexGuard mg(lock);
     if (state != InternalState::SLEEPING) {
@@ -193,13 +192,8 @@ ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname,
   if (not std::filesystem::exists(flashWrite.fullname)) {
     return FILE_NOT_EXISTS;
   }
-  if (target == startracker::FirmwareTarget::MAIN) {
-    flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_MAIN);
-    flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_MAIN);
-  } else if (target == startracker::FirmwareTarget::BACKUP) {
-    flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_BACKUP);
-    flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_BACKUP);
-  }
+  flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST);
+  flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST);
   {
     MutexGuard mg(lock);
     replyWasReceived = false;
@@ -270,7 +264,7 @@ ReturnValue_t StrComHandler::performImageDownload() {
       file.close();
       return returnvalue::OK;
     }
-    prv_arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size);
+    arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size);
     result = sendAndRead(size, downloadReq.position);
     if (result != returnvalue::OK) {
       if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
@@ -281,7 +275,7 @@ ReturnValue_t StrComHandler::performImageDownload() {
       file.close();
       return result;
     }
-    result = checkActionReply(replySize, "downloading image");
+    result = checkActionReply(replySize);
     if (result != returnvalue::OK) {
       if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
         serial::flushRxBuf(serialPort);
@@ -349,12 +343,12 @@ ReturnValue_t StrComHandler::performImageUpload() {
     }
     file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg);
     file.read(reinterpret_cast<char*>(uploadReq.data), SIZE_IMAGE_PART);
-    prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
+    arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
     result = sendAndRead(size, uploadReq.position);
     if (result != returnvalue::OK) {
       return returnvalue::FAILED;
     }
-    result = checkActionReply(replyLen, "sky image upload");
+    result = checkActionReply(replyLen);
     if (result != returnvalue::OK) {
       return result;
     }
@@ -375,12 +369,12 @@ ReturnValue_t StrComHandler::performImageUpload() {
     file.seekg(fullChunks * SIZE_IMAGE_PART, file.beg);
     file.read(reinterpret_cast<char*>(uploadReq.data), remainder);
     file.close();
-    prv_arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
+    arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
     result = sendAndRead(size, uploadReq.position);
     if (result != returnvalue::OK) {
       return returnvalue::FAILED;
     }
-    result = checkActionReply(replyLen, "sky image upload");
+    result = checkActionReply(replyLen);
     if (result != returnvalue::OK) {
       return result;
     }
@@ -394,7 +388,8 @@ ReturnValue_t StrComHandler::performImageUpload() {
 ReturnValue_t StrComHandler::performFirmwareUpdate() {
   using namespace startracker;
   ReturnValue_t result = returnvalue::OK;
-  result = unlockAndEraseRegions(flashWrite.firstRegion, flashWrite.lastRegion);
+  result = unlockAndEraseRegions(static_cast<uint32_t>(startracker::FirmwareRegions::FIRST),
+                                 static_cast<uint32_t>(startracker::FirmwareRegions::LAST));
   if (result != returnvalue::OK) {
     return result;
   }
@@ -445,12 +440,12 @@ ReturnValue_t StrComHandler::performFlashWrite() {
       bytesWrittenInRegion = 0;
     }
     req.address = bytesWrittenInRegion;
-    prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size);
+    arc_pack_write_action_req(&req, cmdBuf.data(), &size);
     result = sendAndRead(size, req.address);
     if (result != returnvalue::OK) {
       return result;
     }
-    result = checkActionReply(replyLen, "firmware image upload");
+    result = checkActionReply(replyLen);
     if (result != returnvalue::OK) {
       return result;
     }
@@ -488,12 +483,12 @@ ReturnValue_t StrComHandler::performFlashWrite() {
     req.length = remainingBytes;
     totalBytesWritten += CHUNK_SIZE;
     bytesWrittenInRegion += remainingBytes;
-    prv_arc_pack_write_action_req(&req, cmdBuf.data(), &size);
+    arc_pack_write_action_req(&req, cmdBuf.data(), &size);
     result = sendAndRead(size, req.address);
     if (result != returnvalue::OK) {
       return result;
     }
-    result = checkActionReply(replyLen, "flash write");
+    result = checkActionReply(replyLen);
     if (result != returnvalue::OK) {
       return result;
     }
@@ -536,7 +531,7 @@ ReturnValue_t StrComHandler::performFlashRead() {
     } else {
       req.length = CHUNK_SIZE;
     }
-    prv_arc_pack_read_action_req(&req, cmdBuf.data(), &size);
+    arc_pack_read_action_req(&req, cmdBuf.data(), &size);
     result = sendAndRead(size, req.address);
     if (result != returnvalue::OK) {
       if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
@@ -547,7 +542,7 @@ ReturnValue_t StrComHandler::performFlashRead() {
       file.close();
       return result;
     }
-    result = checkActionReply(replyLen, "flash read");
+    result = checkActionReply(replyLen);
     if (result != returnvalue::OK) {
       if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
         serial::flushRxBuf(serialPort);
@@ -589,7 +584,7 @@ ReturnValue_t StrComHandler::sendAndRead(size_t size, uint32_t failParameter) {
   return readOneReply(failParameter);
 }
 
-ReturnValue_t StrComHandler::checkActionReply(size_t replySize, const char* context) {
+ReturnValue_t StrComHandler::checkActionReply(size_t replySize) {
   uint8_t type = startracker::getReplyFrameType(replyPtr);
   if (type != TMTC_ACTIONREPLY) {
     sif::warning << "StrHelper::checkActionReply: Received reply with invalid type ID" << std::endl;
@@ -597,7 +592,7 @@ ReturnValue_t StrComHandler::checkActionReply(size_t replySize, const char* cont
   }
   uint8_t status = startracker::getStatusField(replyPtr);
   if (status != ArcsecDatalinkLayer::STATUS_OK) {
-    sif::warning << "StrHelper::checkActionReply: Status failure for " << context << ": "
+    sif::warning << "StrHelper::checkActionReply: Status failure: "
                  << static_cast<unsigned int>(status) << std::endl;
     return STATUS_ERROR;
   }
@@ -725,7 +720,7 @@ ReturnValue_t StrComHandler::readReceivedMessage(CookieIF* cookie, uint8_t** buf
   {
     MutexGuard mg(lock);
     if (state != InternalState::SLEEPING) {
-      return BUSY;
+      return returnvalue::OK;
     }
     replyWasReceived = this->replyWasReceived;
   }
@@ -738,7 +733,7 @@ ReturnValue_t StrComHandler::readReceivedMessage(CookieIF* cookie, uint8_t** buf
     *size = replyLen;
   }
   replyLen = 0;
-  return replyResult;
+  return returnvalue::OK;
 }
 
 ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) {
@@ -749,26 +744,23 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) {
   struct UnlockActionRequest unlockReq;
   struct EraseActionRequest eraseReq;
   uint32_t size = 0;
-  for (uint32_t idx = from; idx < to; idx++) {
+  for (uint32_t idx = from; idx <= to; idx++) {
     unlockReq.region = idx;
-    unlockReq.code = startracker::region_secrets::SECRETS[idx];
-    prv_arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size);
+    unlockReq.code = startracker::region_secrets::secret[idx];
+    arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size);
     result = sendAndRead(size, unlockReq.region);
     if (result != returnvalue::OK) {
       return result;
     }
-    result = checkActionReply(replyLen, "unlocking region");
+    result = checkActionReply(replyLen);
     if (result != returnvalue::OK) {
       sif::warning << "StrHelper::unlockAndEraseRegions: Failed to unlock region with id "
                    << static_cast<unsigned int>(unlockReq.region) << std::endl;
       return result;
     }
     eraseReq.region = idx;
-    prv_arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size);
+    arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size);
     result = sendAndRead(size, eraseReq.region);
-    if (result != returnvalue::OK) {
-    }
-    result = checkActionReply(replyLen, "erasing region");
     if (result != returnvalue::OK) {
       sif::warning << "StrHelper::unlockAndEraseRegions: Failed to erase region with id "
                    << static_cast<unsigned int>(eraseReq.region) << std::endl;

linux/acs/StrComHandler.h

@@ -6,7 +6,6 @@
 #include <string>
 
 #include "OBSWConfig.h"
-#include "mission/acs/str/strHelpers.h"
 
 #ifdef XIPHOS_Q7S
 #include "bsp_q7s/fs/SdCardManager.h"
@@ -128,7 +127,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public
    * @param fullname    Full name including absolute path of file containing firmware
    *                    update.
    */
-  ReturnValue_t startFirmwareUpdate(std::string fullname, startracker::FirmwareTarget target);
+  ReturnValue_t startFirmwareUpdate(std::string fullname);
 
   /**
    * @brief   Starts the flash read procedure
@@ -335,7 +334,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public
    *
    * @return  returnvalue::OK if reply confirms success of packet transfer, otherwise REUTRN_FAILED
    */
-  ReturnValue_t checkActionReply(size_t replySize, const char *context);
+  ReturnValue_t checkActionReply(size_t replySize);
 
   /**
    * @brief   Checks the position field in a star tracker upload/download reply.

linux/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 320 translations.
+ * @brief    Auto-generated event translation file. Contains 315 translations.
  * @details
- * Generated on: 2024-02-29 13:15:00
+ * Generated on: 2023-10-27 14:24:05
  */
 #include "translateEvents.h"
 
@@ -94,17 +94,14 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
 const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
-const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
+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_RECOVERY_STRING = "MEKF_RECOVERY";
 const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
-const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
+const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
 const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
-const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
-const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
-const char *DETUMBLE_TRANSITION_FAILED_STRING = "DETUMBLE_TRANSITION_FAILED";
 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";
@@ -160,8 +157,6 @@ const char *SUPV_EXE_FAILURE_STRING = "SUPV_EXE_FAILURE";
 const char *SUPV_CRC_FAILURE_EVENT_STRING = "SUPV_CRC_FAILURE_EVENT";
 const char *SUPV_HELPER_EXECUTING_STRING = "SUPV_HELPER_EXECUTING";
 const char *SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING = "SUPV_MPSOC_SHUTDOWN_BUILD_FAILED";
-const char *SUPV_ACK_UNKNOWN_COMMAND_STRING = "SUPV_ACK_UNKNOWN_COMMAND";
-const char *SUPV_EXE_ACK_UNKNOWN_COMMAND_STRING = "SUPV_EXE_ACK_UNKNOWN_COMMAND";
 const char *SANITIZATION_FAILED_STRING = "SANITIZATION_FAILED";
 const char *MOUNTED_SD_CARD_STRING = "MOUNTED_SD_CARD";
 const char *SEND_MRAM_DUMP_FAILED_STRING = "SEND_MRAM_DUMP_FAILED";
@@ -507,7 +502,7 @@ const char *translateEvents(Event event) {
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
-      return RATE_RECOVERY_STRING;
+      return SAFE_RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
     case (11203):
@@ -517,17 +512,11 @@ const char *translateEvents(Event event) {
     case (11205):
       return MEKF_AUTOMATIC_RESET_STRING;
     case (11206):
-      return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
+      return MEKF_INVALID_MODE_VIOLATION_STRING;
     case (11207):
       return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11208):
       return TLE_TOO_OLD_STRING;
-    case (11209):
-      return TLE_FILE_READ_FAILED_STRING;
-    case (11210):
-      return PTG_RATE_VIOLATION_STRING;
-    case (11211):
-      return DETUMBLE_TRANSITION_FAILED_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):
@@ -638,10 +627,6 @@ const char *translateEvents(Event event) {
       return SUPV_HELPER_EXECUTING_STRING;
     case (12008):
       return SUPV_MPSOC_SHUTDOWN_BUILD_FAILED_STRING;
-    case (12009):
-      return SUPV_ACK_UNKNOWN_COMMAND_STRING;
-    case (12010):
-      return SUPV_EXE_ACK_UNKNOWN_COMMAND_STRING;
     case (12100):
       return SANITIZATION_FAILED_STRING;
     case (12101):

linux/fsfwconfig/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 179 translations.
- * Generated on: 2024-02-29 13:15:00
+ * Generated on: 2023-10-27 14:24:05
  */
 #include "translateObjects.h"
 

linux/ipcore/PapbVcInterface.cpp

@@ -30,7 +30,6 @@ ReturnValue_t PapbVcInterface::initialize() {
 ReturnValue_t PapbVcInterface::write(const uint8_t* data, size_t size, size_t& writtenSize) {
   // There are no packets smaller than 4, this is considered a configuration error.
   if (size < 4) {
-    sif::warning << "PapbVcInterface::write: Passed packet smaller than 4 bytes" << std::endl;
     return returnvalue::FAILED;
   }
   // The user must call advance until completion before starting a new packet transfer.
@@ -84,9 +83,6 @@ ReturnValue_t PapbVcInterface::advanceWrite(size_t& writtenSize) {
     writtenSize++;
   }
   if (not pollReadyForOctet(MAX_BUSY_POLLS)) {
-    if (not pollReadyForPacket()) {
-      return PARTIALLY_WRITTEN;
-    }
     abortPacketTransfer();
     return returnvalue::FAILED;
   }

linux/ipcore/PdecHandler.cpp

@@ -24,15 +24,12 @@ using namespace pdec;
 uint32_t PdecHandler::CURRENT_FAR = 0;
 
 PdecHandler::PdecHandler(object_id_t objectId, object_id_t tcDestinationId,
-                         LinuxLibgpioIF* gpioComIF, gpioId_t pdecReset, UioNames names,
-                         uint32_t cfgMemPhyAddr, uint32_t pdecRamPhyAddr)
+                         LinuxLibgpioIF* gpioComIF, gpioId_t pdecReset, UioNames names)
     : SystemObject(objectId),
       tcDestinationId(tcDestinationId),
       gpioComIF(gpioComIF),
       pdecReset(pdecReset),
       actionHelper(this, nullptr),
-      cfgMemBaseAddr(cfgMemPhyAddr),
-      pdecRamBaseAddr(pdecRamPhyAddr),
       uioNames(names),
       paramHelper(this) {
   auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
@@ -70,7 +67,7 @@ ReturnValue_t PdecHandler::initialize() {
   };
   memoryBaseAddress = static_cast<uint32_t*>(
       mmap(0, PDEC_CFG_MEM_SIZE, static_cast<int>(UioMapper::Permissions::READ_WRITE), MAP_SHARED,
-           fd, cfgMemBaseAddr));
+           fd, PDEC_CFG_MEM_PHY_ADDR));
   if (memoryBaseAddress == nullptr) {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
@@ -78,7 +75,7 @@ ReturnValue_t PdecHandler::initialize() {
 
   ramBaseAddress = static_cast<uint32_t*>(mmap(0, PDEC_RAM_SIZE,
                                                static_cast<int>(UioMapper::Permissions::READ_WRITE),
-                                               MAP_SHARED, fd, pdecRamBaseAddr));
+                                               MAP_SHARED, fd, PDEC_RAM_PHY_ADDR));
   if (ramBaseAddress == nullptr) {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
@@ -468,7 +465,14 @@ bool PdecHandler::newTcReceived() {
   return true;
 }
 
-void PdecHandler::doPeriodicWork() { checkLocks(); }
+void PdecHandler::doPeriodicWork() {
+  // scuffed test code
+  //  if(testCntr < 30) {
+  //    triggerEvent(pdec::INVALID_TC_FRAME, FRAME_DIRTY_RETVAL);
+  //    testCntr++;
+  //  }
+  checkLocks();
+}
 
 bool PdecHandler::checkFrameAna(uint32_t pdecFar) {
   bool frameValid = false;
@@ -641,7 +645,7 @@ void PdecHandler::handleNewTc() {
 }
 
 ReturnValue_t PdecHandler::readTc(uint32_t& tcLength) {
-  uint32_t tcOffset = (*(registerBaseAddress + PDEC_BPTR_OFFSET) - pdecRamBaseAddr) / 4;
+  uint32_t tcOffset = (*(registerBaseAddress + PDEC_BPTR_OFFSET) - PHYSICAL_RAM_BASE_ADDRESS) / 4;
 
 #if OBSW_DEBUG_PDEC_HANDLER == 1
   sif::debug << "PdecHandler::readTc: TC offset: 0x" << std::hex << tcOffset << std::endl;

linux/ipcore/PdecHandler.h

@@ -52,7 +52,9 @@ class PdecHandler : public SystemObject,
  public:
   static constexpr dur_millis_t IRQ_TIMEOUT_MS = 500;
   static constexpr uint32_t PDEC_CFG_MEM_SIZE = 0x1000;
+  static constexpr uint32_t PDEC_CFG_MEM_PHY_ADDR = 0x24000000;
   static constexpr uint32_t PDEC_RAM_SIZE = 0x10000;
+  static constexpr uint32_t PDEC_RAM_PHY_ADDR = 0x26000000;
 
   enum class Modes { POLLED, IRQ };
 
@@ -66,7 +68,7 @@ class PdecHandler : public SystemObject,
    * @param uioregsiters  String of uio device file same mapped to the PDEC register space
    */
   PdecHandler(object_id_t objectId, object_id_t tcDestinationId, LinuxLibgpioIF* gpioComIF,
-              gpioId_t pdecReset, UioNames names, uint32_t cfgMemPhyAddr, uint32_t pdecRamPhyAddr);
+              gpioId_t pdecReset, UioNames names);
 
   virtual ~PdecHandler();
 
@@ -101,6 +103,12 @@ class PdecHandler : public SystemObject,
   static const size_t MAX_TC_SEGMENT_SIZE = 1017;
   static const uint8_t MAP_ID_MASK = 0x3F;
 
+#ifdef TE0720_1CFA
+  static const uint32_t PHYSICAL_RAM_BASE_ADDRESS = 0x32000000;
+#else
+  static const uint32_t PHYSICAL_RAM_BASE_ADDRESS = 0x26000000;
+#endif
+
   // Expected value stored in FAR register after reset
   static const uint32_t FAR_RESET = 0x7FE0;
 
@@ -187,9 +195,6 @@ class PdecHandler : public SystemObject,
   MessageQueueId_t commandedBy = MessageQueueIF::NO_QUEUE;
   bool ptmeResetWithReinitializationPending = false;
 
-  uint32_t cfgMemBaseAddr;
-  uint32_t pdecRamBaseAddr;
-
   UioNames uioNames;
 
   ParameterHelper paramHelper;

linux/payload/CMakeLists.txt

@@ -2,9 +2,9 @@ target_sources(
   ${OBSW_NAME}
   PUBLIC PlocMemoryDumper.cpp
          PlocMpsocHandler.cpp
-         FreshSupvHandler.cpp
          PlocMpsocSpecialComHelper.cpp
          plocMpsocHelpers.cpp
+         PlocSupervisorHandler.cpp
          PlocSupvUartMan.cpp
          ScexDleParser.cpp
          ScexHelper.cpp

linux/payload/FreshSupvHandler.h

@@ -1,188 +0,0 @@
-#ifndef LINUX_PAYLOAD_FRESHSUPVHANDLER_H_
-#define LINUX_PAYLOAD_FRESHSUPVHANDLER_H_
-
-#include <fsfw/power/PowerSwitchIF.h>
-#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
-
-#include <map>
-
-#include "PlocSupvUartMan.h"
-#include "fsfw/devicehandlers/FreshDeviceHandlerBase.h"
-#include "fsfw/power/definitions.h"
-#include "fsfw_hal/linux/gpio/Gpio.h"
-#include "plocSupvDefs.h"
-
-using supv::TcBase;
-
-class FreshSupvHandler : public FreshDeviceHandlerBase {
- public:
-  enum OpCode { DEFAULT_OPERATION = 0, PARSE_TM = 1 };
-
-  FreshSupvHandler(DhbConfig cfg, CookieIF* comCookie, Gpio uartIsolatorSwitch,
-                   PowerSwitchIF& switchIF, power::Switch_t powerSwitch);
-  /**
-   * Periodic helper executed function, implemented by child class.
-   */
-  void performDeviceOperation(uint8_t opCode) override;
-
-  /**
-   * Implemented by child class. Handle all command messages which are
-   * not health, mode, action or housekeeping messages.
-   * @param message
-   * @return
-   */
-  ReturnValue_t handleCommandMessage(CommandMessage* message) override;
-
-  ReturnValue_t initialize() override;
-
- private:
-  // HK manager abstract functions.
-  LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
-  ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
-                                        LocalDataPoolManager& poolManager) override;
-
-  // Mode abstract functions
-  ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
-                                 uint32_t* msToReachTheMode) override;
-  // Action override. Forward to user.
-  ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
-                              const uint8_t* data, size_t size) override;
-
-  /**
-   * @overload
-   * @param submode
-   */
-  void startTransition(Mode_t newMode, Submode_t submode) override;
-
-  ReturnValue_t performDeviceOperationPreQueueHandling(uint8_t opCode) override;
-  void handleTransitionToOn();
-  void handleTransitionToOff();
-
- private:
-  static constexpr bool SET_TIME_DURING_BOOT = true;
-  static const uint8_t SIZE_NULL_TERMINATOR = 1;
-
-  enum class StartupState : uint8_t {
-    IDLE,
-    POWER_SWITCHING,
-    BOOTING,
-    SET_TIME,
-    WAIT_FOR_TIME_REPLY,
-    TIME_WAS_SET,
-    ON
-  };
-
-  StartupState startupState = StartupState::IDLE;
-  MessageQueueIF* eventQueue = nullptr;
-  supv::TmBase tmReader;
-
-  enum class ShutdownState : uint8_t { IDLE, POWER_SWITCHING };
-  ShutdownState shutdownState = ShutdownState::IDLE;
-
-  PlocSupvUartManager* uartManager;
-  CookieIF* comCookie;
-  PowerSwitchIF& switchIF;
-  power::Switch_t switchId;
-  Gpio uartIsolatorSwitch;
-
-  supv::HkSet hkSet;
-  supv::BootStatusReport bootStatusReport;
-  supv::LatchupStatusReport latchupStatusReport;
-  supv::CountersReport countersReport;
-  supv::AdcReport adcReport;
-
-  bool transitionActive = false;
-
-  Mode_t targetMode = HasModesIF::MODE_INVALID;
-  Submode_t targetSubmode = 0;
-
-  Countdown switchTimeout = Countdown(2000);
-  // Vorago nees some time to boot properly
-  Countdown bootTimeout = Countdown(supv::BOOT_TIMEOUT_MS);
-  // Countdown interCmdCd = Countdown(supv::INTER_COMMAND_DELAY);
-
-  PoolEntry<uint16_t> adcRawEntry = PoolEntry<uint16_t>(16);
-  PoolEntry<uint16_t> adcEngEntry = PoolEntry<uint16_t>(16);
-  PoolEntry<uint32_t> latchupCounters = PoolEntry<uint32_t>(7);
-  PoolEntry<uint8_t> fmcStateEntry = PoolEntry<uint8_t>(1);
-  PoolEntry<uint8_t> bootStateEntry = PoolEntry<uint8_t>(1);
-  PoolEntry<uint8_t> bootCyclesEntry = PoolEntry<uint8_t>(1);
-  PoolEntry<uint32_t> tempSupEntry = PoolEntry<uint32_t>(1);
-
-  pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
-
-  struct ActiveCmdInfo {
-    ActiveCmdInfo(DeviceCommandId_t commandId, uint32_t cmdCountdownMs)
-        : commandId(commandId), cmdCountdown(cmdCountdownMs) {}
-
-    DeviceCommandId_t commandId = DeviceHandlerIF::NO_COMMAND_ID;
-    bool isPending = false;
-    bool ackRecv = false;
-    bool ackExeRecv = false;
-    bool replyPacketExpected = false;
-    bool replyPacketReceived = false;
-    MessageQueueId_t commandedBy = MessageQueueIF::NO_QUEUE;
-    bool requiresActionReply = false;
-    Countdown cmdCountdown;
-  };
-
-  uint32_t buildActiveCmdKey(uint16_t moduleApid, uint8_t serviceId);
-
-  // Map for Action commands. For normal commands, a separate static structure will be used.
-  std::map<uint32_t, ActiveCmdInfo> activeActionCmds;
-
-  std::array<uint8_t, supv::MAX_COMMAND_SIZE> commandBuffer{};
-  SpacePacketCreator creator;
-  supv::TcParams spParams = supv::TcParams(creator);
-  DeviceCommandId_t commandedByCached = MessageQueueIF::NO_QUEUE;
-
-  ReturnValue_t parseTmPackets();
-
-  ReturnValue_t sendCommand(DeviceCommandId_t commandId, TcBase& tc, bool replyPacketExpected,
-                            uint32_t cmdCountdownMs = 1000);
-  ReturnValue_t sendEmptyCmd(DeviceCommandId_t commandId, uint16_t apid, uint8_t serviceId,
-                             bool replyPacketExpected);
-  ReturnValue_t prepareSelBootImageCmd(const uint8_t* commandData);
-  ReturnValue_t prepareSetTimeRefCmd();
-  ReturnValue_t prepareSetBootTimeoutCmd(const uint8_t* commandData, size_t cmdDataLen);
-  ReturnValue_t prepareRestartTriesCmd(const uint8_t* commandData, size_t cmdDataLen);
-  ReturnValue_t prepareDisableHk();
-  ReturnValue_t prepareLatchupConfigCmd(const uint8_t* commandData, DeviceCommandId_t deviceCommand,
-                                        size_t cmdDataLen);
-  ReturnValue_t prepareSetAlertLimitCmd(const uint8_t* commandData, size_t cmdDataLen);
-  ReturnValue_t prepareFactoryResetCmd(const uint8_t* commandData, size_t len);
-  ReturnValue_t prepareSetShutdownTimeoutCmd(const uint8_t* commandData, size_t cmdDataLen);
-  ReturnValue_t prepareSetGpioCmd(const uint8_t* commandData, size_t commandDataLen);
-  ReturnValue_t prepareReadGpioCmd(const uint8_t* commandData, size_t commandDataLen);
-  ReturnValue_t prepareSetAdcEnabledChannelsCmd(const uint8_t* commandData);
-  ReturnValue_t prepareSetAdcWindowAndStrideCmd(const uint8_t* commandData);
-  ReturnValue_t prepareSetAdcThresholdCmd(const uint8_t* commandData);
-  ReturnValue_t prepareWipeMramCmd(const uint8_t* commandData, size_t cmdDataLen);
-  ReturnValue_t extractUpdateCommand(const uint8_t* commandData, size_t size,
-                                     supv::UpdateParams& params);
-  ReturnValue_t extractBaseParams(const uint8_t** commandData, size_t& remSize,
-                                  supv::UpdateParams& params);
-  void handleEvent(EventMessage* eventMessage);
-
-  void handleBadApidServiceCombination(Event event, unsigned int apid, unsigned int serviceId);
-  ReturnValue_t eventSubscription();
-  void handlePacketPrint();
-  bool isCommandAlreadyActive(ActionId_t actionId) const;
-  ReturnValue_t handleAckReport(const uint8_t* data);
-  void printAckFailureInfo(uint16_t statusCode, DeviceCommandId_t commandId);
-  ReturnValue_t handleExecutionReport(const uint8_t* data);
-  ReturnValue_t handleExecutionSuccessReport(ActiveCmdInfo& info, supv::ExecutionReport& report);
-  void handleExecutionFailureReport(ActiveCmdInfo& info, supv::ExecutionReport& report);
-  ReturnValue_t handleHkReport(const uint8_t* data);
-  ReturnValue_t verifyPacket(const uint8_t* start, size_t foundLen);
-  void confirmReplyPacketReceived(supv::Apid apid, uint8_t serviceId);
-  void performCommandCompletionHandling(supv::Apid apid, uint8_t serviceId, ActiveCmdInfo& info);
-  ReturnValue_t handleBootStatusReport(const uint8_t* data);
-  ReturnValue_t genericHandleTm(const char* contextString, const uint8_t* data,
-                                LocalPoolDataSetBase& set, supv::Apid apid, uint8_t serviceId);
-  ReturnValue_t handleLatchupStatusReport(const uint8_t* data);
-
-  bool isCommandPending() const;
-};
-
-#endif /* LINUX_PAYLOAD_FRESHSUPVHANDLER_H_ */

linux/payload/PlocMpsocHandler.cpp

@@ -7,7 +7,6 @@
 #include "OBSWConfig.h"
 #include "fsfw/datapool/PoolReadGuard.h"
 #include "fsfw/globalfunctions/CRC.h"
-#include "fsfw/parameters/HasParametersIF.h"
 
 PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid,
                                    CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper,
@@ -21,8 +20,9 @@ PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid
   if (comCookie == nullptr) {
     sif::error << "PlocMPSoCHandler: Invalid communication cookie" << std::endl;
   }
-  eventQueue = QueueFactory::instance()->createMessageQueue(10);
-  commandActionHelperQueue = QueueFactory::instance()->createMessageQueue(10);
+  eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5);
+  commandActionHelperQueue =
+      QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5);
   spParams.maxSize = sizeof(commandBuffer);
   spParams.buf = commandBuffer;
 }
@@ -998,7 +998,6 @@ ReturnValue_t PlocMpsocHandler::handleGetHkReport(const uint8_t* data) {
   if (result != returnvalue::OK) {
     return result;
   }
-  hkReport.setValidity(true, true);
   return returnvalue::OK;
 }
 
@@ -1397,18 +1396,14 @@ bool PlocMpsocHandler::handleHwStartup() {
   return true;
 #endif
   if (powerState == PowerState::IDLE) {
-    if (skipSupvCommandingToOn) {
-      powerState = PowerState::DONE;
+    if (supv::SUPV_ON) {
+      commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
+      supvTransitionCd.resetTimer();
+      powerState = PowerState::PENDING_STARTUP;
     } else {
-      if (supv::SUPV_ON) {
-        commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
-        supvTransitionCd.resetTimer();
-        powerState = PowerState::PENDING_STARTUP;
-      } else {
-        triggerEvent(SUPV_NOT_ON, 1);
-        // Set back to OFF for now, failing the transition.
-        setMode(MODE_OFF);
-      }
+      triggerEvent(SUPV_NOT_ON, 1);
+      // Set back to OFF for now, failing the transition.
+      setMode(MODE_OFF);
     }
   }
   if (powerState == PowerState::SUPV_FAILED) {
@@ -1538,20 +1533,3 @@ ReturnValue_t PlocMpsocHandler::checkModeCommand(Mode_t commandedMode, Submode_t
   }
   return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
 }
-
-ReturnValue_t PlocMpsocHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
-                                             ParameterWrapper* parameterWrapper,
-                                             const ParameterWrapper* newValues,
-                                             uint16_t startAtIndex) {
-  if (uniqueId == mpsoc::ParamId::SKIP_SUPV_ON_COMMANDING) {
-    uint8_t value = 0;
-    newValues->getElement(&value);
-    if (value > 1) {
-      return HasParametersIF::INVALID_VALUE;
-    }
-    parameterWrapper->set(skipSupvCommandingToOn);
-    return returnvalue::OK;
-  }
-  return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
-                                         startAtIndex);
-}

linux/payload/PlocMpsocHandler.h

@@ -201,8 +201,6 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
 
   PowerState powerState = PowerState::IDLE;
 
-  uint8_t skipSupvCommandingToOn = false;
-
   /**
    * @brief   Handles events received from the PLOC MPSoC helper
    */
@@ -318,9 +316,6 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
   pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
   ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
                                  uint32_t* msToReachTheMode) override;
-
-  ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
-                             const ParameterWrapper* newValues, uint16_t startAtIndex) override;
 };
 
 #endif /* BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ */

linux/payload/PlocMpsocSpecialComHelper.cpp

@@ -504,7 +504,7 @@ ReturnValue_t PlocMpsocSpecialComHelper::checkReceivedTm() {
     triggerEvent(MPSOC_TM_SIZE_ERROR);
     return result;
   }
-  result = spReader.checkCrc();
+  spReader.checkCrc();
   if (result != returnvalue::OK) {
     sif::warning << "PLOC MPSoC: CRC check failed" << std::endl;
     triggerEvent(MPSOC_TM_CRC_MISSMATCH, *sequenceCount);

linux/payload/PlocSupervisorHandler.cpp

@@ -19,6 +19,8 @@
 using namespace supv;
 using namespace returnvalue;
 
+std::atomic_bool supv::SUPV_ON = false;
+
 PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, CookieIF* comCookie,
                                              Gpio uartIsolatorSwitch, power::Switch_t powerSwitch,
                                              PlocSupvUartManager& supvHelper)
@@ -27,7 +29,7 @@ PlocSupervisorHandler::PlocSupervisorHandler(object_id_t objectId, CookieIF* com
       hkset(this),
       bootStatusReport(this),
       latchupStatusReport(this),
-      countersReport(this),
+      loggingReport(this),
       adcReport(this),
       powerSwitch(powerSwitch),
       uartManager(supvHelper) {
@@ -59,19 +61,6 @@ ReturnValue_t PlocSupervisorHandler::initialize() {
 }
 
 void PlocSupervisorHandler::performOperationHook() {
-  if (normalCommandIsPending and normalCmdCd.hasTimedOut()) {
-    // Event, FDIR, printout? Leads to spam though and normally should not happen..
-    normalCommandIsPending = false;
-  }
-  if (commandIsPending and cmdCd.hasTimedOut()) {
-    // Event, FDIR, printout? Leads to spam though and normally should not happen..
-    commandIsPending = false;
-
-    //    if(iter->second.sendReplyTo != NO_COMMANDER) {
-    //      actionHelper.finish(true, iter->second.sendReplyTo, iter->first, returnvalue::OK);
-    //    }
-    disableAllReplies();
-  }
   EventMessage event;
   for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
        result = eventQueue->receiveMessage(&event)) {
@@ -183,16 +172,13 @@ void PlocSupervisorHandler::doShutDown() {
   nextReplyId = supv::NONE;
   uartManager.stop();
   uartIsolatorSwitch.pullLow();
-  disableAllReplies();
   supv::SUPV_ON = false;
   startupState = StartupState::OFF;
 }
 
 ReturnValue_t PlocSupervisorHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
-  if (not normalCommandIsPending) {
+  if (not commandIsExecuting(GET_HK_REPORT)) {
     *id = GET_HK_REPORT;
-    normalCommandIsPending = true;
-    normalCmdCd.resetTimer();
     return buildCommandFromCommand(*id, nullptr, 0);
   }
   return NOTHING_TO_SEND;
@@ -288,7 +274,8 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d
       break;
     }
     case SET_GPIO: {
-      result = prepareSetGpioCmd(commandData, commandDataLen);
+      prepareSetGpioCmd(commandData);
+      result = returnvalue::OK;
       break;
     }
     case FACTORY_RESET: {
@@ -296,7 +283,8 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d
       break;
     }
     case READ_GPIO: {
-      result = prepareReadGpioCmd(commandData, commandDataLen);
+      prepareReadGpioCmd(commandData);
+      result = returnvalue::OK;
       break;
     }
     case SET_SHUTDOWN_TIMEOUT: {
@@ -333,25 +321,12 @@ ReturnValue_t PlocSupervisorHandler::buildCommandFromCommand(DeviceCommandId_t d
       result = prepareWipeMramCmd(commandData);
       break;
     }
-    case REQUEST_ADC_REPORT: {
-      prepareEmptyCmd(Apid::ADC_MON, static_cast<uint8_t>(tc::AdcMonId::REQUEST_ADC_SAMPLE));
-      result = returnvalue::OK;
-      break;
-    }
-    case REQUEST_LOGGING_COUNTERS: {
-      prepareEmptyCmd(Apid::DATA_LOGGER,
-                      static_cast<uint8_t>(tc::DataLoggerServiceId::REQUEST_COUNTERS));
-      result = returnvalue::OK;
-      break;
-    }
     default:
       sif::debug << "PlocSupervisorHandler::buildCommandFromCommand: Command not implemented"
                  << std::endl;
       result = DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
       break;
   }
-  commandIsPending = true;
-  cmdCd.resetTimer();
   return result;
 }
 
@@ -383,8 +358,6 @@ void PlocSupervisorHandler::fillCommandAndReplyMap() {
   insertInCommandMap(SET_ADC_THRESHOLD);
   insertInCommandMap(SET_ADC_WINDOW_AND_STRIDE);
   insertInCommandMap(RESET_PL);
-  insertInCommandMap(REQUEST_ADC_REPORT);
-  insertInCommandMap(REQUEST_LOGGING_COUNTERS);
 
   // ACK replies, use countdown for them
   insertInReplyMap(ACK_REPORT, 0, nullptr, SIZE_ACK_REPORT, false, &acknowledgementReportTimeout);
@@ -395,7 +368,7 @@ void PlocSupervisorHandler::fillCommandAndReplyMap() {
   insertInReplyMap(HK_REPORT, 3, &hkset);
   insertInReplyMap(BOOT_STATUS_REPORT, 3, &bootStatusReport, SIZE_BOOT_STATUS_REPORT);
   insertInReplyMap(LATCHUP_REPORT, 3, &latchupStatusReport, SIZE_LATCHUP_STATUS_REPORT);
-  insertInReplyMap(COUNTERS_REPORT, 3, &countersReport, SIZE_COUNTERS_REPORT);
+  insertInReplyMap(LOGGING_REPORT, 3, &loggingReport, SIZE_LOGGING_REPORT);
   insertInReplyMap(ADC_REPORT, 3, &adcReport, SIZE_ADC_REPORT);
 }
 
@@ -437,13 +410,13 @@ ReturnValue_t PlocSupervisorHandler::enableReplyInReplyMap(DeviceCommandMap::ite
       }
       break;
     }
-    case REQUEST_LOGGING_COUNTERS: {
+    case LOGGING_REQUEST_COUNTERS: {
       enabledReplies = 3;
       result =
-          DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, COUNTERS_REPORT);
+          DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, LOGGING_REPORT);
       if (result != returnvalue::OK) {
         sif::debug << "PlocSupervisorHandler::enableReplyInReplyMap: Reply with id "
-                   << COUNTERS_REPORT << " not in replyMap" << std::endl;
+                   << LOGGING_REPORT << " not in replyMap" << std::endl;
       }
       break;
     }
@@ -568,9 +541,6 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r
     }
     case (Apid::HK): {
       if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::HkId::REPORT)) {
-        normalCommandIsPending = false;
-        // Yeah apparently this is needed??
-        disableCommand(GET_HK_REPORT);
         *foundLen = tmReader.getFullPacketLen();
         *foundId = ReplyId::HK_REPORT;
         return OK;
@@ -589,14 +559,6 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r
       }
       break;
     }
-    case (Apid::ADC_MON): {
-      if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::AdcMonId::ADC_REPORT)) {
-        *foundLen = tmReader.getFullPacketLen();
-        *foundId = ReplyId::ADC_REPORT;
-        return OK;
-      }
-      break;
-    }
     case (Apid::MEM_MAN): {
       if (tmReader.getServiceId() ==
           static_cast<uint8_t>(supv::tm::MemManId::UPDATE_STATUS_REPORT)) {
@@ -604,15 +566,6 @@ ReturnValue_t PlocSupervisorHandler::scanForReply(const uint8_t* start, size_t r
         *foundId = ReplyId::UPDATE_STATUS_REPORT;
         return OK;
       }
-      break;
-    }
-    case (Apid::DATA_LOGGER): {
-      if (tmReader.getServiceId() ==
-          static_cast<uint8_t>(supv::tm::DataLoggerId::COUNTERS_REPORT)) {
-        *foundLen = tmReader.getFullPacketLen();
-        *foundId = ReplyId::COUNTERS_REPORT;
-        return OK;
-      }
     }
   }
   handleBadApidServiceCombination(SUPV_UNKNOWN_TM, apid, tmReader.getServiceId());
@@ -625,14 +578,6 @@ void PlocSupervisorHandler::handlePacketPrint() {
     if ((tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::ACK)) or
         (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::NAK))) {
       AcknowledgmentReport ack(tmReader);
-      ReturnValue_t result = ack.parse();
-      if (result != returnvalue::OK) {
-        sif::warning << "PlocSupervisorHandler: Parsing ACK failed" << std::endl;
-      }
-      if (REDUCE_NORMAL_MODE_PRINTOUT and ack.getRefModuleApid() == (uint8_t)supv::Apid::HK and
-          ack.getRefServiceId() == (uint8_t)supv::tc::HkId::GET_REPORT) {
-        return;
-      }
       const char* printStr = "???";
       if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::ACK)) {
         printStr = "ACK";
@@ -647,15 +592,7 @@ void PlocSupervisorHandler::handlePacketPrint() {
     } else if ((tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)) or
                (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK))) {
       ExecutionReport exe(tmReader);
-      ReturnValue_t result = exe.parse();
-      if (result != returnvalue::OK) {
-        sif::warning << "PlocSupervisorHandler: Parsing EXE failed" << std::endl;
-      }
       const char* printStr = "???";
-      if (REDUCE_NORMAL_MODE_PRINTOUT and exe.getRefModuleApid() == (uint8_t)supv::Apid::HK and
-          exe.getRefServiceId() == (uint8_t)supv::tc::HkId::GET_REPORT) {
-        return;
-      }
       if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)) {
         printStr = "ACK EXE";
 
@@ -690,22 +627,12 @@ ReturnValue_t PlocSupervisorHandler::interpretDeviceReply(DeviceCommandId_t id,
       result = handleBootStatusReport(packet);
       break;
     }
-    case (COUNTERS_REPORT): {
-      result = genericHandleTm("COUNTERS", packet, countersReport);
-#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1
-      countersReport.printSet();
-#endif
-      break;
-    }
     case (LATCHUP_REPORT): {
       result = handleLatchupStatusReport(packet);
       break;
     }
     case (ADC_REPORT): {
-      result = genericHandleTm("ADC", packet, adcReport);
-#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1
-      adcReport.printSet();
-#endif
+      result = handleAdcReport(packet);
       break;
     }
     case (EXE_REPORT): {
@@ -770,8 +697,13 @@ ReturnValue_t PlocSupervisorHandler::initializeLocalDataPool(localpool::DataPool
   localDataPoolMap.emplace(supv::LATCHUP_RPT_TIME_YEAR, new PoolEntry<uint8_t>({0}));
   localDataPoolMap.emplace(supv::LATCHUP_RPT_IS_SET, new PoolEntry<uint8_t>({0}));
 
-  localDataPoolMap.emplace(supv::SIGNATURE, new PoolEntry<uint32_t>());
-  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNTS, &latchupCounters);
+  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_0, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_1, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_2, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_3, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_4, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_5, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(supv::LATCHUP_HAPPENED_CNT_6, new PoolEntry<uint32_t>({0}));
   localDataPoolMap.emplace(supv::ADC_DEVIATION_TRIGGERS_CNT, new PoolEntry<uint32_t>({0}));
   localDataPoolMap.emplace(supv::TC_RECEIVED_CNT, new PoolEntry<uint32_t>({0}));
   localDataPoolMap.emplace(supv::TM_AVAILABLE_CNT, new PoolEntry<uint32_t>({0}));
@@ -780,22 +712,41 @@ ReturnValue_t PlocSupervisorHandler::initializeLocalDataPool(localpool::DataPool
   localDataPoolMap.emplace(supv::MPSOC_BOOT_FAILED_ATTEMPTS, new PoolEntry<uint32_t>({0}));
   localDataPoolMap.emplace(supv::MPSOC_POWER_UP, new PoolEntry<uint32_t>({0}));
   localDataPoolMap.emplace(supv::MPSOC_UPDATES, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::MPSOC_HEARTBEAT_RESETS, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::CPU_WDT_RESETS, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::PS_HEARTBEATS_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::PL_HEARTBEATS_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::EB_TASK_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::BM_TASK_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::LM_TASK_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::AM_TASK_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::TCTMM_TASK_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::MM_TASK_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::HK_TASK_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::DL_TASK_LOST, new PoolEntry<uint32_t>({0}));
-  localDataPoolMap.emplace(supv::RWS_TASKS_LOST, new PoolEntry<uint32_t>(3));
+  localDataPoolMap.emplace(supv::LAST_RECVD_TC, new PoolEntry<uint32_t>({0}));
 
-  localDataPoolMap.emplace(supv::ADC_RAW, &adcRawEntry);
-  localDataPoolMap.emplace(supv::ADC_ENG, &adcEngEntry);
+  localDataPoolMap.emplace(supv::ADC_RAW_0, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_1, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_2, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_3, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_4, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_5, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_6, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_7, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_8, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_9, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_10, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_11, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_12, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_13, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_14, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_RAW_15, new PoolEntry<uint16_t>({0}));
+
+  localDataPoolMap.emplace(supv::ADC_ENG_0, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_1, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_2, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_3, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_4, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_5, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_6, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_7, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_8, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_9, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_10, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_11, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_12, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_13, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_14, new PoolEntry<uint16_t>({0}));
+  localDataPoolMap.emplace(supv::ADC_ENG_15, new PoolEntry<uint16_t>({0}));
 
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(hkset.getSid(), false, 10.0));
@@ -920,7 +871,6 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionReport(const uint8_t* data)
   } else if (tmReader.getServiceId() == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)) {
     handleExecutionFailureReport(report);
   }
-  commandIsPending = false;
   nextReplyId = supv::NONE;
   return result;
 }
@@ -1155,31 +1105,37 @@ ReturnValue_t PlocSupervisorHandler::handleLatchupStatusReport(const uint8_t* da
   return result;
 }
 
-ReturnValue_t PlocSupervisorHandler::genericHandleTm(const char* contextString, const uint8_t* data,
-                                                     LocalPoolDataSetBase& set) {
+ReturnValue_t PlocSupervisorHandler::handleAdcReport(const uint8_t* data) {
   ReturnValue_t result = returnvalue::OK;
 
-  result = verifyPacket(data, tmReader.getFullPacketLen());
+  result = verifyPacket(data, supv::SIZE_ADC_REPORT);
 
   if (result == result::CRC_FAILURE) {
-    sif::warning << "PlocSupervisorHandler: " << contextString << " report has "
-                 << "invalid CRC" << std::endl;
+    sif::error << "PlocSupervisorHandler::handleAdcReport: ADC report has "
+               << "invalid crc" << std::endl;
     return result;
   }
 
   const uint8_t* dataField = data + supv::PAYLOAD_OFFSET;
-  PoolReadGuard pg(&set);
-  if (pg.getReadResult() != returnvalue::OK) {
+  result = adcReport.read();
+  if (result != returnvalue::OK) {
     return result;
   }
-  set.setValidityBufferGeneration(false);
-  size_t size = set.getSerializedSize();
-  result = set.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG);
+  adcReport.setValidityBufferGeneration(false);
+  size_t size = adcReport.getSerializedSize();
+  result = adcReport.deSerialize(&dataField, &size, SerializeIF::Endianness::BIG);
   if (result != returnvalue::OK) {
-    sif::warning << "PlocSupervisorHandler: Deserialization failed" << std::endl;
+    sif::warning << "PlocSupervisorHandler::handleAdcReport: Deserialization failed" << std::endl;
   }
-  set.setValidityBufferGeneration(true);
-  set.setValidity(true, true);
+  adcReport.setValidityBufferGeneration(true);
+  adcReport.setValidity(true, true);
+  result = adcReport.commit();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_PLOC_SUPERVISOR == 1
+  adcReport.printSet();
+#endif
   nextReplyId = supv::EXE_REPORT;
   return result;
 }
@@ -1201,8 +1157,8 @@ void PlocSupervisorHandler::setNextReplyId() {
     case supv::CONSECUTIVE_MRAM_DUMP:
       nextReplyId = supv::CONSECUTIVE_MRAM_DUMP;
       break;
-    case supv::REQUEST_LOGGING_COUNTERS:
-      nextReplyId = supv::COUNTERS_REPORT;
+    case supv::LOGGING_REQUEST_COUNTERS:
+      nextReplyId = supv::LOGGING_REPORT;
       break;
     case supv::REQUEST_ADC_REPORT:
       nextReplyId = supv::ADC_REPORT;
@@ -1451,11 +1407,7 @@ ReturnValue_t PlocSupervisorHandler::prepareRunAutoEmTest(const uint8_t* command
   return returnvalue::OK;
 }
 
-ReturnValue_t PlocSupervisorHandler::prepareSetGpioCmd(const uint8_t* commandData,
-                                                       size_t commandDataLen) {
-  if (commandDataLen < 3) {
-    return HasActionsIF::INVALID_PARAMETERS;
-  }
+ReturnValue_t PlocSupervisorHandler::prepareSetGpioCmd(const uint8_t* commandData) {
   uint8_t port = *commandData;
   uint8_t pin = *(commandData + 1);
   uint8_t val = *(commandData + 2);
@@ -1468,11 +1420,7 @@ ReturnValue_t PlocSupervisorHandler::prepareSetGpioCmd(const uint8_t* commandDat
   return returnvalue::OK;
 }
 
-ReturnValue_t PlocSupervisorHandler::prepareReadGpioCmd(const uint8_t* commandData,
-                                                        size_t commandDataLen) {
-  if (commandDataLen < 2) {
-    return HasActionsIF::INVALID_PARAMETERS;
-  }
+ReturnValue_t PlocSupervisorHandler::prepareReadGpioCmd(const uint8_t* commandData) {
   uint8_t port = *commandData;
   uint8_t pin = *(commandData + 1);
   supv::ReadGpio packet(spParams);
@@ -1518,7 +1466,6 @@ ReturnValue_t PlocSupervisorHandler::prepareSetShutdownTimeoutCmd(const uint8_t*
     sif::warning
         << "PlocSupervisorHandler::prepareSetShutdownTimeoutCmd: Failed to deserialize timeout"
         << std::endl;
-    return result;
   }
   supv::SetShutdownTimeout packet(spParams);
   result = packet.buildPacket(timeout);
@@ -1570,8 +1517,8 @@ void PlocSupervisorHandler::disableAllReplies() {
       disableReply(LATCHUP_REPORT);
       break;
     }
-    case REQUEST_LOGGING_COUNTERS: {
-      disableReply(COUNTERS_REPORT);
+    case LOGGING_REQUEST_COUNTERS: {
+      disableReply(LOGGING_REPORT);
       break;
     }
     default: {
@@ -1585,9 +1532,6 @@ void PlocSupervisorHandler::disableAllReplies() {
 
 void PlocSupervisorHandler::disableReply(DeviceCommandId_t replyId) {
   DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
-  if (iter == deviceReplyMap.end()) {
-    return;
-  }
   DeviceReplyInfo* info = &(iter->second);
   info->delayCycles = 0;
   info->active = false;
@@ -1618,9 +1562,6 @@ void PlocSupervisorHandler::sendFailureReport(DeviceCommandId_t replyId, ReturnV
 
 void PlocSupervisorHandler::disableExeReportReply() {
   DeviceReplyIter iter = deviceReplyMap.find(supv::EXE_REPORT);
-  if (iter == deviceReplyMap.end()) {
-    return;
-  }
   DeviceReplyInfo* info = &(iter->second);
   info->delayCycles = 0;
   info->command = deviceCommandMap.end();
@@ -1643,9 +1584,7 @@ ReturnValue_t PlocSupervisorHandler::handleMramDumpPacket(DeviceCommandId_t id)
     result = handleMramDumpFile(id);
     if (result != returnvalue::OK) {
       DeviceCommandMap::iterator iter = deviceCommandMap.find(id);
-      if (iter != deviceCommandMap.end()) {
-        actionHelper.finish(false, iter->second.sendReplyTo, id, result);
-      }
+      actionHelper.finish(false, iter->second.sendReplyTo, id, result);
       disableAllReplies();
       nextReplyId = supv::NONE;
       return result;
@@ -1912,12 +1851,7 @@ ReturnValue_t PlocSupervisorHandler::eventSubscription() {
 
 ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(ExecutionReport& report) {
   DeviceCommandId_t commandId = getPendingCommand();
-  DeviceCommandMap::iterator iter = deviceCommandMap.find(commandId);
-  if (iter != deviceCommandMap.end() and iter->second.sendReplyTo != NO_COMMANDER) {
-    actionHelper.finish(true, iter->second.sendReplyTo, iter->first, returnvalue::OK);
-    iter->second.isExecuting = false;
-  }
-  commandIsPending = false;
+  ReturnValue_t result = OK;
   switch (commandId) {
     case supv::READ_GPIO: {
       // TODO: Fix
@@ -1925,13 +1859,14 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(ExecutionRepor
 #if OBSW_DEBUG_PLOC_SUPERVISOR == 1
       sif::info << "PlocSupervisorHandler: Read GPIO TM, State: " << gpioState << std::endl;
 #endif /* OBSW_DEBUG_PLOC_SUPERVISOR == 1 */
-      if (iter != deviceCommandMap.end() and iter->second.sendReplyTo == NO_COMMAND_ID) {
+      DeviceCommandMap::iterator iter = deviceCommandMap.find(commandId);
+      if (iter->second.sendReplyTo == NO_COMMAND_ID) {
         return returnvalue::OK;
       }
       uint8_t data[sizeof(gpioState)];
       size_t size = 0;
-      ReturnValue_t result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState),
-                                                         SerializeIF::Endianness::BIG);
+      result = SerializeAdapter::serialize(&gpioState, data, &size, sizeof(gpioState),
+                                           SerializeIF::Endianness::BIG);
       if (result != returnvalue::OK) {
         sif::debug << "PlocSupervisorHandler: Failed to deserialize GPIO state" << std::endl;
       }
@@ -2007,11 +1942,6 @@ uint32_t PlocSupervisorHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t mod
   return 7000;
 }
 
-void PlocSupervisorHandler::disableCommand(DeviceCommandId_t cmd) {
-  auto commandIter = deviceCommandMap.find(GET_HK_REPORT);
-  commandIter->second.isExecuting = false;
-}
-
 ReturnValue_t PlocSupervisorHandler::checkModeCommand(Mode_t commandedMode,
                                                       Submode_t commandedSubmode,
                                                       uint32_t* msToReachTheMode) {

linux/payload/PlocSupervisorHandler.h

@@ -20,8 +20,7 @@
 using supv::ExecutionReport;
 using supv::TcBase;
 
-static constexpr bool DEBUG_PLOC_SUPV = true;
-static constexpr bool REDUCE_NORMAL_MODE_PRINTOUT = true;
+static constexpr bool DEBUG_PLOC_SUPV = false;
 
 /**
  * @brief	This is the device handler for the supervisor of the PLOC which is programmed by
@@ -68,6 +67,26 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
   void doOffActivity() override;
 
  private:
+  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_SUPERVISOR_HANDLER;
+
+  //! [EXPORT] : [COMMENT] PLOC supervisor crc failure in telemetry packet
+  static const Event SUPV_MEMORY_READ_RPT_CRC_FAILURE = MAKE_EVENT(1, severity::LOW);
+  //! [EXPORT] : [COMMENT] Unhandled event. P1: APID, P2: Service ID
+  static constexpr Event SUPV_UNKNOWN_TM = MAKE_EVENT(2, severity::LOW);
+  static constexpr Event SUPV_UNINIMPLEMENTED_TM = MAKE_EVENT(3, severity::LOW);
+  //! [EXPORT] : [COMMENT] PLOC supervisor received acknowledgment failure report
+  static const Event SUPV_ACK_FAILURE = MAKE_EVENT(4, severity::LOW);
+  //! [EXPORT] : [COMMENT] PLOC received execution failure report
+  //! P1: ID of command for which the execution failed
+  //! P2: Status code sent by the supervisor handler
+  static const Event SUPV_EXE_FAILURE = MAKE_EVENT(5, severity::LOW);
+  //! [EXPORT] : [COMMENT] PLOC supervisor reply has invalid crc
+  static const Event SUPV_CRC_FAILURE_EVENT = MAKE_EVENT(6, severity::LOW);
+  //! [EXPORT] : [COMMENT] Supervisor helper currently executing a command
+  static const Event SUPV_HELPER_EXECUTING = MAKE_EVENT(7, severity::LOW);
+  //! [EXPORT] : [COMMENT] Failed to build the command to shutdown the MPSoC
+  static const Event SUPV_MPSOC_SHUTDOWN_BUILD_FAILED = MAKE_EVENT(8, severity::LOW);
+
   static const uint16_t APID_MASK = 0x7FF;
   static const uint16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF;
   static const uint8_t EXE_STATUS_OFFSET = 10;
@@ -75,14 +94,15 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
   // 5 s
   static const uint32_t EXECUTION_DEFAULT_TIMEOUT = 5000;
   // 70 S
-  static const uint32_t ACKNOWLEDGE_DEFAULT_TIMEOUT = 5000;
+  static const uint32_t ACKNOWLEDGE_DEFAULT_TIMEOUT = 70000;
   // 60 s
   static const uint32_t MRAM_DUMP_EXECUTION_TIMEOUT = 60000;
   // 70 s
   static const uint32_t COPY_ADC_TO_MRAM_TIMEOUT = 70000;
   // 60 s
   static const uint32_t MRAM_DUMP_TIMEOUT = 60000;
-
+  // 4 s
+  static const uint32_t BOOT_TIMEOUT = 4000;
   enum class StartupState : uint8_t {
     OFF,
     BOOTING,
@@ -111,18 +131,11 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
   LinuxLibgpioIF* gpioComIF = nullptr;
   Gpio uartIsolatorSwitch;
   bool shutdownCmdSent = false;
-  // Yeah, I am using an extra variable because I once again don't know
-  // what the hell the base class is doing and I don't care anymore.
-  bool normalCommandIsPending = false;
-  // True men implement their reply timeout handling themselves!
-  Countdown normalCmdCd = Countdown(2000);
-  bool commandIsPending = false;
-  Countdown cmdCd = Countdown(2000);
 
   supv::HkSet hkset;
   supv::BootStatusReport bootStatusReport;
   supv::LatchupStatusReport latchupStatusReport;
-  supv::CountersReport countersReport;
+  supv::LoggingReport loggingReport;
   supv::AdcReport adcReport;
 
   const power::Switch_t powerSwitch = power::NO_SWITCH;
@@ -151,12 +164,9 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
   Countdown executionReportTimeout = Countdown(EXECUTION_DEFAULT_TIMEOUT, false);
   Countdown acknowledgementReportTimeout = Countdown(ACKNOWLEDGE_DEFAULT_TIMEOUT, false);
   // Vorago nees some time to boot properly
-  Countdown bootTimeout = Countdown(supv::BOOT_TIMEOUT_MS);
+  Countdown bootTimeout = Countdown(BOOT_TIMEOUT);
   Countdown mramDumpTimeout = Countdown(MRAM_DUMP_TIMEOUT);
 
-  PoolEntry<uint16_t> adcRawEntry = PoolEntry<uint16_t>(16);
-  PoolEntry<uint16_t> adcEngEntry = PoolEntry<uint16_t>(16);
-  PoolEntry<uint32_t> latchupCounters = PoolEntry<uint32_t>(7);
   PoolEntry<uint8_t> fmcStateEntry = PoolEntry<uint8_t>(1);
   PoolEntry<uint8_t> bootStateEntry = PoolEntry<uint8_t>(1);
   PoolEntry<uint8_t> bootCyclesEntry = PoolEntry<uint8_t>(1);
@@ -221,13 +231,8 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
   ReturnValue_t handleBootStatusReport(const uint8_t* data);
 
   ReturnValue_t handleLatchupStatusReport(const uint8_t* data);
-  ReturnValue_t handleCounterReport(const uint8_t* data);
   void handleBadApidServiceCombination(Event result, unsigned int apid, unsigned int serviceId);
   ReturnValue_t handleAdcReport(const uint8_t* data);
-  ReturnValue_t genericHandleTm(const char* contextString, const uint8_t* data,
-                                LocalPoolDataSetBase& set);
-
-  void disableCommand(DeviceCommandId_t cmd);
 
   /**
    * @brief   Depending on the current active command, this function sets the reply id of the
@@ -296,8 +301,8 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
   ReturnValue_t prepareSetAdcThresholdCmd(const uint8_t* commandData);
   ReturnValue_t prepareRunAutoEmTest(const uint8_t* commandData);
   ReturnValue_t prepareWipeMramCmd(const uint8_t* commandData);
-  ReturnValue_t prepareSetGpioCmd(const uint8_t* commandData, size_t commandDataLen);
-  ReturnValue_t prepareReadGpioCmd(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareSetGpioCmd(const uint8_t* commandData);
+  ReturnValue_t prepareReadGpioCmd(const uint8_t* commandData);
 
   /**
    * @brief   Copies the content of a space packet to the command buffer.

linux/payload/PlocSupvUartMan.cpp

@@ -24,7 +24,6 @@
 #include "mission/utility/Filenaming.h"
 #include "mission/utility/ProgressPrinter.h"
 #include "mission/utility/Timestamp.h"
-#include "tas/crc.h"
 
 using namespace returnvalue;
 using namespace supv;
@@ -97,10 +96,9 @@ ReturnValue_t PlocSupvUartManager::initialize() {
 ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) {
   bool putTaskToSleep = false;
   while (true) {
-    {
-      MutexGuard mg(lock);
-      state = InternalState::SLEEPING;
-    }
+    lock->lockMutex();
+    state = InternalState::SLEEPING;
+    lock->unlockMutex();
     semaphore->acquire();
     putTaskToSleep = false;
 #if OBSW_THREAD_TRACING == 1
@@ -112,11 +110,9 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) {
         break;
       }
       handleUartReception();
-      InternalState currentState;
-      {
-        MutexGuard mg(lock);
-        currentState = state;
-      }
+      lock->lockMutex();
+      InternalState currentState = state;
+      lock->unlockMutex();
       switch (currentState) {
         case InternalState::SLEEPING:
         case InternalState::GO_TO_SLEEP: {
@@ -160,7 +156,7 @@ ReturnValue_t PlocSupvUartManager::handleUartReception() {
                << " bytes" << std::endl;
     return FAILED;
   } else if (bytesRead > 0) {
-    if (DEBUG_MODE) {
+    if (debugMode) {
       sif::info << "Received " << bytesRead << " bytes from the PLOC Supervisor:" << std::endl;
       arrayprinter::print(recBuf.data(), bytesRead);
     }
@@ -575,7 +571,7 @@ ReturnValue_t PlocSupvUartManager::handlePacketTransmissionNoReply(
       size_t packetLen = 0;
       decodedQueue.retrieve(&packetLen);
       decodedRingBuf.readData(decodedBuf.data(), packetLen, true);
-      tmReader.setReadOnlyData(decodedBuf.data(), packetLen);
+      tmReader.setData(decodedBuf.data(), packetLen);
       result = checkReceivedTm();
       if (result != returnvalue::OK) {
         continue;
@@ -621,7 +617,7 @@ int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, size_t packetLen)
   if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::ACK) or
       serviceId == static_cast<uint8_t>(supv::tm::TmtcId::NAK)) {
     AcknowledgmentReport ackReport(tmReader);
-    ReturnValue_t result = ackReport.parse(false);
+    ReturnValue_t result = ackReport.parse();
     if (result != returnvalue::OK) {
       triggerEvent(ACK_RECEPTION_FAILURE);
       return -1;
@@ -631,7 +627,7 @@ int PlocSupvUartManager::handleAckReception(supv::TcBase& tc, size_t packetLen)
       if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::ACK)) {
         return 1;
       } else if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::NAK)) {
-        ackReport.printStatusInformationAck();
+        ackReport.printStatusInformation();
         triggerEvent(
             SUPV_ACK_FAILURE_REPORT,
             buildApidServiceParam1(ackReport.getRefModuleApid(), ackReport.getRefServiceId()),
@@ -653,7 +649,7 @@ int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, size_t packetLe
   if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK) or
       serviceId == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)) {
     ExecutionReport exeReport(tmReader);
-    ReturnValue_t result = exeReport.parse(false);
+    ReturnValue_t result = exeReport.parse();
     if (result != returnvalue::OK) {
       triggerEvent(EXE_RECEPTION_FAILURE);
       return -1;
@@ -663,7 +659,7 @@ int PlocSupvUartManager::handleExeAckReception(supv::TcBase& tc, size_t packetLe
       if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_ACK)) {
         return 1;
       } else if (serviceId == static_cast<uint8_t>(supv::tm::TmtcId::EXEC_NAK)) {
-        exeReport.printStatusInformationExe();
+        exeReport.printStatusInformation();
         triggerEvent(
             SUPV_EXE_FAILURE_REPORT,
             buildApidServiceParam1(exeReport.getRefModuleApid(), exeReport.getRefServiceId()),
@@ -686,7 +682,7 @@ ReturnValue_t PlocSupvUartManager::checkReceivedTm() {
     triggerEvent(SUPV_REPLY_SIZE_MISSMATCH, rememberApid);
     return result;
   }
-  if (tmReader.checkCrc() != returnvalue::OK) {
+  if (not tmReader.verifyCrc()) {
     triggerEvent(SUPV_REPLY_CRC_MISSMATCH, rememberApid);
     return result;
   }
@@ -762,7 +758,7 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand(uint8_t failStep) {
       size_t packetLen = 0;
       decodedQueue.retrieve(&packetLen);
       decodedRingBuf.readData(decodedBuf.data(), packetLen, true);
-      tmReader.setReadOnlyData(decodedBuf.data(), packetLen);
+      tmReader.setData(decodedBuf.data(), packetLen);
       result = checkReceivedTm();
       if (result != returnvalue::OK) {
         continue;
@@ -790,7 +786,7 @@ ReturnValue_t PlocSupvUartManager::handleCheckMemoryCommand(uint8_t failStep) {
       } else if (tmReader.getModuleApid() == Apid::MEM_MAN) {
         if (ackReceived) {
           supv::UpdateStatusReport report(tmReader);
-          result = report.parse(false);
+          result = report.parse();
           if (result != returnvalue::OK) {
             return result;
           }
@@ -945,7 +941,15 @@ ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() {
       break;
     }
     case Request::REQUEST_EVENT_BUFFER: {
-      sif::error << "Requesting event buffer is not implemented" << std::endl;
+      //      result = performEventBufferRequest();
+      //      if (result == returnvalue::OK) {
+      //        triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result);
+      //      } else if (result == PROCESS_TERMINATED) {
+      //        // Event already triggered
+      //        break;
+      //      } else {
+      //        triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result);
+      //      }
       break;
     }
     case Request::DEFAULT: {
@@ -958,7 +962,7 @@ ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() {
 ReturnValue_t PlocSupvUartManager::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) {
   size_t encodedLen = 0;
   addHdlcFraming(sendData, sendLen, encodedSendBuf.data(), &encodedLen, encodedSendBuf.size());
-  if (PRINT_TC) {
+  if (printTc) {
     sif::debug << "Sending TC" << std::endl;
     arrayprinter::print(encodedSendBuf.data(), encodedLen);
   }
@@ -980,9 +984,6 @@ ReturnValue_t PlocSupvUartManager::readReceivedMessage(CookieIF* cookie, uint8_t
     return OK;
   }
   ipcQueue.retrieve(size);
-  if (*size > ipcBuffer.size()) {
-    return FAILED;
-  }
   *buffer = ipcBuffer.data();
   ReturnValue_t result = ipcRingBuf.readData(ipcBuffer.data(), *size, true);
   if (result != OK) {
@@ -1053,7 +1054,6 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize, size
           triggerEvent(HDLC_CRC_ERROR);
         }
         if (retval != 0) {
-          readSize = ++idx;
           return HDLC_ERROR;
         }
         return returnvalue::OK;
@@ -1084,14 +1084,11 @@ void PlocSupvUartManager::performUartShutdown() {
   while (not decodedQueue.empty()) {
     decodedQueue.pop();
   }
-  {
-    MutexGuard mg0(ipcLock);
-    ipcRingBuf.clear();
-    while (not ipcQueue.empty()) {
-      ipcQueue.pop();
-    }
+  MutexGuard mg(ipcLock);
+  ipcRingBuf.clear();
+  while (not ipcQueue.empty()) {
+    ipcQueue.pop();
   }
-  MutexGuard mg1(lock);
   state = InternalState::GO_TO_SLEEP;
 }
 

linux/payload/PlocSupvUartMan.h

@@ -16,6 +16,7 @@
 #include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw_hal/linux/serial/SerialComIF.h"
+#include "tas/crc.h"
 
 #ifdef XIPHOS_Q7S
 #include "bsp_q7s/fs/SdCardManager.h"
@@ -120,32 +121,6 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
 
   PlocSupvUartManager(object_id_t objectId);
   virtual ~PlocSupvUartManager();
-  /**
-   * @brief Device specific initialization, using the cookie.
-   * @details
-   * The cookie is already prepared in the factory. If the communication
-   * interface needs to be set up in some way and requires cookie information,
-   * this can be performed in this function, which is called on device handler
-   * initialization.
-   * @param cookie
-   * @return
-   *  - @c returnvalue::OK if initialization was successfull
-   *  - Everything else triggers failure event with returnvalue as parameter 1
-   */
-  ReturnValue_t initializeInterface(CookieIF* cookie) override;
-  /**
-   * Called by DHB in the SEND_WRITE doSendWrite().
-   * This function is used to send data to the physical device
-   * by implementing and calling related drivers or wrapper functions.
-   * @param cookie
-   * @param data
-   * @param len If this is 0, nothing shall be sent.
-   * @return
-   *  - @c returnvalue::OK for successfull send
-   *  - Everything else triggers failure event with returnvalue as parameter 1
-   */
-  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
-  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
 
   ReturnValue_t initialize() override;
   ReturnValue_t performOperation(uint8_t operationCode = 0) override;
@@ -231,11 +206,11 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
 
   struct Update update;
 
-  int serialPort = 0;
   SemaphoreIF* semaphore;
   MutexIF* lock;
   MutexIF* ipcLock;
   supv::TmBase tmReader;
+  int serialPort = 0;
   struct termios tty = {};
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;
@@ -282,8 +257,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
 
   std::array<uint8_t, supv::MAX_COMMAND_SIZE> tmBuf{};
 
-  static constexpr bool PRINT_TC = false;
-  static constexpr bool DEBUG_MODE = false;
+  bool printTc = false;
+  bool debugMode = false;
   bool timestamping = true;
 
   // Remembers APID to know at which command a procedure failed
@@ -344,6 +319,32 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
   void resetSpParams();
   void pushIpcData(const uint8_t* data, size_t len);
 
+  /**
+   * @brief Device specific initialization, using the cookie.
+   * @details
+   * The cookie is already prepared in the factory. If the communication
+   * interface needs to be set up in some way and requires cookie information,
+   * this can be performed in this function, which is called on device handler
+   * initialization.
+   * @param cookie
+   * @return
+   *  - @c returnvalue::OK if initialization was successfull
+   *  - Everything else triggers failure event with returnvalue as parameter 1
+   */
+  ReturnValue_t initializeInterface(CookieIF* cookie) override;
+
+  /**
+   * Called by DHB in the SEND_WRITE doSendWrite().
+   * This function is used to send data to the physical device
+   * by implementing and calling related drivers or wrapper functions.
+   * @param cookie
+   * @param data
+   * @param len If this is 0, nothing shall be sent.
+   * @return
+   *  - @c returnvalue::OK for successfull send
+   *  - Everything else triggers failure event with returnvalue as parameter 1
+   */
+  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
   /**
    * Called by DHB in the GET_WRITE doGetWrite().
    * Get send confirmation that the data in sendMessage() was sent successfully.
@@ -368,6 +369,7 @@ class PlocSupvUartManager : public DeviceCommunicationIF,
    *           returnvalue as parameter 1
    */
   ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
+  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
 
   void performUartShutdown();
   void updateVtime(uint8_t vtime);

linux/payload/plocMpsocHelpers.h

@@ -13,8 +13,6 @@
 
 namespace mpsoc {
 
-enum ParamId : uint8_t { SKIP_SUPV_ON_COMMANDING = 0x01 };
-
 enum FileAccessModes : uint8_t {
   // Opens a file, fails if the file does not exist.
   OPEN_EXISTING = 0x00,

linux/payload/plocSupvDefs.h

@@ -11,46 +11,13 @@
 #include <mission/payload/plocSpBase.h>
 
 #include <atomic>
+#include <optional>
 
-#include "eive/eventSubsystemIds.h"
 #include "eive/resultClassIds.h"
 
 namespace supv {
 
-static constexpr bool DEBUG_PLOC_SUPV = false;
-static constexpr bool REDUCE_NORMAL_MODE_PRINTOUT = true;
-
-static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_SUPERVISOR_HANDLER;
-
-//! [EXPORT] : [COMMENT] PLOC supervisor crc failure in telemetry packet
-static const Event SUPV_MEMORY_READ_RPT_CRC_FAILURE = MAKE_EVENT(1, severity::LOW);
-//! [EXPORT] : [COMMENT] Unhandled event. P1: APID, P2: Service ID
-static constexpr Event SUPV_UNKNOWN_TM = MAKE_EVENT(2, severity::LOW);
-static constexpr Event SUPV_UNINIMPLEMENTED_TM = MAKE_EVENT(3, severity::LOW);
-//! [EXPORT] : [COMMENT] PLOC supervisor received acknowledgment failure report
-static const Event SUPV_ACK_FAILURE = MAKE_EVENT(4, severity::LOW);
-//! [EXPORT] : [COMMENT] PLOC received execution failure report
-//! P1: ID of command for which the execution failed
-//! P2: Status code sent by the supervisor handler
-static const Event SUPV_EXE_FAILURE = MAKE_EVENT(5, severity::LOW);
-//! [EXPORT] : [COMMENT] PLOC supervisor reply has invalid crc
-static const Event SUPV_CRC_FAILURE_EVENT = MAKE_EVENT(6, severity::LOW);
-//! [EXPORT] : [COMMENT] Supervisor helper currently executing a command
-static const Event SUPV_HELPER_EXECUTING = MAKE_EVENT(7, severity::LOW);
-//! [EXPORT] : [COMMENT] Failed to build the command to shutdown the MPSoC
-static const Event SUPV_MPSOC_SHUTDOWN_BUILD_FAILED = MAKE_EVENT(8, severity::LOW);
-//! [EXPORT] : [COMMENT] Received ACK, but no related command is unknown or has not been sent
-//  by this software instance. P1: Module APID. P2: Service ID.
-static const Event SUPV_ACK_UNKNOWN_COMMAND = MAKE_EVENT(9, severity::LOW);
-//! [EXPORT] : [COMMENT] Received ACK EXE, but no related command is unknown or has not been sent
-//  by this software instance. P1: Module APID. P2: Service ID.
-static const Event SUPV_EXE_ACK_UNKNOWN_COMMAND = MAKE_EVENT(10, severity::LOW);
-
 extern std::atomic_bool SUPV_ON;
-static constexpr uint32_t INTER_COMMAND_DELAY = 20;
-static constexpr uint32_t BOOT_TIMEOUT_MS = 4000;
-static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 3000;
-static constexpr uint32_t MAX_TRANSITION_TIME_TO_OFF_MS = 1000;
 
 namespace result {
 static const uint8_t INTERFACE_ID = CLASS_ID::SUPV_RETURN_VALUES_IF;
@@ -140,7 +107,7 @@ static const DeviceCommandId_t FIRST_MRAM_DUMP = 30;
 static const DeviceCommandId_t SET_GPIO = 34;
 static const DeviceCommandId_t READ_GPIO = 35;
 static const DeviceCommandId_t RESTART_SUPERVISOR = 36;
-static const DeviceCommandId_t REQUEST_LOGGING_COUNTERS = 38;
+static const DeviceCommandId_t LOGGING_REQUEST_COUNTERS = 38;
 static constexpr DeviceCommandId_t FACTORY_RESET = 39;
 static const DeviceCommandId_t CONSECUTIVE_MRAM_DUMP = 43;
 static const DeviceCommandId_t START_MPSOC_QUIET = 45;
@@ -153,7 +120,7 @@ static const DeviceCommandId_t DISABLE_AUTO_TM = 51;
 static const DeviceCommandId_t LOGGING_REQUEST_EVENT_BUFFERS = 54;
 static const DeviceCommandId_t LOGGING_CLEAR_COUNTERS = 55;
 static const DeviceCommandId_t LOGGING_SET_TOPIC = 56;
-static constexpr DeviceCommandId_t REQUEST_ADC_REPORT = 57;
+static const DeviceCommandId_t REQUEST_ADC_REPORT = 57;
 static const DeviceCommandId_t RESET_PL = 58;
 static const DeviceCommandId_t ENABLE_NVMS = 59;
 static const DeviceCommandId_t CONTINUE_UPDATE = 60;
@@ -167,7 +134,7 @@ enum ReplyId : DeviceCommandId_t {
   HK_REPORT = 102,
   BOOT_STATUS_REPORT = 103,
   LATCHUP_REPORT = 104,
-  COUNTERS_REPORT = 105,
+  LOGGING_REPORT = 105,
   ADC_REPORT = 106,
   UPDATE_STATUS_REPORT = 107,
 };
@@ -177,7 +144,7 @@ static const uint16_t SIZE_ACK_REPORT = 14;
 static const uint16_t SIZE_EXE_REPORT = 14;
 static const uint16_t SIZE_BOOT_STATUS_REPORT = 24;
 static const uint16_t SIZE_LATCHUP_STATUS_REPORT = 31;
-static const uint16_t SIZE_COUNTERS_REPORT = 120;
+static const uint16_t SIZE_LOGGING_REPORT = 73;
 static const uint16_t SIZE_ADC_REPORT = 72;
 
 // 2 bits APID SRC, 00 for OBC, 2 bits APID DEST, 01 for SUPV, 7 bits CMD ID -> Mask 0x080
@@ -240,18 +207,12 @@ enum class AdcMonId : uint8_t {
   SET_ENABLED_CHANNELS = 0x02,
   SET_WINDOW_STRIDE = 0x03,
   SET_ADC_THRESHOLD = 0x04,
-  COPY_ADC_DATA_TO_MRAM = 0x05,
-  REQUEST_ADC_SAMPLE = 0x06
+  COPY_ADC_DATA_TO_MRAM = 0x05
 };
 
 enum class MemManId : uint8_t { ERASE = 0x01, WRITE = 0x02, CHECK = 0x03 };
 
 enum class DataLoggerServiceId : uint8_t {
-  // Not implemented.
-  READ_MRAM_CFG_DATA_LOGGER = 0x00,
-  REQUEST_COUNTERS = 0x01,
-  // Not implemented.
-  EVENT_BUFFER_DOWNLOAD = 0x02,
   WIPE_MRAM = 0x05,
   DUMP_MRAM = 0x06,
   FACTORY_RESET = 0x07
@@ -270,12 +231,10 @@ enum class TmtcId : uint8_t { ACK = 0x01, NAK = 0x02, EXEC_ACK = 0x03, EXEC_NAK
 enum class HkId : uint8_t { REPORT = 0x01, HARDFAULTS = 0x02 };
 
 enum class BootManId : uint8_t { BOOT_STATUS_REPORT = 0x01 };
-enum class AdcMonId : uint8_t { ADC_REPORT = 0x01 };
 
 enum class MemManId : uint8_t { UPDATE_STATUS_REPORT = 0x01 };
 
 enum class LatchupMonId : uint8_t { LATCHUP_STATUS_REPORT = 0x01 };
-enum class DataLoggerId : uint8_t { COUNTERS_REPORT = 0x01 };
 
 }  // namespace tm
 
@@ -362,13 +321,13 @@ static const uint16_t SEQUENCE_COUNT_MASK = 0xFFF;
 static const uint8_t HK_SET_ENTRIES = 13;
 static const uint8_t BOOT_REPORT_SET_ENTRIES = 10;
 static const uint8_t LATCHUP_RPT_SET_ENTRIES = 16;
-static const uint8_t LOGGING_RPT_SET_ENTRIES = 30;
+static const uint8_t LOGGING_RPT_SET_ENTRIES = 16;
 static const uint8_t ADC_RPT_SET_ENTRIES = 32;
 
 static const uint32_t HK_SET_ID = HK_REPORT;
 static const uint32_t BOOT_REPORT_SET_ID = BOOT_STATUS_REPORT;
 static const uint32_t LATCHUP_RPT_ID = LATCHUP_REPORT;
-static const uint32_t LOGGING_RPT_ID = COUNTERS_REPORT;
+static const uint32_t LOGGING_RPT_ID = LOGGING_REPORT;
 static const uint32_t ADC_REPORT_SET_ID = ADC_REPORT;
 
 namespace timeout {
@@ -433,7 +392,7 @@ enum PoolIds : lp_id_t {
   BR_SOC_STATE,
   POWER_CYCLES,
   BOOT_AFTER_MS,
-  BOOT_TIMEOUT_POOL_VAR_MS,
+  BOOT_TIMEOUT_MS,
   ACTIVE_NVM,
   BP0_STATE,
   BP1_STATE,
@@ -458,8 +417,13 @@ enum PoolIds : lp_id_t {
   LATCHUP_RPT_TIME_MSEC,
   LATCHUP_RPT_IS_SET,
 
-  SIGNATURE,
-  LATCHUP_HAPPENED_CNTS,
+  LATCHUP_HAPPENED_CNT_0,
+  LATCHUP_HAPPENED_CNT_1,
+  LATCHUP_HAPPENED_CNT_2,
+  LATCHUP_HAPPENED_CNT_3,
+  LATCHUP_HAPPENED_CNT_4,
+  LATCHUP_HAPPENED_CNT_5,
+  LATCHUP_HAPPENED_CNT_6,
   ADC_DEVIATION_TRIGGERS_CNT,
   TC_RECEIVED_CNT,
   TM_AVAILABLE_CNT,
@@ -468,22 +432,40 @@ enum PoolIds : lp_id_t {
   MPSOC_BOOT_FAILED_ATTEMPTS,
   MPSOC_POWER_UP,
   MPSOC_UPDATES,
-  MPSOC_HEARTBEAT_RESETS,
-  CPU_WDT_RESETS,
-  PS_HEARTBEATS_LOST,
-  PL_HEARTBEATS_LOST,
-  EB_TASK_LOST,
-  BM_TASK_LOST,
-  LM_TASK_LOST,
-  AM_TASK_LOST,
-  TCTMM_TASK_LOST,
-  MM_TASK_LOST,
-  HK_TASK_LOST,
-  DL_TASK_LOST,
-  RWS_TASKS_LOST,
+  LAST_RECVD_TC,
 
-  ADC_RAW,
-  ADC_ENG,
+  ADC_RAW_0,
+  ADC_RAW_1,
+  ADC_RAW_2,
+  ADC_RAW_3,
+  ADC_RAW_4,
+  ADC_RAW_5,
+  ADC_RAW_6,
+  ADC_RAW_7,
+  ADC_RAW_8,
+  ADC_RAW_9,
+  ADC_RAW_10,
+  ADC_RAW_11,
+  ADC_RAW_12,
+  ADC_RAW_13,
+  ADC_RAW_14,
+  ADC_RAW_15,
+  ADC_ENG_0,
+  ADC_ENG_1,
+  ADC_ENG_2,
+  ADC_ENG_3,
+  ADC_ENG_4,
+  ADC_ENG_5,
+  ADC_ENG_6,
+  ADC_ENG_7,
+  ADC_ENG_8,
+  ADC_ENG_9,
+  ADC_ENG_10,
+  ADC_ENG_11,
+  ADC_ENG_12,
+  ADC_ENG_13,
+  ADC_ENG_14,
+  ADC_ENG_15
 };
 
 struct TcParams : public ploc::SpTcParams {
@@ -557,6 +539,15 @@ class TmBase : public ploc::SpTmReader {
     }
   }
 
+  bool verifyCrc() {
+    if (checkCrc() == returnvalue::OK) {
+      crcOk = true;
+    }
+    return crcOk;
+  }
+
+  bool crcIsOk() const { return crcOk; }
+
   uint8_t getServiceId() const { return getPacketData()[TIMESTAMP_LEN]; }
 
   uint16_t getModuleApid() const { return getApid() & APID_MODULE_MASK; }
@@ -568,6 +559,9 @@ class TmBase : public ploc::SpTmReader {
     }
     return 0;
   }
+
+ private:
+  bool crcOk = false;
 };
 
 class NoPayloadPacket : public TcBase {
@@ -775,6 +769,8 @@ class SetRestartTries : public TcBase {
   }
 
  private:
+  uint8_t restartTries = 0;
+
   void initPacket(uint8_t restartTries) { payloadStart[0] = restartTries; }
 };
 
@@ -835,6 +831,8 @@ class LatchupAlert : public TcBase {
   }
 
  private:
+  uint8_t latchupId = 0;
+
   void initPacket(uint8_t latchupId) { payloadStart[0] = latchupId; }
 };
 
@@ -864,6 +862,9 @@ class SetAlertlimit : public TcBase {
   }
 
  private:
+  uint8_t latchupId = 0;
+  uint32_t dutycycle = 0;
+
   ReturnValue_t initPacket(uint8_t latchupId, uint32_t dutycycle) {
     payloadStart[0] = latchupId;
     size_t serLen = 0;
@@ -1294,8 +1295,8 @@ class VerificationReport {
 
   virtual ~VerificationReport() = default;
 
-  virtual ReturnValue_t parse(bool checkCrc) {
-    if (checkCrc and readerBase.checkCrc() != returnvalue::OK) {
+  virtual ReturnValue_t parse() {
+    if (not readerBase.crcIsOk()) {
       return result::CRC_FAILURE;
     }
     if (readerBase.getModuleApid() != Apid::TMTC_MAN) {
@@ -1312,27 +1313,27 @@ class VerificationReport {
     ReturnValue_t result = SerializeAdapter::deSerialize(&refApid, &payloadStart, &remLen,
                                                          SerializeIF::Endianness::BIG);
     if (result != returnvalue::OK) {
-      sif::warning << "VerificationReport: Failed to deserialize reference APID field" << std::endl;
+      sif::debug << "VerificationReport: Failed to deserialize reference APID field" << std::endl;
       return result;
     }
     result = SerializeAdapter::deSerialize(&refServiceId, &payloadStart, &remLen,
                                            SerializeIF::Endianness::BIG);
     if (result != returnvalue::OK) {
-      sif::warning << "VerificationReport: Failed to deserialize reference Service ID field"
-                   << std::endl;
+      sif::debug << "VerificationReport: Failed to deserialize reference Service ID field"
+                 << std::endl;
       return result;
     }
     result = SerializeAdapter::deSerialize(&refSeqCount, &payloadStart, &remLen,
                                            SerializeIF::Endianness::BIG);
     if (result != returnvalue::OK) {
-      sif::warning << "VerificationReport: Failed to deserialize reference sequence count field"
-                   << std::endl;
+      sif::debug << "VerificationReport: Failed to deserialize reference sequence count field"
+                 << std::endl;
       return result;
     }
     result = SerializeAdapter::deSerialize(&statusCode, &payloadStart, &remLen,
                                            SerializeIF::Endianness::BIG);
     if (result != returnvalue::OK) {
-      sif::warning << "VerificationReport: Failed to deserialize status code field" << std::endl;
+      sif::debug << "VerificationReport: Failed to deserialize status code field" << std::endl;
       return result;
     }
     return returnvalue::OK;
@@ -1349,7 +1350,7 @@ class VerificationReport {
 
   uint32_t getStatusCode() const { return statusCode; }
 
-  virtual void printStatusInformation(const char* prefix) const {
+  virtual void printStatusInformation(const char* prefix) {
     bool codeHandled = true;
     if (statusCode < 0x100) {
       GeneralStatusCode code = static_cast<GeneralStatusCode>(getStatusCode());
@@ -1636,15 +1637,15 @@ class AcknowledgmentReport : public VerificationReport {
  public:
   AcknowledgmentReport(TmBase& readerBase) : VerificationReport(readerBase) {}
 
-  ReturnValue_t parse(bool checkCrc) override {
+  virtual ReturnValue_t parse() override {
     if (readerBase.getServiceId() != static_cast<uint8_t>(tm::TmtcId::ACK) and
         readerBase.getServiceId() != static_cast<uint8_t>(tm::TmtcId::NAK)) {
       return result::INVALID_SERVICE_ID;
     }
-    return VerificationReport::parse(checkCrc);
+    return VerificationReport::parse();
   }
 
-  void printStatusInformationAck() {
+  void printStatusInformation() {
     VerificationReport::printStatusInformation(STATUS_PRINTOUT_PREFIX);
   }
 
@@ -1658,15 +1659,15 @@ class ExecutionReport : public VerificationReport {
 
   TmBase& getReader() { return readerBase; }
 
-  ReturnValue_t parse(bool checkCrc) override {
+  ReturnValue_t parse() override {
     if (readerBase.getServiceId() != static_cast<uint8_t>(tm::TmtcId::EXEC_ACK) and
         readerBase.getServiceId() != static_cast<uint8_t>(tm::TmtcId::EXEC_NAK)) {
       return returnvalue::FAILED;
     }
-    return VerificationReport::parse(checkCrc);
+    return VerificationReport::parse();
   }
 
-  void printStatusInformationExe() {
+  void printStatusInformation() {
     VerificationReport::printStatusInformation(STATUS_PRINTOUT_PREFIX);
   }
 
@@ -1678,8 +1679,8 @@ class UpdateStatusReport {
  public:
   UpdateStatusReport(TmBase& tmReader) : tmReader(tmReader) {}
 
-  ReturnValue_t parse(bool checkCrc) {
-    if (checkCrc and tmReader.checkCrc() != returnvalue::OK) {
+  ReturnValue_t parse() {
+    if (not tmReader.crcIsOk()) {
       return result::CRC_FAILURE;
     }
     if (tmReader.getModuleApid() != Apid::MEM_MAN) {
@@ -1741,7 +1742,7 @@ class BootStatusReport : public StaticLocalDataSet<BOOT_REPORT_SET_ENTRIES> {
   lp_var_t<uint32_t> bootAfterMs = lp_var_t<uint32_t>(sid.objectId, PoolIds::BOOT_AFTER_MS, this);
   /** The currently set boot timeout */
   lp_var_t<uint32_t> bootTimeoutMs =
-      lp_var_t<uint32_t>(sid.objectId, PoolIds::BOOT_TIMEOUT_POOL_VAR_MS, this);
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::BOOT_TIMEOUT_MS, this);
   lp_var_t<uint8_t> activeNvm = lp_var_t<uint8_t>(sid.objectId, PoolIds::ACTIVE_NVM, this);
   /** States of the boot partition pins */
   lp_var_t<uint8_t> bp0State = lp_var_t<uint8_t>(sid.objectId, PoolIds::BP0_STATE, this);
@@ -1805,7 +1806,7 @@ class LatchupStatusReport : public StaticLocalDataSet<LATCHUP_RPT_SET_ENTRIES> {
   lp_var_t<uint8_t> timeMon = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_TIME_MON, this);
   lp_var_t<uint8_t> timeYear =
       lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_TIME_YEAR, this);
-  lp_var_t<uint8_t> isSynced = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_IS_SET, this);
+  lp_var_t<uint8_t> isSet = lp_var_t<uint8_t>(sid.objectId, PoolIds::LATCHUP_RPT_IS_SET, this);
 
   static const uint8_t IS_SET_BIT_POS = 15;
 };
@@ -1813,16 +1814,26 @@ class LatchupStatusReport : public StaticLocalDataSet<LATCHUP_RPT_SET_ENTRIES> {
 /**
  * @brief   This dataset stores the logging report.
  */
-class CountersReport : public StaticLocalDataSet<LOGGING_RPT_SET_ENTRIES> {
+class LoggingReport : public StaticLocalDataSet<LOGGING_RPT_SET_ENTRIES> {
  public:
-  CountersReport(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, LOGGING_RPT_ID) {}
+  LoggingReport(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, LOGGING_RPT_ID) {}
 
-  CountersReport(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, LOGGING_RPT_ID)) {}
+  LoggingReport(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, LOGGING_RPT_ID)) {}
 
-  lp_var_t<uint32_t> signature =
-      lp_var_t<uint32_t>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNTS, this);
-  lp_vec_t<uint32_t, 7> latchupHappenCnts =
-      lp_vec_t<uint32_t, 7>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNTS, this);
+  lp_var_t<uint32_t> latchupHappenCnt0 =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNT_0, this);
+  lp_var_t<uint32_t> latchupHappenCnt1 =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNT_1, this);
+  lp_var_t<uint32_t> latchupHappenCnt2 =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNT_2, this);
+  lp_var_t<uint32_t> latchupHappenCnt3 =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNT_3, this);
+  lp_var_t<uint32_t> latchupHappenCnt4 =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNT_4, this);
+  lp_var_t<uint32_t> latchupHappenCnt5 =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNT_5, this);
+  lp_var_t<uint32_t> latchupHappenCnt6 =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::LATCHUP_HAPPENED_CNT_6, this);
   lp_var_t<uint32_t> adcDeviationTriggersCnt =
       lp_var_t<uint32_t>(sid.objectId, PoolIds::ADC_DEVIATION_TRIGGERS_CNT, this);
   lp_var_t<uint32_t> tcReceivedCnt =
@@ -1836,31 +1847,23 @@ class CountersReport : public StaticLocalDataSet<LOGGING_RPT_SET_ENTRIES> {
       lp_var_t<uint32_t>(sid.objectId, PoolIds::MPSOC_BOOT_FAILED_ATTEMPTS, this);
   lp_var_t<uint32_t> mpsocPowerup = lp_var_t<uint32_t>(sid.objectId, PoolIds::MPSOC_POWER_UP, this);
   lp_var_t<uint32_t> mpsocUpdates = lp_var_t<uint32_t>(sid.objectId, PoolIds::MPSOC_UPDATES, this);
-  lp_var_t<uint32_t> mpsocHeartbeatResets =
-      lp_var_t<uint32_t>(sid.objectId, PoolIds::MPSOC_HEARTBEAT_RESETS, this);
-  lp_var_t<uint32_t> cpuWdtResets =
-      lp_var_t<uint32_t>(sid.objectId, PoolIds::MPSOC_HEARTBEAT_RESETS, this);
-  lp_var_t<uint32_t> psHeartbeatsLost =
-      lp_var_t<uint32_t>(sid.objectId, PoolIds::PS_HEARTBEATS_LOST, this);
-  lp_var_t<uint32_t> plHeartbeatsLost =
-      lp_var_t<uint32_t>(sid.objectId, PoolIds::PL_HEARTBEATS_LOST, this);
-  lp_var_t<uint32_t> ebTaskLost = lp_var_t<uint32_t>(sid.objectId, PoolIds::EB_TASK_LOST, this);
-  lp_var_t<uint32_t> bmTaskLost = lp_var_t<uint32_t>(sid.objectId, PoolIds::BM_TASK_LOST, this);
-  lp_var_t<uint32_t> lmTaskLost = lp_var_t<uint32_t>(sid.objectId, PoolIds::LM_TASK_LOST, this);
-  lp_var_t<uint32_t> amTaskLost = lp_var_t<uint32_t>(sid.objectId, PoolIds::AM_TASK_LOST, this);
-  lp_var_t<uint32_t> tctmmTaskLost =
-      lp_var_t<uint32_t>(sid.objectId, PoolIds::TCTMM_TASK_LOST, this);
-  lp_var_t<uint32_t> mmTaskLost = lp_var_t<uint32_t>(sid.objectId, PoolIds::MM_TASK_LOST, this);
-  lp_var_t<uint32_t> hkTaskLost = lp_var_t<uint32_t>(sid.objectId, PoolIds::HK_TASK_LOST, this);
-  lp_var_t<uint32_t> dlTaskLost = lp_var_t<uint32_t>(sid.objectId, PoolIds::DL_TASK_LOST, this);
-  lp_vec_t<uint32_t, 3> rwsTasksLost =
-      lp_vec_t<uint32_t, 3>(sid.objectId, PoolIds::RWS_TASKS_LOST, this);
+  lp_var_t<uint32_t> lastRecvdTc = lp_var_t<uint32_t>(sid.objectId, PoolIds::LAST_RECVD_TC, this);
 
   void printSet() {
-    for (unsigned i = 0; i < 7; i++) {
-      sif::info << "LoggingReport: Latchup happened count " << i << ": "
-                << this->latchupHappenCnts[i] << std::endl;
-    }
+    sif::info << "LoggingReport: Latchup happened count 0: " << this->latchupHappenCnt0
+              << std::endl;
+    sif::info << "LoggingReport: Latchup happened count 1: " << this->latchupHappenCnt1
+              << std::endl;
+    sif::info << "LoggingReport: Latchup happened count 2: " << this->latchupHappenCnt2
+              << std::endl;
+    sif::info << "LoggingReport: Latchup happened count 3: " << this->latchupHappenCnt3
+              << std::endl;
+    sif::info << "LoggingReport: Latchup happened count 4: " << this->latchupHappenCnt4
+              << std::endl;
+    sif::info << "LoggingReport: Latchup happened count 5: " << this->latchupHappenCnt5
+              << std::endl;
+    sif::info << "LoggingReport: Latchup happened count 6: " << this->latchupHappenCnt6
+              << std::endl;
     sif::info << "LoggingReport: ADC deviation triggers count: " << this->adcDeviationTriggersCnt
               << std::endl;
     sif::info << "LoggingReport: TC received count: " << this->tcReceivedCnt << std::endl;
@@ -1871,29 +1874,88 @@ class CountersReport : public StaticLocalDataSet<LOGGING_RPT_SET_ENTRIES> {
               << std::endl;
     sif::info << "LoggingReport: MPSoC power up: " << this->mpsocPowerup << std::endl;
     sif::info << "LoggingReport: MPSoC updates: " << this->mpsocUpdates << std::endl;
+    sif::info << "LoggingReport: APID of last received TC: 0x" << std::hex << this->lastRecvdTc
+              << std::endl;
   }
 };
 
 /**
  * @brief   This dataset stores the ADC report.
  */
-class AdcReport : public StaticLocalDataSet<3> {
+class AdcReport : public StaticLocalDataSet<ADC_RPT_SET_ENTRIES> {
  public:
   AdcReport(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, ADC_REPORT_SET_ID) {}
 
   AdcReport(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ADC_REPORT_SET_ID)) {}
 
-  lp_vec_t<uint16_t, 16> adcRaw = lp_vec_t<uint16_t, 16>(sid.objectId, PoolIds::ADC_RAW, this);
-  lp_vec_t<uint16_t, 16> adcEng = lp_vec_t<uint16_t, 16>(sid.objectId, PoolIds::ADC_ENG, this);
+  lp_var_t<uint16_t> adcRaw0 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_0, this);
+  lp_var_t<uint16_t> adcRaw1 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_1, this);
+  lp_var_t<uint16_t> adcRaw2 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_2, this);
+  lp_var_t<uint16_t> adcRaw3 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_3, this);
+  lp_var_t<uint16_t> adcRaw4 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_4, this);
+  lp_var_t<uint16_t> adcRaw5 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_5, this);
+  lp_var_t<uint16_t> adcRaw6 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_6, this);
+  lp_var_t<uint16_t> adcRaw7 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_7, this);
+  lp_var_t<uint16_t> adcRaw8 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_8, this);
+  lp_var_t<uint16_t> adcRaw9 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_9, this);
+  lp_var_t<uint16_t> adcRaw10 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_10, this);
+  lp_var_t<uint16_t> adcRaw11 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_11, this);
+  lp_var_t<uint16_t> adcRaw12 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_12, this);
+  lp_var_t<uint16_t> adcRaw13 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_13, this);
+  lp_var_t<uint16_t> adcRaw14 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_14, this);
+  lp_var_t<uint16_t> adcRaw15 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_RAW_15, this);
+  lp_var_t<uint16_t> adcEng0 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_0, this);
+  lp_var_t<uint16_t> adcEng1 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_1, this);
+  lp_var_t<uint16_t> adcEng2 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_2, this);
+  lp_var_t<uint16_t> adcEng3 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_3, this);
+  lp_var_t<uint16_t> adcEng4 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_4, this);
+  lp_var_t<uint16_t> adcEng5 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_5, this);
+  lp_var_t<uint16_t> adcEng6 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_6, this);
+  lp_var_t<uint16_t> adcEng7 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_7, this);
+  lp_var_t<uint16_t> adcEng8 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_8, this);
+  lp_var_t<uint16_t> adcEng9 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_9, this);
+  lp_var_t<uint16_t> adcEng10 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_10, this);
+  lp_var_t<uint16_t> adcEng11 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_11, this);
+  lp_var_t<uint16_t> adcEng12 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_12, this);
+  lp_var_t<uint16_t> adcEng13 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_13, this);
+  lp_var_t<uint16_t> adcEng14 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_14, this);
+  lp_var_t<uint16_t> adcEng15 = lp_var_t<uint16_t>(sid.objectId, PoolIds::ADC_ENG_15, this);
 
   void printSet() {
     sif::info << "---- Adc Report: Raw values ----" << std::endl;
-    for (unsigned i = 0; i < 16; i++) {
-      sif::info << "AdcReport: ADC raw " << i << ": " << std::dec << this->adcRaw[i] << std::endl;
-    }
-    for (unsigned i = 0; i < 16; i++) {
-      sif::info << "AdcReport: ADC eng " << i << ": " << std::dec << this->adcEng[i] << std::endl;
-    }
+    sif::info << "AdcReport: ADC raw 0: " << std::dec << this->adcRaw0 << std::endl;
+    sif::info << "AdcReport: ADC raw 1: " << this->adcRaw1 << std::endl;
+    sif::info << "AdcReport: ADC raw 2: " << this->adcRaw2 << std::endl;
+    sif::info << "AdcReport: ADC raw 3: " << this->adcRaw3 << std::endl;
+    sif::info << "AdcReport: ADC raw 4: " << this->adcRaw4 << std::endl;
+    sif::info << "AdcReport: ADC raw 5: " << this->adcRaw5 << std::endl;
+    sif::info << "AdcReport: ADC raw 6: " << this->adcRaw6 << std::endl;
+    sif::info << "AdcReport: ADC raw 7: " << this->adcRaw7 << std::endl;
+    sif::info << "AdcReport: ADC raw 8: " << this->adcRaw8 << std::endl;
+    sif::info << "AdcReport: ADC raw 9: " << this->adcRaw9 << std::endl;
+    sif::info << "AdcReport: ADC raw 10: " << this->adcRaw10 << std::endl;
+    sif::info << "AdcReport: ADC raw 11: " << this->adcRaw11 << std::endl;
+    sif::info << "AdcReport: ADC raw 12: " << this->adcRaw12 << std::endl;
+    sif::info << "AdcReport: ADC raw 13: " << this->adcRaw13 << std::endl;
+    sif::info << "AdcReport: ADC raw 14: " << this->adcRaw14 << std::endl;
+    sif::info << "AdcReport: ADC raw 15: " << this->adcRaw15 << std::endl;
+    sif::info << "---- Adc Report: Engineering values ----" << std::endl;
+    sif::info << "AdcReport: ADC eng 0: " << this->adcEng0 << std::endl;
+    sif::info << "AdcReport: ADC eng 1: " << this->adcEng1 << std::endl;
+    sif::info << "AdcReport: ADC eng 2: " << this->adcEng2 << std::endl;
+    sif::info << "AdcReport: ADC eng 3: " << this->adcEng3 << std::endl;
+    sif::info << "AdcReport: ADC eng 4: " << this->adcEng4 << std::endl;
+    sif::info << "AdcReport: ADC eng 5: " << this->adcEng5 << std::endl;
+    sif::info << "AdcReport: ADC eng 6: " << this->adcEng6 << std::endl;
+    sif::info << "AdcReport: ADC eng 7: " << this->adcEng7 << std::endl;
+    sif::info << "AdcReport: ADC eng 8: " << this->adcEng8 << std::endl;
+    sif::info << "AdcReport: ADC eng 9: " << this->adcEng9 << std::endl;
+    sif::info << "AdcReport: ADC eng 10: " << this->adcEng10 << std::endl;
+    sif::info << "AdcReport: ADC eng 11: " << this->adcEng11 << std::endl;
+    sif::info << "AdcReport: ADC eng 12: " << this->adcEng12 << std::endl;
+    sif::info << "AdcReport: ADC eng 13: " << this->adcEng13 << std::endl;
+    sif::info << "AdcReport: ADC eng 14: " << this->adcEng14 << std::endl;
+    sif::info << "AdcReport: ADC eng 15: " << this->adcEng15 << std::endl;
   }
 };
 
@@ -1983,7 +2045,11 @@ class EnableNvms : public TcBase {
  */
 class EnableAutoTm : public TcBase {
  public:
-  EnableAutoTm(TcParams params) : TcBase(params) { spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2); }
+  EnableAutoTm(TcParams params) : TcBase(params) {
+    spParams.setFullPayloadLen(PAYLOAD_LENGTH + 2);
+    // spParams.creator.setApid(APID_AUTO_TM);
+    // spParams.creator.setSeqCount(DEFAULT_SEQUENCE_COUNT);
+  }
 
   ReturnValue_t buildPacket() {
     auto res = checkSizeAndSerializeHeader();

linux/scheduling.cpp

@@ -47,3 +47,35 @@ void scheduling::addMpsocSupvHandlers(PeriodicTaskIF* plTask) {
   plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::SEND_READ);
   plTask->addComponent(objects::PLOC_MPSOC_HANDLER, DeviceHandlerIF::GET_READ);
 }
+
+void scheduling::scheduleScexDev(PeriodicTaskIF*& scexDevHandler) {
+  ReturnValue_t result =
+      scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::PERFORM_OPERATION);
+  if (result != returnvalue::OK) {
+    printAddObjectError("SCEX_DEV", objects::SCEX);
+  }
+  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_WRITE);
+  if (result != returnvalue::OK) {
+    printAddObjectError("SCEX_DEV", objects::SCEX);
+  }
+  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_WRITE);
+  if (result != returnvalue::OK) {
+    printAddObjectError("SCEX_DEV", objects::SCEX);
+  }
+  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ);
+  if (result != returnvalue::OK) {
+    printAddObjectError("SCEX_DEV", objects::SCEX);
+  }
+  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ);
+  if (result != returnvalue::OK) {
+    printAddObjectError("SCEX_DEV", objects::SCEX);
+  }
+  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::SEND_READ);
+  if (result != returnvalue::OK) {
+    printAddObjectError("SCEX_DEV", objects::SCEX);
+  }
+  result = scexDevHandler->addComponent(objects::SCEX, DeviceHandlerIF::GET_READ);
+  if (result != returnvalue::OK) {
+    printAddObjectError("SCEX_DEV", objects::SCEX);
+  }
+}

linux/scheduling.h

@@ -8,6 +8,7 @@ namespace scheduling {
 extern PosixThreadArgs RR_SCHEDULING;
 extern PosixThreadArgs NORMAL_SCHEDULING;
 
+void scheduleScexDev(PeriodicTaskIF*& scexDevHandler);
 void scheduleScexReader(TaskFactory& factory, PeriodicTaskIF*& scexReaderTask);
 void addMpsocSupvHandlers(PeriodicTaskIF* task);
 }  // namespace scheduling

mission/acs/RwHandler.h

@@ -59,7 +59,6 @@ class RwHandler : public DeviceHandlerBase {
   LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
 
  private:
-  static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
   //! [EXPORT] : [COMMENT] Action Message with invalid speed was received. Valid speeds must be in
   //! the range of [-65000; 1000] or [1000; 65000]
   static const ReturnValue_t INVALID_SPEED = MAKE_RETURN_CODE(0xA0);

mission/acs/defs.h

@@ -22,10 +22,10 @@ enum AcsMode : Mode_t {
 
 enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
 
-enum ControlModeStrategy : uint8_t {
-  CTRL_OFF = 0,
-  CTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
-  CTRL_NO_SENSORS_FOR_CONTROL = 2,
+enum SafeModeStrategy : uint8_t {
+  SAFECTRL_OFF = 0,
+  SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
+  SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
   // OBSW version <= v6.1.0
   LEGACY_SAFECTRL_ACTIVE_MEKF = 10,
   LEGACY_SAFECTRL_WITHOUT_MEKF = 11,
@@ -40,39 +40,20 @@ enum ControlModeStrategy : uint8_t {
   SAFECTRL_ECLIPSE_IDELING = 19,
   SAFECTRL_DETUMBLE_FULL = 20,
   SAFECTRL_DETUMBLE_DETERIORATED = 21,
-  // Added in vNext
-  PTGCTRL_MEKF = 100,
-  PTGCTRL_STR = 101,
-  PTGCTRL_QUEST = 102,
 };
-namespace gps {
-enum Source : uint8_t {
+
+enum GpsSource : uint8_t {
   NONE,
   GPS,
   GPS_EXTRAPOLATED,
   SPG4,
 };
-}
-
-namespace rotrate {
-enum Source : uint8_t {
-  NONE,
-  SUSMGM,
-  QUEST,
-  STR,
-};
-}
 
 static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
 //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
 static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
-//! [EXPORT] : [COMMENT] The limits for the rotation in pointing mode were violated.
-static constexpr Event PTG_RATE_VIOLATION = MAKE_EVENT(10, severity::MEDIUM);
-//! [EXPORT] : [COMMENT] The system has recovered from a rate rotation violation.
-static constexpr Event RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
-//! [EXPORT] : [COMMENT] The detumble transition has failed.
-//! //! P1: Last detumble state before failure.
-static constexpr Event DETUMBLE_TRANSITION_FAILED = MAKE_EVENT(11, severity::HIGH);
+//! [EXPORT] : [COMMENT] The system has recovered from a safe rate rotation violation.
+static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
 //! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes
 //! cannot be maintained.
 static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
@@ -83,17 +64,15 @@ static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
 static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO);
 //! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values.
 static constexpr Event MEKF_AUTOMATIC_RESET = MAKE_EVENT(5, severity::INFO);
-//! [EXPORT] : [COMMENT] For a prolonged time, no attitude information was available for the
-//! Pointing Controller. Falling back to Safe Mode.
-static constexpr Event PTG_CTRL_NO_ATTITUDE_INFORMATION = MAKE_EVENT(6, severity::HIGH);
+//! [EXPORT] : [COMMENT] 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(6, severity::HIGH);
 //! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has
 //! failed.
 //! P1: Missing information about magnetic field, P2: Missing information about rotational rate
 static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH);
 //! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old.
 static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO);
-//! [EXPORT] : [COMMENT] The TLE could not be read from the filesystem.
-static constexpr Event TLE_FILE_READ_FAILED = MAKE_EVENT(9, severity::LOW);
 
 extern const char* getModeStr(AcsMode mode);
 

mission/acs/str/ArcsecDatalinkLayer.cpp

@@ -32,7 +32,7 @@ ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedF
     size_t encodedDataSize = 0;
     slip_error_t slipError =
         slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx,
-                          idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID, 0);
+                          idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID);
     decodeRingBuf.deleteData(idx + 1);
     switch (slipError) {
       case (SLIP_OK): {
@@ -76,7 +76,7 @@ void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); }
 
 void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame,
                                       size_t& size) {
-  slip_encode_frame(data, length, txEncoded, sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID);
+  slip_encode_frame(data, length, txEncoded, &size, ARC_DEF_SAGITTA_SLIP_ID);
   if (txFrame != nullptr) {
     *txFrame = txEncoded;
   }

mission/acs/str/ArcsecJsonParamBase.cpp

@@ -65,7 +65,7 @@ void ArcsecJsonParamBase::addSetParamHeader(uint8_t* buffer, uint8_t setId) {
   *(buffer + 1) = setId;
 }
 
-ReturnValue_t ArcsecJsonParamBase::init(const std::string& filename) {
+ReturnValue_t ArcsecJsonParamBase::init(const std::string filename) {
   ReturnValue_t result = returnvalue::OK;
   if (not std::filesystem::exists(filename)) {
     sif::warning << "ArcsecJsonParamBase::init: JSON file " << filename << " does not exist"

mission/acs/str/ArcsecJsonParamBase.h

@@ -46,7 +46,7 @@ class ArcsecJsonParamBase {
    * @param return    JSON_FILE_NOT_EXISTS if specified file does not exist, otherwise
    *                  returnvalue::OK
    */
-  ReturnValue_t init(const std::string& filename);
+  ReturnValue_t init(const std::string filename);
 
   /**
    * @brief   Fills a buffer with a parameter set

mission/acs/str/StarTrackerHandler.cpp

@@ -5,13 +5,6 @@
 #include <mission/acs/str/strHelpers.h>
 #include <mission/acs/str/strJsonCommands.h>
 
-#include <string>
-
-#include "fsfw/filesystem/HasFileSystemIF.h"
-#include "fsfw/ipc/MessageQueueIF.h"
-#include "fsfw/returnvalues/returnvalue.h"
-#include "mission/memory/SdCardMountedIF.h"
-
 extern "C" {
 #include <sagitta/client/actionreq.h>
 #include <sagitta/client/client_tm_structs.h>
@@ -21,6 +14,7 @@ extern "C" {
 }
 
 #include <atomic>
+#include <fstream>
 #include <thread>
 
 #include "OBSWConfig.h"
@@ -30,9 +24,8 @@ extern "C" {
 std::atomic_bool JCFG_DONE(false);
 
 StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
-                                       StrComHandler* strHelper, power::Switch_t powerSwitch,
-                                       startracker::SdCardConfigPathGetter& cfgPathGetter,
-                                       SdCardMountedIF& sdCardIF)
+                                       const char* jsonFileStr, StrComHandler* strHelper,
+                                       power::Switch_t powerSwitch)
     : DeviceHandlerBase(objectId, comIF, comCookie),
       temperatureSet(this),
       versionSet(this),
@@ -63,11 +56,9 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
       centroidSet(this),
       centroidsSet(this),
       contrastSet(this),
-      blobStatsSet(this),
       strHelper(strHelper),
-      powerSwitch(powerSwitch),
-      sdCardIF(sdCardIF),
-      cfgPathGetter(cfgPathGetter) {
+      paramJsonFile(jsonFileStr),
+      powerSwitch(powerSwitch) {
   if (comCookie == nullptr) {
     sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
   }
@@ -91,12 +82,17 @@ void StarTrackerHandler::doStartUp() {
       // the device handler's submode to the star tracker's mode
       return;
     case StartupState::DONE:
-      if (!JCFG_DONE) {
+      if (jcfgCountdown.isBusy()) {
         startupState = StartupState::WAIT_JCFG;
         return;
       }
+      startupState = StartupState::IDLE;
       break;
     case StartupState::WAIT_JCFG: {
+      if (jcfgCountdown.hasTimedOut()) {
+        startupState = StartupState::IDLE;
+        break;
+      }
       return;
     }
     default:
@@ -143,10 +139,8 @@ ReturnValue_t StarTrackerHandler::initialize() {
 
   // Spin up a thread to do the JSON initialization, takes 200-250 ms which would
   // delay whole satellite boot process.
-  reloadJsonCfgFile();
-
-  // Default firmware target is always initialized from persistent file.
-  loadTargetFirmwareFromPersistentCfg();
+  jcfgCountdown.resetTimer();
+  jsonCfgTask = std::thread{setUpJsonCfgs, std::ref(jcfgs), paramJsonFile.c_str()};
 
   EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
   if (manager == nullptr) {
@@ -175,33 +169,6 @@ ReturnValue_t StarTrackerHandler::initialize() {
   return returnvalue::OK;
 }
 
-void StarTrackerHandler::loadTargetFirmwareFromPersistentCfg() {
-  const char* prefix = sdCardIF.getCurrentMountPrefix();
-  std::filesystem::path path = std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
-  std::ifstream ifile(path);
-  if (ifile.is_open() and !ifile.bad()) {
-    std::string targetStr;
-    std::getline(ifile, targetStr);
-    if (targetStr == "backup") {
-      firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP);
-    }
-  }
-}
-
-bool StarTrackerHandler::reloadJsonCfgFile() {
-  jcfgCountdown.resetTimer();
-  auto strCfgPath = cfgPathGetter.getCfgPath();
-  if (strCfgPath.has_value()) {
-    jcfgPending = true;
-    JCFG_DONE = false;
-    jsonCfgTask = std::thread{setUpJsonCfgs, std::ref(jcfgs), strCfgPath.value()};
-    return true;
-  }
-  // Simplified FDIR: Just continue as usual..
-  JCFG_DONE = true;
-  return false;
-}
-
 ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                                 const uint8_t* data, size_t size) {
   ReturnValue_t result = returnvalue::OK;
@@ -330,11 +297,21 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
       strHelper->setFlashReadFilename(std::string(reinterpret_cast<const char*>(data), size));
       return EXECUTION_FINISHED;
     }
-    case (startracker::FIRMWARE_UPDATE_MAIN): {
-      return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::MAIN);
-    }
-    case (startracker::FIRMWARE_UPDATE_BACKUP): {
-      return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::BACKUP);
+    case (startracker::FIRMWARE_UPDATE): {
+      result = DeviceHandlerBase::acceptExternalDeviceCommands();
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
+        return FILE_PATH_TOO_LONG;
+      }
+      result =
+          strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size));
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      strHelperHandlingSpecialRequest = true;
+      return EXECUTION_FINISHED;
     }
     default:
       break;
@@ -343,23 +320,6 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
   reinitNextSetParam = true;
   return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size);
 }
-ReturnValue_t StarTrackerHandler::handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
-                                                              startracker::FirmwareTarget target) {
-  ReturnValue_t result = DeviceHandlerBase::acceptExternalDeviceCommands();
-  if (result != returnvalue::OK) {
-    return result;
-  }
-  if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
-    return FILE_PATH_TOO_LONG;
-  }
-  result = strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size),
-                                          target);
-  if (result != returnvalue::OK) {
-    return result;
-  }
-  strHelperHandlingSpecialRequest = true;
-  return EXECUTION_FINISHED;
-}
 
 void StarTrackerHandler::performOperationHook() {
   EventMessage event;
@@ -375,24 +335,6 @@ void StarTrackerHandler::performOperationHook() {
         break;
     }
   }
-  if (jcfgPending) {
-    if (JCFG_DONE) {
-      if (startupState == StartupState::WAIT_JCFG) {
-        startupState = StartupState::DONE;
-      }
-      jsonCfgTask.join();
-      jcfgPending = false;
-      auto iter = deviceCommandMap.find(startracker::RELOAD_JSON_CFG_FILE);
-      if (iter != deviceCommandMap.end() and iter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) {
-        actionHelper.finish(true, iter->second.sendReplyTo, startracker::RELOAD_JSON_CFG_FILE);
-      }
-    } else if (jcfgCountdown.hasTimedOut()) {
-      auto iter = deviceCommandMap.find(startracker::RELOAD_JSON_CFG_FILE);
-      if (iter != deviceCommandMap.end() and iter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) {
-        actionHelper.finish(false, iter->second.sendReplyTo, startracker::RELOAD_JSON_CFG_FILE);
-      }
-    }
-  }
 }
 
 Submode_t StarTrackerHandler::getInitialSubmode() { return startracker::SUBMODE_BOOTLOADER; }
@@ -557,23 +499,13 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       preparePingRequest();
       return returnvalue::OK;
     }
-    case (startracker::RELOAD_JSON_CFG_FILE): {
-      if (jcfgPending) {
-        return HasActionsIF::IS_BUSY;
-      }
-      // It should be noted that this just reloads the JSON config structure in memory from the
-      // JSON file. The configuration still needs to be sent to the STR. The easiest way to achieve
-      // this is to simply reboot the device after a reload.
-      reloadJsonCfgFile();
-      return returnvalue::OK;
-    }
     case (startracker::SET_TIME_FROM_SYS_TIME): {
       SetTimeActionRequest setTimeRequest{};
       timeval tv;
       Clock::getClock(&tv);
       setTimeRequest.unixTime =
           (static_cast<uint64_t>(tv.tv_sec) * 1000 * 1000) + (static_cast<uint64_t>(tv.tv_usec));
-      prv_arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen);
+      arc_pack_settime_action_req(&setTimeRequest, commandBuffer, &rawPacketLen);
       size_t serLen = 0;
       // Time in milliseconds. Manual serialization because arcsec API ignores endianness.
       SerializeAdapter::serialize(&setTimeRequest.unixTime, commandBuffer + 2, &serLen,
@@ -581,7 +513,6 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       rawPacket = commandBuffer;
       return returnvalue::OK;
     }
-
     case (startracker::REQ_TIME): {
       prepareTimeRequest();
       return returnvalue::OK;
@@ -598,12 +529,8 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       prepareRequestContrastTm();
       return returnvalue::OK;
     }
-    case (startracker::REQ_BLOB_STATS): {
-      prepareRequestBlobStatsTm();
-      return returnvalue::OK;
-    }
     case (startracker::BOOT): {
-      prepareBootCommand(static_cast<startracker::FirmwareTarget>(firmwareTargetRaw));
+      prepareBootCommand();
       return returnvalue::OK;
     }
     case (startracker::REQ_VERSION): {
@@ -799,7 +726,6 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
   this->insertInCommandMap(startracker::UPLOAD_IMAGE);
   this->insertInCommandMap(startracker::DOWNLOAD_IMAGE);
-  this->insertInCommandMap(startracker::RELOAD_JSON_CFG_FILE);
   this->insertInCommandAndReplyMap(startracker::REQ_POWER, 3, &powerSet,
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
   this->insertInCommandAndReplyMap(startracker::REQ_INTERFACE, 3, &interfaceSet,
@@ -890,8 +816,6 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
   this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet,
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
-  this->insertInCommandAndReplyMap(startracker::REQ_BLOB_STATS, 3, &blobStatsSet,
-                                   startracker::MAX_FRAME_SIZE * 2 + 2);
 }
 
 ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
@@ -1004,7 +928,7 @@ void StarTrackerHandler::bootFirmware(Mode_t toMode) {
   }
 }
 
-void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, std::string paramJsonFile) {
+void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonFile) {
   cfgs.tracking.init(paramJsonFile);
   cfgs.logLevel.init(paramJsonFile);
   cfgs.logSubscription.init(paramJsonFile);
@@ -1089,12 +1013,6 @@ void StarTrackerHandler::resetSecondaryTmSet() {
       histogramSet.setValidity(false, true);
     }
   }
-  {
-    PoolReadGuard pg(&blobStatsSet);
-    if (pg.getReadResult() == returnvalue::OK) {
-      histogramSet.setValidity(false, true);
-    }
-  }
 }
 
 void StarTrackerHandler::bootBootloader() {
@@ -1204,7 +1122,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       break;
     }
     case (startracker::REQ_SOLUTION): {
-      result = handleSolution(packet);
+      result = handleTm(packet, solutionSet, "REQ_SOLUTION");
       break;
     }
     case (startracker::REQ_CONTRAST): {
@@ -1243,10 +1161,6 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       result = handleTm(packet, histogramSet, "REQ_HISTO");
       break;
     }
-    case (startracker::REQ_BLOB_STATS): {
-      result = handleTm(packet, blobStatsSet, "REQ_BLOB_STATS");
-      break;
-    }
     case (startracker::SUBSCRIPTION):
     case (startracker::LOGLEVEL):
     case (startracker::LOGSUBSCRIPTION):
@@ -1648,13 +1562,6 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
   localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry<uint32_t>(9));
   localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry<uint32_t>(9));
 
-  localDataPoolMap.emplace(startracker::TICKS_BLOB_STATS, new PoolEntry<uint32_t>());
-  localDataPoolMap.emplace(startracker::TIME_BLOB_STATS, new PoolEntry<uint64_t>());
-  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_NOISE, new PoolEntry<uint8_t>(16));
-  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_THOLD, new PoolEntry<uint8_t>(16));
-  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_LVALID, new PoolEntry<uint8_t>(16));
-  localDataPoolMap.emplace(startracker::PoolIds::BLOB_STATS_OFLOW, new PoolEntry<uint8_t>(16));
-
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
@@ -1683,8 +1590,6 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
       subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0));
-  poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(blobStatsSet.getSid(), false, 10.0));
   return returnvalue::OK;
 }
 
@@ -1714,8 +1619,7 @@ ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) {
     case startracker::UPLOAD_IMAGE:
     case startracker::DOWNLOAD_IMAGE:
     case startracker::FLASH_READ:
-    case startracker::FIRMWARE_UPDATE_BACKUP:
-    case startracker::FIRMWARE_UPDATE_MAIN: {
+    case startracker::FIRMWARE_UPDATE: {
       return DeviceHandlerBase::acceptExternalDeviceCommands();
       default:
         break;
@@ -1931,10 +1835,6 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandI
       *foundId = startracker::REQ_BLOB;
       break;
     }
-    case (startracker::ID::BLOB_STATS): {
-      *foundId = startracker::REQ_BLOB_STATS;
-      break;
-    }
     case (startracker::ID::BLOBS): {
       *foundId = startracker::REQ_BLOBS;
       break;
@@ -2016,10 +1916,10 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command
   return result;
 }
 
-void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) {
+void StarTrackerHandler::prepareBootCommand() {
   uint32_t length = 0;
-  struct BootActionRequest bootRequest = {static_cast<uint8_t>(target)};
-  prv_arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
+  struct BootActionRequest bootRequest = {BOOT_REGION_ID};
+  arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
   rawPacket = commandBuffer;
   rawPacketLen = length;
 }
@@ -2052,7 +1952,7 @@ ReturnValue_t StarTrackerHandler::prepareChecksumCommand(const uint8_t* commandD
     return result;
   }
   uint32_t rawCmdLength = 0;
-  prv_arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
+  arc_pack_checksum_action_req(&req, commandBuffer, &rawCmdLength);
   rawPacket = commandBuffer;
   rawPacketLen = rawCmdLength;
   checksumCmd.rememberRegion = req.region;
@@ -2071,7 +1971,7 @@ void StarTrackerHandler::prepareTimeRequest() {
 void StarTrackerHandler::preparePingRequest() {
   uint32_t length = 0;
   struct PingActionRequest pingRequest = {PING_ID};
-  prv_arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
+  arc_pack_ping_action_req(&pingRequest, commandBuffer, &length);
   rawPacket = commandBuffer;
   rawPacketLen = length;
 }
@@ -2100,7 +2000,7 @@ void StarTrackerHandler::preparePowerRequest() {
 void StarTrackerHandler::prepareSwitchToBootloaderCmd() {
   uint32_t length = 0;
   struct RebootActionRequest rebootReq {};
-  prv_arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
+  arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length);
   rawPacket = commandBuffer;
   rawPacketLen = length;
 }
@@ -2109,7 +2009,7 @@ void StarTrackerHandler::prepareTakeImageCommand(const uint8_t* commandData) {
   uint32_t length = 0;
   struct CameraActionRequest camReq;
   camReq.actionid = *commandData;
-  prv_arc_pack_camera_action_req(&camReq, commandBuffer, &length);
+  arc_pack_camera_action_req(&camReq, commandBuffer, &length);
   rawPacket = commandBuffer;
   rawPacketLen = length;
 }
@@ -2175,14 +2075,6 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() {
   return returnvalue::OK;
 }
 
-ReturnValue_t StarTrackerHandler::prepareRequestBlobStatsTm() {
-  uint32_t length = 0;
-  arc_tm_pack_blobstats_req(commandBuffer, &length);
-  rawPacket = commandBuffer;
-  rawPacketLen = length;
-  return returnvalue::OK;
-}
-
 ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() {
   uint32_t length = 0;
   arc_tm_pack_centroids_req(commandBuffer, &length);
@@ -2457,8 +2349,7 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
         internalState = InternalState::DONE;
       }
       break;
-    case startracker::Program::FIRMWARE_BACKUP:
-    case startracker::Program::FIRMWARE_MAIN: {
+    case startracker::Program::FIRMWARE:
       if (startupState == StartupState::WAIT_CHECK_PROGRAM) {
         startupState = StartupState::BOOT_BOOTLOADER;
       }
@@ -2469,10 +2360,9 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
         internalState = InternalState::FAILED_BOOTLOADER_BOOT;
       }
       break;
-    }
     default:
-      sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID "
-                   << static_cast<int>(versionSet.program.value) << std::endl;
+      sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID"
+                   << std::endl;
       return INVALID_PROGRAM;
   }
   return returnvalue::OK;
@@ -2508,36 +2398,6 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
   return result;
 }
 
-ReturnValue_t StarTrackerHandler::handleSolution(const uint8_t* rawFrame) {
-  ReturnValue_t result = statusFieldCheck(rawFrame);
-  if (result != returnvalue::OK) {
-    return result;
-  }
-  PoolReadGuard pg(&solutionSet);
-  if (pg.getReadResult() != returnvalue::OK) {
-    return result;
-  }
-  const uint8_t* reply = rawFrame + TICKS_OFFSET;
-  solutionSet.setValidityBufferGeneration(false);
-  size_t sizeLeft = fullPacketLen;
-  result = solutionSet.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE);
-  if (result != returnvalue::OK) {
-    sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for solution set: 0x"
-                 << std::hex << std::setw(4) << result << std::dec << std::endl;
-  }
-  solutionSet.setValidityBufferGeneration(true);
-  solutionSet.setValidity(true, true);
-  solutionSet.caliQw.setValid(solutionSet.isTrustWorthy.value);
-  solutionSet.caliQx.setValid(solutionSet.isTrustWorthy.value);
-  solutionSet.caliQy.setValid(solutionSet.isTrustWorthy.value);
-  solutionSet.caliQz.setValid(solutionSet.isTrustWorthy.value);
-  solutionSet.trackQw.setValid(solutionSet.isTrustWorthy.value);
-  solutionSet.trackQx.setValid(solutionSet.isTrustWorthy.value);
-  solutionSet.trackQy.setValid(solutionSet.isTrustWorthy.value);
-  solutionSet.trackQz.setValid(solutionSet.isTrustWorthy.value);
-  return result;
-}
-
 ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) {
   ReturnValue_t result = statusFieldCheck(rawFrame);
   if (result != returnvalue::OK) {
@@ -2932,19 +2792,17 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
     case startracker::REQ_MATCHED_CENTROIDS:
     case startracker::REQ_BLOB:
     case startracker::REQ_BLOBS:
-    case startracker::REQ_BLOB_STATS:
     case startracker::REQ_CENTROID:
     case startracker::REQ_CENTROIDS:
     case startracker::REQ_CONTRAST: {
-      if (getMode() == MODE_ON and getSubmode() != startracker::SUBMODE_FIRMWARE) {
+      if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
         return STARTRACKER_NOT_RUNNING_FIRMWARE;
       }
       break;
     }
-    case startracker::FIRMWARE_UPDATE_MAIN:
-    case startracker::FIRMWARE_UPDATE_BACKUP:
+    case startracker::FIRMWARE_UPDATE:
     case startracker::FLASH_READ:
-      if (getMode() != MODE_ON or getSubmode() != startracker::SUBMODE_BOOTLOADER) {
+      if (getMode() != MODE_ON or getSubmode() != startracker::Program::BOOTLOADER) {
         return STARTRACKER_NOT_RUNNING_BOOTLOADER;
       }
       break;
@@ -2953,44 +2811,3 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
   }
   return returnvalue::OK;
 }
-
-ReturnValue_t StarTrackerHandler::acceptExternalDeviceCommands() { return returnvalue::OK; }
-
-ReturnValue_t StarTrackerHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
-                                               ParameterWrapper* parameterWrapper,
-                                               const ParameterWrapper* newValues,
-                                               uint16_t startAtIndex) {
-  auto firmwareTargetUpdate = [&](bool persistent) {
-    uint8_t value = 0;
-    newValues->getElement(&value);
-    if (value != static_cast<uint8_t>(startracker::FirmwareTarget::MAIN) &&
-        value != static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP)) {
-      return HasParametersIF::INVALID_VALUE;
-    }
-    parameterWrapper->set(firmwareTargetRaw);
-    if (persistent) {
-      if (sdCardIF.isSdCardUsable(std::nullopt)) {
-        const char* prefix = sdCardIF.getCurrentMountPrefix();
-        std::filesystem::path path =
-            std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
-        std::ofstream of(path, std::ofstream::out | std::ofstream::trunc);
-        if (value == static_cast<uint8_t>(startracker::FirmwareTarget::MAIN)) {
-          of << "main\n";
-        } else {
-          of << "backup\n";
-        }
-      } else {
-        return HasFileSystemIF::FILESYSTEM_INACTIVE;
-      }
-    };
-    return returnvalue::OK;
-  };
-  if (uniqueId == startracker::ParamId::FIRMWARE_TARGET) {
-    return firmwareTargetUpdate(false);
-  }
-  if (uniqueId == startracker::ParamId::FIRMWARE_TARGET_PERSISTENT) {
-    return firmwareTargetUpdate(true);
-  }
-  return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
-                                         startAtIndex);
-}

mission/acs/str/StarTrackerHandler.h

@@ -11,7 +11,10 @@
 #include <set>
 #include <thread>
 
+#include "OBSWConfig.h"
+#include "devices/powerSwitcherList.h"
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
+#include "fsfw/src/fsfw/serialize/SerializeAdapter.h"
 #include "fsfw/timemanager/Countdown.h"
 
 extern "C" {
@@ -24,9 +27,7 @@ extern "C" {
  * @details Datasheet: https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
  *                     Arbeitsdaten/08_Used%20Components/ArcSec_KULeuven_Startracker/
  *                     Sagitta%201.0%20Datapack&fileid=659181
- * @note The STR code is a chaotic inconsistent mess and should be re-written with a simpler base
- *       class. DO NOT USE THIS AS REFERENCE. Stay away from it.
- * @author	J. Meier, R. Mueller
+ * @author	J. Meier
  */
 class StarTrackerHandler : public DeviceHandlerBase {
  public:
@@ -41,8 +42,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
    * 						to high to enable the device.
    */
   StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
-                     StrComHandler* strHelper, power::Switch_t powerSwitch,
-                     startracker::SdCardConfigPathGetter& cfgPathGetter, SdCardMountedIF& sdCardIF);
+                     const char* jsonFileStr, StrComHandler* strHelper,
+                     power::Switch_t powerSwitch);
   virtual ~StarTrackerHandler();
 
   ReturnValue_t initialize() override;
@@ -58,9 +59,6 @@ class StarTrackerHandler : public DeviceHandlerBase {
 
   Submode_t getInitialSubmode() override;
 
-  ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
-                             const ParameterWrapper* newValues, uint16_t startAtIndex) override;
-
  protected:
   void doStartUp() override;
   void doShutDown() override;
@@ -161,8 +159,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
 
   // Ping request will reply ping with this ID (data field)
   static const uint32_t PING_ID = 0x55;
-  uint8_t firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::MAIN);
-
+  static const uint32_t BOOT_REGION_ID = 1;
   static const MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
   static const uint32_t MUTEX_TIMEOUT = 20;
   static const uint32_t BOOT_TIMEOUT = 1000;
@@ -216,7 +213,6 @@ class StarTrackerHandler : public DeviceHandlerBase {
   startracker::CentroidSet centroidSet;
   startracker::CentroidsSet centroidsSet;
   startracker::ContrastSet contrastSet;
-  startracker::BlobStatsSet blobStatsSet;
 
   // Pointer to object responsible for uploading and downloading images to/from the star tracker
   StrComHandler* strHelper = nullptr;
@@ -244,12 +240,11 @@ class StarTrackerHandler : public DeviceHandlerBase {
     Subscription subscription;
     AutoThreshold autoThreshold;
   };
-  bool jcfgPending = false;
   JsonConfigs jcfgs;
-  Countdown jcfgCountdown = Countdown(1000);
+  Countdown jcfgCountdown = Countdown(250);
   bool commandExecuted = false;
   std::thread jsonCfgTask;
-  static void setUpJsonCfgs(JsonConfigs& cfgs, std::string paramJsonFile);
+  static void setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonFile);
 
   std::string paramJsonFile;
 
@@ -316,14 +311,10 @@ class StarTrackerHandler : public DeviceHandlerBase {
   std::set<DeviceCommandId_t> additionalRequestedTm{};
   std::set<DeviceCommandId_t>::iterator currentSecondaryTmIter;
 
-  SdCardMountedIF& sdCardIF;
-  startracker::SdCardConfigPathGetter& cfgPathGetter;
-
   /**
    * @brief   Handles internal state
    */
   void handleInternalState();
-  void loadTargetFirmwareFromPersistentCfg();
 
   /**
    * @brief   Checks mode for commands requiring MODE_ON of MODE_NORMAL for execution.
@@ -384,7 +375,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
    * @brief   Fills command buffer with data to boot image (works only when star tracker is
    *          in bootloader mode).
    */
-  void prepareBootCommand(startracker::FirmwareTarget target);
+  void prepareBootCommand();
 
   /**
    * @brief   Fills command buffer with command to get the checksum of a flash part
@@ -471,7 +462,6 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t prepareRequestCentroidTm();
   ReturnValue_t prepareRequestCentroidsTm();
   ReturnValue_t prepareRequestContrastTm();
-  ReturnValue_t prepareRequestBlobStatsTm();
   ReturnValue_t prepareRequestTrackingParams();
   ReturnValue_t prepareRequestValidationParams();
   ReturnValue_t prepareRequestAlgoParams();
@@ -532,7 +522,6 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
                          const char* context);
 
-  ReturnValue_t handleSolution(const uint8_t* rawFrame);
   ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame);
   ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
   ReturnValue_t handleBlobTm(const uint8_t* rawFrame);
@@ -553,11 +542,6 @@ class StarTrackerHandler : public DeviceHandlerBase {
   void doNormalTransition(Mode_t modeFrom, Submode_t subModeFrom);
   void bootFirmware(Mode_t toMode);
   void bootBootloader();
-  bool reloadJsonCfgFile();
-  ReturnValue_t acceptExternalDeviceCommands() override;
-
-  ReturnValue_t handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
-                                            startracker::FirmwareTarget target);
 };
 
 #endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */

mission/acs/str/strHelpers.h

@@ -7,25 +7,13 @@
 #include <fsfw/devicehandlers/DeviceHandlerIF.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
 
-#include <optional>
+#include "objects/systemObjectList.h"
 
 namespace startracker {
 
 static const Submode_t SUBMODE_BOOTLOADER = 1;
 static const Submode_t SUBMODE_FIRMWARE = 2;
 
-enum class FirmwareTarget : uint8_t { MAIN = 1, BACKUP = 10 };
-
-static constexpr char FW_TARGET_CFG_PATH[] = "startracker/fw-target.txt";
-
-enum ParamId : uint32_t { FIRMWARE_TARGET = 1, FIRMWARE_TARGET_PERSISTENT = 2 };
-
-class SdCardConfigPathGetter {
- public:
-  virtual ~SdCardConfigPathGetter() = default;
-  virtual std::optional<std::string> getCfgPath() = 0;
-};
-
 /**
  * @brief    Returns the frame type field of a decoded frame.
  */
@@ -328,13 +316,6 @@ enum PoolIds : lp_id_t {
   CONTRAST_B,
   CONTRAST_C,
   CONTRAST_D,
-
-  TICKS_BLOB_STATS,
-  TIME_BLOB_STATS,
-  BLOB_STATS_NOISE,
-  BLOB_STATS_THOLD,
-  BLOB_STATS_LVALID,
-  BLOB_STATS_OFLOW,
 };
 
 static const DeviceCommandId_t PING_REQUEST = 0;
@@ -386,7 +367,7 @@ static const DeviceCommandId_t REQ_DEBUG_CAMERA = 80;
 static const DeviceCommandId_t LOGLEVEL = 81;
 static const DeviceCommandId_t LOGSUBSCRIPTION = 82;
 static const DeviceCommandId_t DEBUG_CAMERA = 83;
-static const DeviceCommandId_t FIRMWARE_UPDATE_MAIN = 84;
+static const DeviceCommandId_t FIRMWARE_UPDATE = 84;
 static const DeviceCommandId_t DISABLE_TIMESTAMP_GENERATION = 85;
 static const DeviceCommandId_t ENABLE_TIMESTAMP_GENERATION = 86;
 static constexpr DeviceCommandId_t SET_TIME_FROM_SYS_TIME = 87;
@@ -400,10 +381,6 @@ static constexpr DeviceCommandId_t REQ_CENTROIDS = 94;
 static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 95;
 static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 96;
 static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97;
-static constexpr DeviceCommandId_t RELOAD_JSON_CFG_FILE = 100;
-static constexpr DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101;
-static constexpr DeviceCommandId_t REQ_BLOB_STATS = 102;
-
 static const DeviceCommandId_t NONE = 0xFFFFFFFF;
 
 static const uint32_t VERSION_SET_ID = REQ_VERSION;
@@ -435,7 +412,6 @@ static const uint32_t BLOBS_SET_ID = REQ_BLOBS;
 static const uint32_t CENTROID_SET_ID = REQ_CENTROID;
 static const uint32_t CENTROIDS_SET_ID = REQ_CENTROIDS;
 static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST;
-static const uint32_t BLOB_STATS_SET_ID = REQ_BLOB_STATS;
 
 /** Max size of unencoded frame */
 static const size_t MAX_FRAME_SIZE = 1200;
@@ -502,13 +478,11 @@ static constexpr uint8_t CENTROID = 26;
 static constexpr uint8_t CENTROIDS = 37;
 static constexpr uint8_t AUTO_BLOB = 39;
 static constexpr uint8_t MATCHED_CENTROIDS = 40;
-static constexpr uint8_t BLOB_STATS = 49;
 }  // namespace ID
 
 namespace Program {
 static const uint8_t BOOTLOADER = 1;
-static const uint8_t FIRMWARE_MAIN = 2;
-static const uint8_t FIRMWARE_BACKUP = 3;
+static const uint8_t FIRMWARE = 2;
 }  // namespace Program
 
 namespace region_secrets {
@@ -528,7 +502,7 @@ static const uint32_t REGION_12_SECRET = 0x42fedef6;
 static const uint32_t REGION_13_SECRET = 0xe53cf10d;
 static const uint32_t REGION_14_SECRET = 0xe862b70b;
 static const uint32_t REGION_15_SECRET = 0x79b537ca;
-static const uint32_t SECRETS[16]{
+static const uint32_t secret[16]{
     REGION_0_SECRET,  REGION_1_SECRET,  REGION_2_SECRET,  REGION_3_SECRET,
     REGION_4_SECRET,  REGION_5_SECRET,  REGION_6_SECRET,  REGION_7_SECRET,
     REGION_8_SECRET,  REGION_9_SECRET,  REGION_10_SECRET, REGION_11_SECRET,
@@ -557,12 +531,7 @@ enum class FlashSections : uint8_t {
 };
 
 // Flash region IDs of firmware partition
-enum class FirmwareRegions : uint32_t {
-  FIRST_MAIN = 1,
-  LAST_MAIN = 8,
-  FIRST_BACKUP = 10,
-  LAST_BACKUP = 16
-};
+enum class FirmwareRegions : uint32_t { FIRST = 1, LAST = 8 };
 
 static const uint32_t FLASH_REGION_SIZE = 0x20000;
 
@@ -1569,23 +1538,6 @@ class CentroidsSet : public StaticLocalDataSet<10> {
       lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this);
 };
 
-class BlobStatsSet : public StaticLocalDataSet<6> {
- public:
-  BlobStatsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOB_STATS_SET_ID) {}
-
-  // Data received from the Centroids Telemetry Set (ID 49)
-  lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_BLOB_STATS, this);
-  lp_var_t<uint64_t> time = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_BLOB_STATS, this);
-  lp_vec_t<uint8_t, 16> noise =
-      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_NOISE, this);
-  lp_vec_t<uint8_t, 16> thold =
-      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_THOLD, this);
-  lp_vec_t<uint8_t, 16> lvalid =
-      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_LVALID, this);
-  lp_vec_t<uint8_t, 16> oflow =
-      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::BLOB_STATS_OFLOW, this);
-};
-
 class ContrastSet : public StaticLocalDataSet<8> {
  public:
   ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {}

mission/com/LiveTmTask.cpp

@@ -54,13 +54,8 @@ ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
           }
         }
       } else if (result != MessageQueueIF::EMPTY) {
-        const char* contextStr = "Regular TM queue";
-        if (isCfdp) {
-          contextStr = "CFDP TM queue";
-        }
-        sif::warning << "LiveTmTask: " << contextStr << " handling failure, returncode 0x"
-                     << std::setfill('0') << std::hex << std::setw(4) << result << std::dec
-                     << std::endl;
+        sif::warning << "LiveTmTask: TM queue failure, returncode 0x" << std::hex << std::setw(4)
+                     << result << std::dec << std::endl;
       }
     }
 
@@ -178,16 +173,15 @@ ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue, bool isCfd
     size_t writtenSize = 0;
     result = channel.write(data, size, writtenSize);
     if (result == DirectTmSinkIF::PARTIALLY_WRITTEN) {
-      result = channel.handleWriteCompletionSynchronously(writtenSize, 400);
+      result = channel.handleWriteCompletionSynchronously(writtenSize, 200);
       if (result != returnvalue::OK) {
         // TODO: Event? Might lead to dangerous spam though..
         sif::warning << "LiveTmTask: Synchronous write of last segment failed with code 0x"
-                     << std::setfill('0') << std::setw(4) << std::hex << result << std::dec
-                     << std::endl;
+                     << std::setw(4) << std::hex << result << std::dec << std::endl;
       }
     } else if (result != returnvalue::OK) {
-      sif::error << "LiveTmTask: Channel write failed with code 0x" << std::setfill('0') << std::hex
-                 << std::setw(4) << result << std::dec << std::endl;
+      sif::error << "LiveTmTask: Channel write failed with code 0x" << std::hex << std::setw(4)
+                 << result << std::dec << std::endl;
     }
   }
   // Try delete in any case, ignore failures (which should not happen), it is more important to

mission/com/TmStoreTaskBase.cpp

@@ -141,12 +141,11 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store,
   size_t writtenSize = 0;
   result = channel.write(tmReader.getFullData(), dumpedLen, writtenSize);
   if (result == VirtualChannelIF::PARTIALLY_WRITTEN) {
-    result = channel.handleWriteCompletionSynchronously(writtenSize, 400);
+    result = channel.handleWriteCompletionSynchronously(writtenSize, 200);
     if (result != returnvalue::OK) {
       // TODO: Event? Might lead to dangerous spam though..
-      sif::warning << "LiveTmTask: Synchronous write of last segment failed with code 0x"
-                   << std::setfill('0') << std::setw(4) << std::hex << result << std::dec
-                   << std::endl;
+      sif::warning << "PersistentTmStore: Synchronous write of last segment failed with code 0x"
+                   << std::setw(4) << std::hex << result << std::dec << std::endl;
     }
   } else if (result == DirectTmSinkIF::IS_BUSY) {
     sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl;

mission/com/VirtualChannel.cpp

@@ -74,7 +74,5 @@ ReturnValue_t VirtualChannel::handleWriteCompletionSynchronously(size_t& written
       return result;
     }
   }
-  // Timeout. Cancel the transfer
-  cancelTransfer();
-  return TIMEOUT;
+  return returnvalue::FAILED;
 }

mission/com/VirtualChannelWithQueue.cpp

@@ -41,7 +41,7 @@ ReturnValue_t VirtualChannelWithQueue::handleNextTm(bool performWriteOp) {
   if (performWriteOp) {
     result = write(data, size, writtenSize);
     if (result == PARTIALLY_WRITTEN) {
-      result = handleWriteCompletionSynchronously(writtenSize, 400);
+      result = handleWriteCompletionSynchronously(writtenSize, 200);
       if (result != returnvalue::OK) {
         // TODO: Event? Might lead to dangerous spam though..
         sif::warning

mission/controller/AcsController.cpp

@@ -1,12 +1,13 @@
 #include "AcsController.h"
 
-AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF &sdcMan)
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <mission/acs/defs.h>
+#include <mission/config/torquer.h>
+
+AcsController::AcsController(object_id_t objectId, bool enableHkSets)
     : ExtendedControllerBase(objectId),
       enableHkSets(enableHkSets),
-      sdcMan(sdcMan),
-      attitudeEstimation(&acsParameters),
       fusedRotationEstimation(&acsParameters),
-      navigation(&acsParameters),
       guidance(&acsParameters),
       safeCtrl(&acsParameters),
       ptgCtrl(&acsParameters),
@@ -18,11 +19,11 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
       gyrDataRaw(this),
       gyrDataProcessed(this),
       gpsDataProcessed(this),
-      attitudeEstimationData(this),
+      mekfData(this),
       ctrlValData(this),
       actuatorCmdData(this),
       fusedRotRateData(this),
-      fusedRotRateSourcesData(this) {}
+      tleData(this) {}
 
 ReturnValue_t AcsController::initialize() {
   ReturnValue_t result = parameterHelper.initialize();
@@ -50,12 +51,12 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
     case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
       ReturnValue_t result = guidance.solarArrayDeploymentComplete();
       if (result == returnvalue::FAILED) {
-        return acsctrl::FILE_DELETION_FAILED;
+        return FILE_DELETION_FAILED;
       }
       return HasActionsIF::EXECUTION_FINISHED;
     }
     case RESET_MEKF: {
-      navigation.resetMekf(&attitudeEstimationData);
+      navigation.resetMekf(&mekfData);
       return HasActionsIF::EXECUTION_FINISHED;
     }
     case RESTORE_MEKF_NONFINITE_RECOVERY: {
@@ -64,32 +65,24 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
     }
     case UPDATE_TLE: {
       if (size != 69 * 2) {
-        return HasActionsIF::INVALID_PARAMETERS;
+        return INVALID_PARAMETERS;
       }
-      ReturnValue_t result = updateTle(data, data + 69, false);
+      ReturnValue_t result = navigation.updateTle(data, data + 69);
       if (result != returnvalue::OK) {
+        PoolReadGuard pg(&tleData);
+        navigation.updateTle(tleData.line1.value, tleData.line2.value);
         return result;
       }
-      result = writeTleToFs(data);
-      if (result != returnvalue::OK) {
-        return result;
+      {
+        PoolReadGuard pg(&tleData);
+        if (pg.getReadResult() == returnvalue::OK) {
+          std::memcpy(tleData.line1.value, data, 69);
+          std::memcpy(tleData.line2.value, data + 69, 69);
+          tleData.setValidity(true, true);
+        }
       }
       return HasActionsIF::EXECUTION_FINISHED;
     }
-    case (READ_TLE): {
-      uint8_t tle[69 * 2] = {};
-      uint8_t line2[69] = {};
-      ReturnValue_t result = readTleFromFs(tle, line2);
-      if (result != returnvalue::OK) {
-        return result;
-      }
-      std::memcpy(tle + 69, line2, 69);
-      actionHelper.reportData(commandedBy, actionId, tle, 69 * 2);
-      return HasActionsIF::EXECUTION_FINISHED;
-    }
-    case (UPDATE_MEKF_STANDARD_DEVIATIONS):
-      navigation.updateMekfStandardDeviations(&acsParameters);
-      return HasActionsIF::EXECUTION_FINISHED;
     default: {
       return HasActionsIF::INVALID_ACTION_ID;
     }
@@ -137,17 +130,31 @@ void AcsController::performControlOperation() {
     }
     case InternalState::INITIAL_DELAY: {
       if (initialCountdown.hasTimedOut()) {
-        uint8_t line1[69] = {};
-        uint8_t line2[69] = {};
-        readTleFromFs(line1, line2);
-        updateTle(line1, line2, true);
         internalState = InternalState::READY;
       }
       return;
     }
     case InternalState::READY: {
       if (mode != MODE_OFF) {
-        performAttitudeControl();
+        switch (mode) {
+          case acs::SAFE:
+            switch (submode) {
+              case SUBMODE_NONE:
+                performSafe();
+                break;
+              case acs::DETUMBLE:
+                performDetumble();
+                break;
+            }
+            break;
+          case acs::PTG_IDLE:
+          case acs::PTG_TARGET:
+          case acs::PTG_TARGET_GS:
+          case acs::PTG_NADIR:
+          case acs::PTG_INERTIAL:
+            performPointingCtrl();
+            break;
+        }
       }
       break;
     }
@@ -156,42 +163,32 @@ void AcsController::performControlOperation() {
   }
 }
 
-void AcsController::performAttitudeControl() {
-  Clock::getClock_timeval(&timeAbsolute);
-  Clock::getClockMonotonic(&timeRelative);
+void AcsController::performSafe() {
+  timeval now;
+  Clock::getClock_timeval(&now);
 
-  if (timeRelative.tv_sec != 0 and oldTimeRelative.tv_sec != 0) {
-    timeDelta = timevalOperations::toDouble(timeRelative - oldTimeRelative);
-  }
-  oldTimeRelative = timeRelative;
-
-  ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed);
+  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
   if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
     triggerEvent(acs::TLE_TOO_OLD);
     tleTooOldFlag = true;
   } else if (result != Sgp4Propagator::TLE_TOO_OLD) {
     tleTooOldFlag = false;
   }
-
-  sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
-                           &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
-  fusedRotationEstimation.estimateFusedRotationRate(
-      mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
-      &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
+  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
+                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
+  fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
+                                                        &gyrDataProcessed, &fusedRotRateData);
   result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                              &susDataProcessed, timeDelta, &attitudeEstimationData);
-
-  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
+                              &susDataProcessed, &mekfData, &acsParameters);
+  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
     if (not mekfInvalidFlag) {
-      triggerEvent(acs::MEKF_INVALID_INFO,
-                   static_cast<uint32_t>(attitudeEstimationData.mekfStatus.value));
+      triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
       mekfInvalidFlag = true;
     }
-    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
       triggerEvent(acs::MEKF_AUTOMATIC_RESET);
-      navigation.resetMekf(&attitudeEstimationData);
+      navigation.resetMekf(&mekfData);
       mekfLost = true;
     }
   } else if (mekfInvalidFlag) {
@@ -199,57 +196,32 @@ void AcsController::performAttitudeControl() {
     mekfInvalidFlag = false;
   }
 
-  handleDetumbling();
-
-  switch (mode) {
-    case acs::SAFE:
-      switch (submode) {
-        case SUBMODE_NONE:
-          performSafe();
-          break;
-        case acs::DETUMBLE:
-          performDetumble();
-          break;
-      }
-      break;
-    case acs::PTG_IDLE:
-    case acs::PTG_TARGET:
-    case acs::PTG_TARGET_GS:
-    case acs::PTG_NADIR:
-    case acs::PTG_INERTIAL:
-      performPointingCtrl();
-      break;
-  }
-}
-
-void AcsController::performSafe() {
   // get desired satellite rate, sun direction to align to and inertia
   double sunTargetDir[3] = {0, 0, 0};
   guidance.getTargetParamsSafe(sunTargetDir);
 
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
-  acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
+  acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
       fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
       acsParameters.safeModeControllerParameters.useGyr,
       acsParameters.safeModeControllerParameters.dampingDuringEclipse);
   switch (safeCtrlStrat) {
-    case (acs::ControlModeStrategy::SAFECTRL_MEKF):
-      safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value,
-                        attitudeEstimationData.satRotRateMekf.value,
-                        susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value,
-                        sunTargetDir, magMomMtq, errAng);
+    case (acs::SafeModeStrategy::SAFECTRL_MEKF):
+      safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
+                        susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
+                        magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::ControlModeStrategy::SAFECTRL_GYR):
+    case (acs::SafeModeStrategy::SAFECTRL_GYR):
       safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
                        susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
+    case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
       safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
                           fusedRotRateData.rotRateParallel.value,
                           fusedRotRateData.rotRateOrthogonal.value,
@@ -257,29 +229,29 @@ void AcsController::performSafe() {
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
+    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
       safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
                                   gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
                                   errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
+    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
       safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
                                      fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
                                      errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING):
+    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
       errAng = NAN;
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
+    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
       safeCtrlFailure(1, 0);
       break;
-    case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
+    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
       safeCtrlFailure(0, 1);
       break;
     default:
@@ -290,6 +262,33 @@ void AcsController::performSafe() {
   actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                            acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
 
+  // detumble check and switch
+  if (acsParameters.safeModeControllerParameters.useMekf) {
+    if (mekfData.satRotRateMekf.isValid() and
+        VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
+            acsParameters.detumbleParameter.omegaDetumbleStart) {
+      detumbleCounter++;
+    }
+  } else if (acsParameters.safeModeControllerParameters.useGyr) {
+    if (gyrDataProcessed.gyrVecTot.isValid() and
+        VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
+            acsParameters.detumbleParameter.omegaDetumbleStart) {
+      detumbleCounter++;
+    }
+  } else if (fusedRotRateData.rotRateTotal.isValid() and
+             VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
+                 acsParameters.detumbleParameter.omegaDetumbleStart) {
+    detumbleCounter++;
+  } else if (detumbleCounter > 0) {
+    detumbleCounter -= 1;
+  }
+  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
+    detumbleCounter = 0;
+    // Triggers detumble mode transition in subsystem
+    triggerEvent(acs::SAFE_RATE_VIOLATION);
+    startTransition(mode, acs::SafeSubmode::DETUMBLE);
+  }
+
   updateCtrlValData(errAng, safeCtrlStrat);
   updateActuatorCmdData(cmdDipoleMtqs);
   commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
@@ -297,24 +296,55 @@ void AcsController::performSafe() {
 }
 
 void AcsController::performDetumble() {
-  acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
+  timeval now;
+  Clock::getClock_timeval(&now);
+
+  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
+  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
+    triggerEvent(acs::TLE_TOO_OLD);
+    tleTooOldFlag = true;
+  } else {
+    tleTooOldFlag = false;
+  }
+  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
+                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
+  fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
+                                                        &gyrDataProcessed, &fusedRotRateData);
+  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                              &susDataProcessed, &mekfData, &acsParameters);
+  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
+      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
+      mekfInvalidFlag = true;
+    }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+      triggerEvent(acs::MEKF_AUTOMATIC_RESET);
+      navigation.resetMekf(&mekfData);
+      mekfLost = true;
+    }
+  } else if (mekfInvalidFlag) {
+    triggerEvent(acs::MEKF_RECOVERY);
+    mekfInvalidFlag = false;
+  }
+  acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
       mgmDataProcessed.mgmVecTotDerivative.isValid(),
       acsParameters.detumbleParameter.useFullDetumbleLaw);
   double magMomMtq[3] = {0, 0, 0};
   switch (safeCtrlStrat) {
-    case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL):
+    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
       detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
                            magMomMtq, acsParameters.detumbleParameter.gainFull);
       break;
-    case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
+    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
       detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
                        magMomMtq, acsParameters.detumbleParameter.gainBdot);
       break;
-    case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
+    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
       safeCtrlFailure(1, 0);
       break;
-    case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
+    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
       safeCtrlFailure(0, 1);
       break;
     default:
@@ -325,6 +355,33 @@ void AcsController::performDetumble() {
   actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                            acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
 
+  if (acsParameters.safeModeControllerParameters.useMekf) {
+    if (mekfData.satRotRateMekf.isValid() and
+        VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
+            acsParameters.detumbleParameter.omegaDetumbleEnd) {
+      detumbleCounter++;
+    }
+  } else if (acsParameters.safeModeControllerParameters.useGyr) {
+    if (gyrDataProcessed.gyrVecTot.isValid() and
+        VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
+            acsParameters.detumbleParameter.omegaDetumbleEnd) {
+      detumbleCounter++;
+    }
+  } else if (fusedRotRateData.rotRateTotal.isValid() and
+             VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
+                 acsParameters.detumbleParameter.omegaDetumbleEnd) {
+    detumbleCounter++;
+  } else if (detumbleCounter > 0) {
+    detumbleCounter -= 1;
+  }
+
+  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
+    detumbleCounter = 0;
+    // Triggers safe mode transition in subsystem
+    triggerEvent(acs::SAFE_RATE_RECOVERY);
+    startTransition(mode, acs::SafeSubmode::DEFAULT);
+  }
+
   updateCtrlValData(safeCtrlStrat);
   updateActuatorCmdData(cmdDipoleMtqs);
   commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
@@ -332,76 +389,52 @@ void AcsController::performDetumble() {
 }
 
 void AcsController::performPointingCtrl() {
-  bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
-                   sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
-  uint8_t useMekf = false;
-  switch (mode) {
-    case acs::PTG_IDLE:
-      useMekf = acsParameters.idleModeControllerParameters.useMekf;
-      break;
-    case acs::PTG_TARGET:
-      useMekf = acsParameters.targetModeControllerParameters.useMekf;
-      break;
-    case acs::PTG_TARGET_GS:
-      useMekf = acsParameters.gsTargetModeControllerParameters.useMekf;
-      break;
-    case acs::PTG_NADIR:
-      useMekf = acsParameters.nadirModeControllerParameters.useMekf;
-      break;
-    case acs::PTG_INERTIAL:
-      useMekf = acsParameters.inertialModeControllerParameters.useMekf;
-      break;
-  }
-  acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
-      mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
-      attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
-      fusedRotRateData.rotRateSource.value, useMekf);
+  timeval now;
+  Clock::getClock_timeval(&now);
 
-  if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
-      ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
-    ptgCtrlLostCounter++;
-    if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
-      triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
-      ptgCtrlLostCounter = 0;
+  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
+  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
+    triggerEvent(acs::TLE_TOO_OLD);
+    tleTooOldFlag = true;
+  } else {
+    tleTooOldFlag = false;
+  }
+  sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
+                           &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
+  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                              &susDataProcessed, &mekfData, &acsParameters);
+  if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
+      result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
+    mekfInvalidCounter++;
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
+      mekfInvalidFlag = true;
+    }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+      triggerEvent(acs::MEKF_AUTOMATIC_RESET);
+      navigation.resetMekf(&mekfData);
+      mekfLost = true;
+    }
+    if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
+      // Trigger this so STR FDIR can set the device faulty.
+      EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
+      mekfInvalidCounter = 0;
     }
-    guidance.resetValues();
-    updateCtrlValData(ptgCtrlStrat);
-    updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
     commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
                      cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
                      acsParameters.rwHandlingParameters.rampTime);
     return;
   } else {
-    ptgCtrlLostCounter = 0;
+    if (mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_RECOVERY);
+      mekfInvalidFlag = false;
+    }
+    mekfInvalidCounter = 0;
   }
-
-  double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0};
-  switch (ptgCtrlStrat) {
-    case acs::ControlModeStrategy::PTGCTRL_MEKF:
-      std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI));
-      std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB));
-      break;
-    case acs::ControlModeStrategy::PTGCTRL_STR:
-      quatBI[0] = sensorValues.strSet.caliQx.value;
-      quatBI[1] = sensorValues.strSet.caliQy.value;
-      quatBI[2] = sensorValues.strSet.caliQz.value;
-      quatBI[3] = sensorValues.strSet.caliQw.value;
-      std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
-      break;
-    case acs::ControlModeStrategy::PTGCTRL_QUEST:
-      std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
-      std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
-      break;
-    default:
-      sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
-                 << std::endl;
-      break;
-  }
-
-  bool allRwAvailable = true;
+  uint8_t enableAntiStiction = true;
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv, &rwAvail);
-  if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) {
+  result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
+  if (result == returnvalue::FAILED) {
     if (multipleRwUnavailableCounter >=
         acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
       triggerEvent(acs::MULTIPLE_RW_INVALID);
@@ -409,10 +442,8 @@ void AcsController::performPointingCtrl() {
     }
     multipleRwUnavailableCounter++;
     return;
-  }
-  multipleRwUnavailableCounter = 0;
-  if (result == acsctrl::SINGLE_RW_UNAVAILABLE) {
-    allRwAvailable = false;
+  } else {
+    multipleRwUnavailableCounter = 0;
   }
 
   // Variables required for guidance
@@ -424,110 +455,116 @@ void AcsController::performPointingCtrl() {
 
   switch (mode) {
     case acs::PTG_IDLE:
-      guidance.targetQuatPtgIdle(timeAbsolute, timeDelta, susDataProcessed.sunIjkModel.value,
-                                 gpsDataProcessed.gpsPosition.value, targetQuat, targetSatRotRate);
-      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
-                          errorSatRotRate, errorAngle);
+      guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
+                                targetSatRotRate);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.idleModeControllerParameters,
+      ptgCtrl.ptgNullspace(&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, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters,
-          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          &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.targetQuatPtgTarget(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
-                                   gpsDataProcessed.gpsVelocity.value, targetQuat,
-                                   targetSatRotRate);
-      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
-                          acsParameters.targetModeControllerParameters.quatRef,
+      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(allRwAvailable, &acsParameters.targetModeControllerParameters,
+      ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
                            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, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters,
-          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          &acsParameters.targetModeControllerParameters, 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.targetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET_GS:
-      guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
+      guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
                                susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
-      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
-                          errorSatRotRate, errorAngle);
+      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(allRwAvailable, &acsParameters.gsTargetModeControllerParameters,
+      ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
                            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, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters,
-          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          &acsParameters.gsTargetModeControllerParameters, 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.gsTargetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_NADIR:
-      guidance.targetQuatPtgNadir(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
-                                  gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
-      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
-                          acsParameters.nadirModeControllerParameters.quatRef,
+      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(allRwAvailable, &acsParameters.nadirModeControllerParameters,
+      ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
                            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, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters,
-          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          &acsParameters.nadirModeControllerParameters, 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.nadirModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_INERTIAL:
       std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
                   sizeof(targetQuat));
-      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
-                          acsParameters.inertialModeControllerParameters.quatRef,
+      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(allRwAvailable, &acsParameters.inertialModeControllerParameters,
+      ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
                            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, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
-          allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters,
-          mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+          &acsParameters.inertialModeControllerParameters, 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.inertialModeControllerParameters.enableAntiStiction;
       break;
     default:
       sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
@@ -539,11 +576,13 @@ void AcsController::performPointingCtrl() {
       sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
       acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
       acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
-  ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
+  if (enableAntiStiction) {
+    ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
+  }
   actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                            acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
 
-  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgCtrlStrat);
+  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
   updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
   commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                    acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
@@ -551,74 +590,6 @@ void AcsController::performPointingCtrl() {
                    acsParameters.rwHandlingParameters.rampTime);
 }
 
-void AcsController::handleDetumbling() {
-  switch (detumbleState) {
-    case DetumbleState::NO_DETUMBLE:
-      if (fusedRotRateData.rotRateTotal.isValid() and
-          VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
-              acsParameters.detumbleParameter.omegaDetumbleStart) {
-        detumbleCounter++;
-      } else if (detumbleCounter > 0) {
-        detumbleCounter -= 1;
-      }
-      if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
-        if (mode == acs::AcsMode::SAFE) {
-          detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
-          break;
-        }
-        detumbleState = DetumbleState::DETUMBLE_FROM_PTG;
-      }
-      break;
-    case DetumbleState::DETUMBLE_FROM_PTG:
-      triggerEvent(acs::PTG_RATE_VIOLATION);
-      detumbleTransitionCountdow.resetTimer();
-      detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION;
-      break;
-    case DetumbleState::PTG_TO_SAFE_TRANSITION:
-      if (detumbleTransitionCountdow.hasTimedOut()) {
-        triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 2);
-        detumbleCounter = 0;
-        detumbleState = DetumbleState::NO_DETUMBLE;
-        break;
-      }
-      if (mode == acs::AcsMode::SAFE) {
-        detumbleState = DetumbleState::DETUMBLE_FROM_SAFE;
-      }
-      break;
-    case DetumbleState::DETUMBLE_FROM_SAFE:
-      detumbleCounter = 0;
-      // Triggers detumble mode transition in subsystem
-      if (mode == acs::AcsMode::SAFE) {
-        triggerEvent(acs::SAFE_RATE_VIOLATION);
-        startTransition(mode, acs::SafeSubmode::DETUMBLE);
-        detumbleState = DetumbleState::IN_DETUMBLE;
-        break;
-      }
-      triggerEvent(acs::DETUMBLE_TRANSITION_FAILED, 3);
-      detumbleState = DetumbleState::NO_DETUMBLE;
-      break;
-    case DetumbleState::IN_DETUMBLE:
-      if (fusedRotRateData.rotRateTotal.isValid() and
-          VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
-              acsParameters.detumbleParameter.omegaDetumbleEnd) {
-        detumbleCounter++;
-      } else if (detumbleCounter > 0) {
-        detumbleCounter -= 1;
-      }
-
-      if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
-        detumbleCounter = 0;
-        // Triggers safe mode transition in subsystem
-        triggerEvent(acs::RATE_RECOVERY);
-        startTransition(mode, acs::SafeSubmode::DEFAULT);
-        detumbleState = DetumbleState::NO_DETUMBLE;
-      }
-      break;
-    default:
-      sif::error << "AcsController: Invalid DetumbleState" << std::endl;
-  }
-}
-
 void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
   if (not safeCtrlFailureFlag) {
     triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
@@ -689,7 +660,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
   }
 }
 
-void AcsController::updateCtrlValData(acs::ControlModeStrategy ctrlStrat) {
+void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
     std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@@ -700,13 +671,13 @@ void AcsController::updateCtrlValData(acs::ControlModeStrategy ctrlStrat) {
     ctrlValData.errAng.setValid(false);
     std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
     ctrlValData.tgtRotRate.setValid(false);
-    ctrlValData.safeStrat.value = ctrlStrat;
+    ctrlValData.safeStrat.value = safeModeStrat;
     ctrlValData.safeStrat.setValid(true);
     ctrlValData.setValidity(true, false);
   }
 }
 
-void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat) {
+void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
     std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@@ -717,22 +688,21 @@ void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ct
     ctrlValData.errAng.setValid(true);
     std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
     ctrlValData.tgtRotRate.setValid(false);
-    ctrlValData.safeStrat.value = ctrlStrat;
+    ctrlValData.safeStrat.value = safeModeStrat;
     ctrlValData.safeStrat.setValid(true);
     ctrlValData.setValidity(true, false);
   }
 }
 
 void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
-                                      const double *tgtRotRate,
-                                      acs::ControlModeStrategy ctrlStrat) {
+                                      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.safeStrat.value = ctrlStrat;
+    ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF;
     ctrlValData.setValidity(true, true);
   }
 }
@@ -809,12 +779,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
   localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
   poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
-  // Attitude Estimation
+  // MEKF
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
-  localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
-  poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0});
+  poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
   // Ctrl Values
   localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
@@ -831,15 +800,10 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
   poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
-  // Fused Rot Rate Sources
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
-  poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
+  // TLE Data
+  localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1);
+  localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2);
   return returnvalue::OK;
 }
 
@@ -860,15 +824,13 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
     case acsctrl::GPS_PROCESSED_DATA:
       return &gpsDataProcessed;
     case acsctrl::MEKF_DATA:
-      return &attitudeEstimationData;
+      return &mekfData;
     case acsctrl::CTRL_VAL_DATA:
       return &ctrlValData;
     case acsctrl::ACTUATOR_CMD_DATA:
       return &actuatorCmdData;
     case acsctrl::FUSED_ROTATION_RATE_DATA:
       return &fusedRotRateData;
-    case acsctrl::FUSED_ROTATION_RATE_SOURCES_DATA:
-      return &fusedRotRateSourcesData;
     default:
       return nullptr;
   }
@@ -900,31 +862,6 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
 }
 
 void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
-  guidance.resetValues();
-  if (mode == acs::AcsMode::SAFE) {
-    {
-      PoolReadGuard pg(&rw1SpeedSet);
-      rw1SpeedSet.setRwSpeed(0, 10);
-    }
-    {
-      PoolReadGuard pg(&rw2SpeedSet);
-      rw2SpeedSet.setRwSpeed(0, 10);
-    }
-    {
-      PoolReadGuard pg(&rw3SpeedSet);
-      rw3SpeedSet.setRwSpeed(0, 10);
-    }
-    {
-      PoolReadGuard pg(&rw4SpeedSet);
-      rw4SpeedSet.setRwSpeed(0, 10);
-    }
-  }
-  if (submode == acs::SafeSubmode::DETUMBLE) {
-    detumbleState = DetumbleState::IN_DETUMBLE;
-  }
-  if (detumbleState == DetumbleState::IN_DETUMBLE and submode != acs::SafeSubmode::DETUMBLE) {
-    detumbleState = DetumbleState::NO_DETUMBLE;
-  }
   return ExtendedControllerBase::modeChanged(mode, submode);
 }
 
@@ -1094,67 +1031,6 @@ void AcsController::copySusData() {
   }
 }
 
-ReturnValue_t AcsController::updateTle(const uint8_t *line1, const uint8_t *line2, bool fromFile) {
-  ReturnValue_t result = navigation.updateTle(line1, line2);
-  if (result != returnvalue::OK) {
-    if (not fromFile) {
-      uint8_t fileLine1[69] = {};
-      uint8_t fileLine2[69] = {};
-      readTleFromFs(fileLine1, fileLine2);
-      navigation.updateTle(fileLine1, fileLine2);
-    }
-    return result;
-  }
-  return returnvalue::OK;
-}
-
-ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) {
-  auto mntPrefix = sdcMan.getCurrentMountPrefix();
-  if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) {
-    return returnvalue::FAILED;
-  }
-  std::string path = mntPrefix + TLE_FILE;
-  // Clear existing TLE from file
-  std::ofstream tleFile(path.c_str(), std::ofstream::out | std::ofstream::trunc);
-  if (tleFile.is_open()) {
-    tleFile.write(reinterpret_cast<const char *>(tle), 69);
-    tleFile << "\n";
-    tleFile.write(reinterpret_cast<const char *>(tle + 69), 69);
-  } else {
-    return acsctrl::WRITE_FILE_FAILED;
-  }
-  tleFile.close();
-  return returnvalue::OK;
-}
-
-ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) {
-  auto mntPrefix = sdcMan.getCurrentMountPrefix();
-  if (mntPrefix == nullptr or !sdcMan.isSdCardUsable(std::nullopt)) {
-    return returnvalue::FAILED;
-  }
-  std::string path = mntPrefix + TLE_FILE;
-  std::error_code e;
-  if (std::filesystem::exists(path, e)) {
-    // Read existing TLE from file
-    std::fstream tleFile = std::fstream(path.c_str(), std::fstream::in);
-    if (tleFile.is_open()) {
-      std::string tleLine1, tleLine2;
-      getline(tleFile, tleLine1);
-      std::memcpy(line1, tleLine1.c_str(), 69);
-      getline(tleFile, tleLine2);
-      std::memcpy(line2, tleLine2.c_str(), 69);
-    } else {
-      triggerEvent(acs::TLE_FILE_READ_FAILED);
-      return acsctrl::READ_FILE_FAILED;
-    }
-    tleFile.close();
-  } else {
-    triggerEvent(acs::TLE_FILE_READ_FAILED);
-    return acsctrl::READ_FILE_FAILED;
-  }
-  return returnvalue::OK;
-}
-
 void AcsController::copyGyrData() {
   {
     PoolReadGuard pg(&sensorValues.gyr0AdisSet);

mission/controller/AcsController.h

@@ -4,20 +4,16 @@
 #include <eive/objects.h>
 #include <fsfw/controller/ExtendedControllerBase.h>
 #include <fsfw/coordinates/Sgp4Propagator.h>
-#include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/health/HealthTable.h>
 #include <fsfw/parameters/ParameterHelper.h>
 #include <fsfw/parameters/ReceivesParameterMessagesIF.h>
 #include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
 #include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
-#include <mission/acs/defs.h>
 #include <mission/acs/imtqHelpers.h>
 #include <mission/acs/rwHelpers.h>
 #include <mission/acs/susMax1227Helpers.h>
-#include <mission/config/torquer.h>
 #include <mission/controller/acs/ActuatorCmd.h>
-#include <mission/controller/acs/AttitudeEstimation.h>
 #include <mission/controller/acs/FusedRotationEstimation.h>
 #include <mission/controller/acs/Guidance.h>
 #include <mission/controller/acs/MultiplicativeKalmanFilter.h>
@@ -27,18 +23,13 @@
 #include <mission/controller/acs/control/PtgCtrl.h>
 #include <mission/controller/acs/control/SafeCtrl.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
-#include <mission/memory/SdCardMountedIF.h>
 #include <mission/utility/trace.h>
 
-#include <filesystem>
-#include <fstream>
-#include <optional>
-
 class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
  public:
   static constexpr dur_millis_t INIT_DELAY = 500;
 
-  AcsController(object_id_t objectId, bool enableHkSets, SdCardMountedIF& sdcMan);
+  AcsController(object_id_t objectId, bool enableHkSets);
 
   MessageQueueId_t getCommandQueue() const;
   ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
@@ -46,8 +37,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
                              uint16_t startAtIndex) override;
 
  protected:
+  void performSafe();
+  void performDetumble();
+  void performPointingCtrl();
+
  private:
-  static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
   static constexpr double ZERO_VEC3[3] = {0, 0, 0};
   static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
   static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
@@ -55,16 +49,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   bool enableHkSets = false;
 
-  SdCardMountedIF& sdcMan;
-
-  timeval timeAbsolute;
-  timeval timeRelative;
-  double timeDelta = 0.0;
-  timeval oldTimeRelative;
-
   AcsParameters acsParameters;
   SensorProcessing sensorProcessing;
-  AttitudeEstimation attitudeEstimation;
   FusedRotationEstimation fusedRotationEstimation;
   Navigation navigation;
   ActuatorCmd actuatorCmd;
@@ -80,7 +66,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   uint8_t detumbleCounter = 0;
   uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = false;
-  uint16_t ptgCtrlLostCounter = 0;
+  uint16_t mekfInvalidCounter = 0;
   bool safeCtrlFailureFlag = false;
   uint8_t safeCtrlFailureCounter = 0;
   uint8_t resetMekfCount = 0;
@@ -89,8 +75,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
   int16_t cmdDipoleMtqs[3] = {0, 0, 0};
 
-  acsctrl::RwAvail rwAvail;
-
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;
 #endif
@@ -98,22 +82,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };
   InternalState internalState = InternalState::STARTUP;
 
-  enum class DetumbleState {
-    NO_DETUMBLE,
-    DETUMBLE_FROM_PTG,
-    PTG_TO_SAFE_TRANSITION,
-    DETUMBLE_FROM_SAFE,
-    IN_DETUMBLE
-  };
-  DetumbleState detumbleState = DetumbleState::NO_DETUMBLE;
-
   /** Device command IDs */
   static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
   static const DeviceCommandId_t RESET_MEKF = 0x1;
   static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
   static const DeviceCommandId_t UPDATE_TLE = 0x3;
-  static const DeviceCommandId_t READ_TLE = 0x4;
-  static const DeviceCommandId_t UPDATE_MEKF_STANDARD_DEVIATIONS = 0x5;
+
+  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
+  //! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
+  static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
 
   ReturnValue_t initialize() override;
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;
@@ -133,13 +110,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void modeChanged(Mode_t mode, Submode_t submode);
   void announceMode(bool recursive);
 
-  void performAttitudeControl();
-  void performSafe();
-  void performDetumble();
-  void performPointingCtrl();
-
-  void handleDetumbling();
-
   void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
 
   ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
@@ -150,16 +120,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void updateActuatorCmdData(const int16_t* mtqTargetDipole);
   void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
                              const int16_t* mtqTargetDipole);
-  void updateCtrlValData(acs::ControlModeStrategy ctrlStrat);
-  void updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat);
+  void updateCtrlValData(uint8_t safeModeStrat);
+  void updateCtrlValData(double errAng, uint8_t safeModeStrat);
   void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
-                         const double* tgtRotRate, acs::ControlModeStrategy cStrat);
-
-  ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile);
-  ReturnValue_t writeTleToFs(const uint8_t* tle);
-  ReturnValue_t readTleFromFs(uint8_t* line1, uint8_t* line2);
-
-  const std::string TLE_FILE = "/conf/tle.txt";
+                         const double* tgtRotRate);
 
   /* ACS Sensor Values */
   ACS::SensorValues sensorValues;
@@ -248,12 +212,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
   PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
 
-  // Attitude Estimation
-  acsctrl::AttitudeEstimationData attitudeEstimationData;
+  // MEKF
+  acsctrl::MekfData mekfData;
   PoolEntry<double> quatMekf = PoolEntry<double>(4);
   PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
   PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
-  PoolEntry<double> quatQuest = PoolEntry<double>(4);
 
   // Ctrl Values
   acsctrl::CtrlValData ctrlValData;
@@ -274,22 +237,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
   PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
   PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
-  PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
 
-  // Fused Rot Rate Sources
-  acsctrl::FusedRotRateSourcesData fusedRotRateSourcesData;
-  PoolEntry<double> rotRateOrthogonalSusMgm = PoolEntry<double>(3);
-  PoolEntry<double> rotRateParallelSusMgm = PoolEntry<double>(3);
-  PoolEntry<double> rotRateTotalSusMgm = PoolEntry<double>(3);
-  PoolEntry<double> rotRateTotalQuest = PoolEntry<double>(3);
-  PoolEntry<double> rotRateTotalStr = PoolEntry<double>(3);
+  // TLE Dataset
+  acsctrl::TleData tleData;
+  PoolEntry<uint8_t> line1 = PoolEntry<uint8_t>(69);
+  PoolEntry<uint8_t> line2 = PoolEntry<uint8_t>(69);
 
   // Initial delay to make sure all pool variables have been initialized their owners
   Countdown initialCountdown = Countdown(INIT_DELAY);
-
-  // Countdown after which the detumbling mode change should have been finished
-  static constexpr dur_millis_t MAX_DURATION = 60 * 1e3;
-  Countdown detumbleTransitionCountdow = Countdown(MAX_DURATION);
 };
 
 #endif /* MISSION_CONTROLLER_ACSCONTROLLER_H_ */

mission/controller/PowerController.cpp

@@ -157,12 +157,7 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
 
 void PowerController::calculateStateOfCharge() {
   // get time
-  Clock::getClockMonotonic(&now);
-  double timeDelta = 0.0;
-  if (now.tv_sec != 0 and oldTime.tv_sec != 0) {
-    timeDelta = timevalOperations::toDouble(now - oldTime);
-  }
-  oldTime = now;
+  Clock::getClock_timeval(&now);
 
   // update EPS HK values
   ReturnValue_t result = updateEpsData();
@@ -178,6 +173,8 @@ void PowerController::calculateStateOfCharge() {
         pwrCtrlCoreHk.setValidity(false, true);
       }
     }
+    // store time for next run
+    oldTime = now;
     return;
   }
 
@@ -198,10 +195,12 @@ void PowerController::calculateStateOfCharge() {
         pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
       }
     }
+    // store time for next run
+    oldTime = now;
     return;
   }
 
-  result = calculateCoulombCounterCharge(timeDelta);
+  result = calculateCoulombCounterCharge();
   if (result != returnvalue::OK) {
     // notifying events have already been triggered
     {
@@ -216,6 +215,8 @@ void PowerController::calculateStateOfCharge() {
         pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
       }
     }
+    // store time for next run
+    oldTime = now;
     return;
   }
 
@@ -230,6 +231,8 @@ void PowerController::calculateStateOfCharge() {
       pwrCtrlCoreHk.setValidity(true, true);
     }
   }
+  // store time for next run
+  oldTime = now;
 }
 
 void PowerController::watchStateOfCharge() {
@@ -282,14 +285,12 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
   return returnvalue::OK;
 }
 
-ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) {
-  if (timeDelta == 0.0) {
-    return returnvalue::FAILED;
-  }
-  if (timeDelta > maxAllowedTimeDiff) {
+ReturnValue_t PowerController::calculateCoulombCounterCharge() {
+  double timeDiff = timevalOperations::toDouble(now - oldTime);
+  if (timeDiff > maxAllowedTimeDiff) {
     // should not be a permanent state so no spam protection required
-    triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDelta * 10));
-    sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDelta
+    triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10));
+    sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff
                << std::endl;
     return returnvalue::FAILED;
   }
@@ -297,7 +298,7 @@ ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) {
     coulombCounterCharge = openCircuitVoltageCharge;
   } else {
     coulombCounterCharge =
-        coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDelta * SECONDS_TO_HOURS;
+        coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS;
     if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
       coulombCounterCharge = coulombCounterChargeUpperThreshold;
     }

mission/controller/PowerController.h

@@ -45,7 +45,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   void calculateStateOfCharge();
   void watchStateOfCharge();
   ReturnValue_t calculateOpenCircuitVoltageCharge();
-  ReturnValue_t calculateCoulombCounterCharge(double timeDelta);
+  ReturnValue_t calculateCoulombCounterCharge();
   ReturnValue_t updateEpsData();
   float charge2stateOfCharge(float capacity, bool coulombCounter);
   ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd);

mission/controller/acs/AcsParameters.cpp

@@ -24,20 +24,11 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(onBoardParams.sampleTime);
           break;
         case 0x1:
-          parameterWrapper->set(onBoardParams.ptgCtrlLostTimer);
+          parameterWrapper->set(onBoardParams.mekfViolationTimer);
           break;
         case 0x2:
           parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse);
           break;
-        case 0x3:
-          parameterWrapper->set(onBoardParams.fusedRateFromStr);
-          break;
-        case 0x4:
-          parameterWrapper->set(onBoardParams.fusedRateFromQuest);
-          break;
-        case 0x5:
-          parameterWrapper->set(onBoardParams.questFilterWeight);
-          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -333,16 +324,16 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
           break;
         case 0x2:
-          parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW1);
+          parameterWrapper->setMatrix(rwMatrices.without1);
           break;
         case 0x3:
-          parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW2);
+          parameterWrapper->setMatrix(rwMatrices.without2);
           break;
         case 0x4:
-          parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW3);
+          parameterWrapper->setMatrix(rwMatrices.without3);
           break;
         case 0x5:
-          parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW4);
+          parameterWrapper->setMatrix(rwMatrices.without4);
           break;
         case 0x6:
           parameterWrapper->setVector(rwMatrices.nullspaceVector);
@@ -432,7 +423,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(idleModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(idleModeControllerParameters.useMekf);
+          parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
           break;
         default:
           return INVALID_IDENTIFIER_ID;
@@ -468,7 +459,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(targetModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(targetModeControllerParameters.useMekf);
+          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
           parameterWrapper->setVector(targetModeControllerParameters.refDirection);
@@ -537,7 +528,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
+          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
           parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
@@ -588,7 +579,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(nadirModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(nadirModeControllerParameters.useMekf);
+          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
           parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
@@ -636,7 +627,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(inertialModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(inertialModeControllerParameters.useMekf);
+          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
           parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
@@ -717,22 +708,22 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
     case (0x11):  // KalmanFilterParameters
       switch (parameterId) {
         case 0x0:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseStr);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
           break;
         case 0x1:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSus);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
           break;
         case 0x2:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseMgm);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
           break;
         case 0x3:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyr);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
           break;
         case 0x4:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrArw);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
           break;
         case 0x5:
-          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs);
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
           break;
         default:
           return INVALID_IDENTIFIER_ID;

mission/controller/acs/AcsParameters.h

@@ -8,9 +8,6 @@
 typedef unsigned char uint8_t;
 
 class AcsParameters : public HasParametersIF {
- private:
-  static constexpr double DEG2RAD = M_PI / 180.;
-
  public:
   AcsParameters();
   virtual ~AcsParameters();
@@ -21,11 +18,8 @@ class AcsParameters : public HasParametersIF {
 
   struct OnBoardParams {
     double sampleTime = 0.4;  // [s]
-    uint16_t ptgCtrlLostTimer = 750;
+    uint16_t mekfViolationTimer = 750;
     uint8_t fusedRateSafeDuringEclipse = true;
-    uint8_t fusedRateFromStr = true;
-    uint8_t fusedRateFromQuest = true;
-    double questFilterWeight = 0.9;
   } onBoardParams;
 
   struct InertiaEIVE {
@@ -81,9 +75,9 @@ class AcsParameters : public HasParametersIF {
                                        {-0.007534, 1.253879, 0.006812},
                                        {-0.037072, 0.006812, 1.313158}};
 
-    double mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
-    double mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
-    double mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
+    float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
+    float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
+    float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
     float mgmVectorFilterWeight = 0.85;
     float mgmDerivativeFilterWeight = 0.99;
     uint8_t useMgm4 = false;
@@ -776,7 +770,7 @@ class AcsParameters : public HasParametersIF {
          0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
          -0.000889232196185857, -0.00168429567131815}};
     float susBrightnessThreshold = 0.7;
-    float susVectorFilterWeight = .95;
+    float susVectorFilterWeight = .85;
     float susRateFilterWeight = .99;
   } susHandlingParameters;
 
@@ -793,10 +787,10 @@ class AcsParameters : public HasParametersIF {
 
     /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
      * assumed to be equal for the same class of sensors */
-    double gyr02variance[3] = {pow(4.6e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
-                               pow(4.6e-3, 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
-                               pow(6.1e-3, 2)};  // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
-    double gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
+    float gyr02variance[3] = {pow(4.6e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
+                              pow(4.6e-3, 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
+                              pow(6.1e-3, 2)};  // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
+    float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
     uint8_t preferAdis = false;
     float gyrFilterWeight = 0.6;
   } gyrHandlingParameters;
@@ -815,19 +809,19 @@ class AcsParameters : public HasParametersIF {
   } rwHandlingParameters;
 
   struct RwMatrices {
-    double alignmentMatrix[3][4] = {{-0.9205, 0.0000, 0.9205, 0.0000},
-                                    {0.0000, 0.9205, 0.0000, -0.9205},
-                                    {-0.3907, -0.3907, -0.3907, -0.3907}};
+    double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
+                                    {0.0000, -0.9205, 0.0000, 0.9205},
+                                    {0.3907, 0.3907, 0.3907, 0.3907}};
     double pseudoInverse[4][3] = {
-        {-0.5432, 0, -0.6399}, {0, 0.5432, -0.6399}, {0.5432, 0, -0.6399}, {0, -0.5432, -0.6399}};
-    double pseudoInverseWithoutRW1[4][3] = {
-        {0, 0, 0}, {-0.5432, 0.5432, -1.2798}, {1.0864, 0, 0}, {-0.5432, -0.5432, -1.2798}};
-    double pseudoInverseWithoutRW2[4][3] = {
-        {-0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, -1.0864, 0}};
-    double pseudoInverseWithoutRW3[4][3] = {
-        {-1.0864, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, -0.5432, -1.2798}};
-    double pseudoInverseWithoutRW4[4][3] = {
-        {-0.5432, -0.5432, -1.2798}, {0, 1.0864, 0}, {0.5432, -0.5432, -1.2798}, {0, 0, 0}};
+        {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
+    double without1[4][3] = {
+        {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
+    double without2[4][3] = {
+        {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
+    double without3[4][3] = {
+        {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
+    double without4[4][3] = {
+        {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
     double nullspaceVector[4] = {-1, 1, -1, 1};
   } rwMatrices;
 
@@ -857,7 +851,7 @@ class AcsParameters : public HasParametersIF {
   struct PointingLawParameters {
     double zeta = 0.3;
     double om = 0.3;
-    double omMax = 1 * DEG2RAD;
+    double omMax = 1 * M_PI / 180;
     double qiMin = 0.1;
 
     double gainNullspace = 0.01;
@@ -866,7 +860,7 @@ class AcsParameters : public HasParametersIF {
     double desatMomentumRef[3] = {0, 0, 0};
     double deSatGainFactor = 1000;
     uint8_t desatOn = true;
-    uint8_t useMekf = false;
+    uint8_t enableAntiStiction = true;
   } pointingLawParameters;
 
   struct IdleModeControllerParameters : PointingLawParameters {
@@ -879,15 +873,15 @@ class AcsParameters : public HasParametersIF {
     uint8_t timeElapsedMax = 10;  // rot rate calculations
 
     // Default is Stuttgart GS
-    double latitudeTgt = 48.7495 * DEG2RAD;   // [rad] Latitude
-    double longitudeTgt = 9.10384 * DEG2RAD;  // [rad] Longitude
-    double altitudeTgt = 500;                 // [m]
+    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
+    double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
+    double altitudeTgt = 500;                     // [m]
 
     // For one-axis control:
     uint8_t avoidBlindStr = true;
     double blindAvoidStart = 1.5;
     double blindAvoidStop = 2.5;
-    double blindRotRate = 1. * DEG2RAD;
+    double blindRotRate = 1 * M_PI / 180;
   } targetModeControllerParameters;
 
   struct GsTargetModeControllerParameters : PointingLawParameters {
@@ -895,9 +889,9 @@ class AcsParameters : public HasParametersIF {
     uint8_t timeElapsedMax = 10;          // rot rate calculations
 
     // Default is Stuttgart GS
-    double latitudeTgt = 48.7495 * DEG2RAD;   // [rad] Latitude
-    double longitudeTgt = 9.10384 * DEG2RAD;  // [rad] Longitude
-    double altitudeTgt = 500;                 // [m]
+    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 {
@@ -914,8 +908,8 @@ class AcsParameters : public HasParametersIF {
   } inertialModeControllerParameters;
 
   struct StrParameters {
-    double exclusionAngle = 20. * DEG2RAD;
-    double boresightAxis[3] = {0.7593, 0.0000, -0.6508};  // body rf
+    double exclusionAngle = 20 * M_PI / 180;
+    double boresightAxis[3] = {0.7593, 0.0000, -0.6508};  // geometry frame
   } strParameters;
 
   struct GpsParameters {
@@ -928,25 +922,25 @@ class AcsParameters : public HasParametersIF {
 
   struct SunModelParameters {
     float domega = 36000.771;
-    float omega_0 = 280.46 * DEG2RAD;  // RAAN plus argument of
-                                       // perigee
-    float m_0 = 357.5277;              // coefficients for mean anomaly
-    float dm = 35999.049;              // coefficients for mean anomaly
-    float e = 23.4392911 * DEG2RAD;    // angle of earth's rotation axis
-    float e1 = 0.74508 * DEG2RAD;
+    float omega_0 = 280.46 * M_PI / 180.;  // RAAN plus argument of
+                                           // perigee
+    float m_0 = 357.5277;                  // coefficients for mean anomaly
+    float dm = 35999.049;                  // coefficients for mean anomaly
+    float e = 23.4392911 * M_PI / 180.;    // angle of earth's rotation axis
+    float e1 = 0.74508 * M_PI / 180.;
 
-    float p1 = 6892. / 3600. * DEG2RAD;  // some parameter
-    float p2 = 72. / 3600. * DEG2RAD;    // some parameter
+    float p1 = 6892. / 3600. * M_PI / 180.;  // some parameter
+    float p2 = 72. / 3600. * M_PI / 180.;    // some parameter
   } sunModelParameters;
 
   struct KalmanFilterParameters {
-    double sensorNoiseStr = 0.1 * DEG2RAD;
-    double sensorNoiseSus = 8. * DEG2RAD;
-    double sensorNoiseMgm = 4. * DEG2RAD;
-    double sensorNoiseGyr = 0.1 * DEG2RAD;
+    double sensorNoiseSTR = 0.1 * M_PI / 180;
+    double sensorNoiseSS = 8 * M_PI / 180;
+    double sensorNoiseMAG = 4 * M_PI / 180;
+    double sensorNoiseGYR = 0.1 * M_PI / 180;
 
-    double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD;  // Angular Random Walk
-    double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD;               // Bias Stability
+    double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180;  // Angular Random Walk
+    double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600;                // Bias Stability
   } kalmanFilterParameters;
 
   struct MagnetorquerParameter {
@@ -962,8 +956,8 @@ class AcsParameters : public HasParametersIF {
 
   struct DetumbleParameter {
     uint8_t detumblecounter = 75;  // 30 s
-    double omegaDetumbleStart = 2 * DEG2RAD;
-    double omegaDetumbleEnd = 1 * DEG2RAD;
+    double omegaDetumbleStart = 2 * M_PI / 180;
+    double omegaDetumbleEnd = 1 * M_PI / 180;
     double gainBdot = pow(10.0, -3.3);
     double gainFull = pow(10.0, -2.3);
     uint8_t useFullDetumbleLaw = false;

mission/controller/acs/AttitudeEstimation.cpp

@@ -1,109 +0,0 @@
-#include "AttitudeEstimation.h"
-
-AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) {
-  acsParameters = acsParameters_;
-}
-
-AttitudeEstimation::~AttitudeEstimation() {}
-
-void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
-                               acsctrl::MgmDataProcessed *mgmData,
-                               acsctrl::AttitudeEstimationData *attitudeEstimationData) {
-  if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
-          mgmData->mgmVecTot.isValid() and mgmData->magIgrfModel.isValid())) {
-    {
-      PoolReadGuard pg{attitudeEstimationData};
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(attitudeEstimationData->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
-        attitudeEstimationData->quatQuest.setValid(false);
-      }
-    }
-    return;
-  }
-
-  // Normalize Data
-  double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
-         normSusI[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(susData->susVecTot.value, normSusB, 3);
-  VectorOperations<double>::normalize(susData->sunIjkModel.value, normSusI, 3);
-  VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMgmB, 3);
-  VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMgmI, 3);
-
-  // Create Helper Vectors
-  double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
-         helperSum[3] = {0, 0, 0};
-  VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
-  VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
-  VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
-  VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
-  VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
-  VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
-
-  // Sensor Weights
-  double kSus = 0, kMgm = 0;
-  kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSus, -2);
-  kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMgm, -2);
-
-  // Weighted Vectors
-  double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
-         kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
-  VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
-  VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
-  VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
-  VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
-
-  // Some weird Angles
-  double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
-                     (VectorOperations<double>::dot(weightedSusB, normSusI) +
-                      VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
-                 VectorOperations<double>::dot(helperCross, kSumVec);
-  double beta = VectorOperations<double>::dot(helperSum, kSumVec);
-  double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
-
-  // I don't even know what this is supposed to be
-  double constPlus =
-      1. / (2 * std::sqrt(gamma * (gamma + alpha) *
-                          (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
-  double constMinus =
-      1. / (2 * std::sqrt(gamma * (gamma - alpha) *
-                          (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
-
-  // Calculate Quaternion
-  double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
-         qRotVecPt1[3] = {0, 0, 0};
-  if (alpha >= 0) {
-    // Scalar Part
-    qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
-    // Rotational Vector Part
-    VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
-    VectorOperations<double>::mulScalar(helperSum, beta, qRotVecPt1, 3);
-    VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
-    std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
-
-    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 4);
-    QuaternionOperations::normalize(qBI, qBI);
-  } else {
-    // Scalar Part
-    qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
-    // Rotational Vector Part
-    VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
-    VectorOperations<double>::mulScalar(helperSum, gamma - alpha, qRotVecPt1, 3);
-    VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
-    std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
-
-    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 4);
-    QuaternionOperations::normalize(qBI, qBI);
-  }
-  // Low Pass
-  if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
-    QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
-  }
-  {
-    PoolReadGuard pg{attitudeEstimationData};
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(attitudeEstimationData->quatQuest.value, qBI, 4 * sizeof(double));
-      attitudeEstimationData->quatQuest.setValid(true);
-    }
-  }
-}

mission/controller/acs/AttitudeEstimation.h

@@ -1,31 +0,0 @@
-#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
-#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
-
-#include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <mission/controller/acs/AcsParameters.h>
-#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
-
-#include <cmath>
-#include <iostream>
-
-class AttitudeEstimation {
- public:
-  AttitudeEstimation(AcsParameters *acsParameters_);
-  virtual ~AttitudeEstimation();
-  ;
-
-  void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData,
-             acsctrl::AttitudeEstimationData *attitudeEstimation);
-
- protected:
- private:
-  AcsParameters *acsParameters;
-
-  double qOld[4] = {0, 0, 0, 0};
-
-  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
-};
-
-#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */

mission/controller/acs/CMakeLists.txt

@@ -2,7 +2,6 @@ target_sources(
   ${LIB_EIVE_MISSION}
   PRIVATE AcsParameters.cpp
           ActuatorCmd.cpp
-          AttitudeEstimation.cpp
           FusedRotationEstimation.cpp
           Guidance.cpp
           Igrf13Model.cpp

mission/controller/acs/FusedRotationEstimation.cpp

@@ -4,221 +4,19 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
   acsParameters = acsParameters_;
 }
 
-void FusedRotationEstimation::estimateFusedRotationRate(
-    const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
-    acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
-    ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData,
-    const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
-    acsctrl::FusedRotRateData *fusedRotRateData) {
-  estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData);
-  estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData);
-  estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
-                                  fusedRotRateSourcesData);
-
-  if (not(mode == acs::AcsMode::SAFE) and (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
-                                           acsParameters->onBoardParams.fusedRateFromStr)) {
-    PoolReadGuard pg(fusedRotRateData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->rotRateOrthogonal.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->rotRateParallel.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateTotal.value,
-                  fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
-      fusedRotRateData->rotRateTotal.setValid(true);
-      fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
-      fusedRotRateData->rotRateSource.setValid(true);
-    }
-  } else if (not(mode == acs::AcsMode::SAFE) and
-             (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
-              acsParameters->onBoardParams.fusedRateFromQuest)) {
-    PoolReadGuard pg(fusedRotRateData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->rotRateOrthogonal.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->rotRateParallel.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateTotal.value,
-                  fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
-      fusedRotRateData->rotRateTotal.setValid(true);
-      fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
-      fusedRotRateData->rotRateSource.setValid(true);
-    }
-  } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
-    std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
-                fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double));
-    fusedRotRateData->rotRateOrthogonal.setValid(
-        fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid());
-    std::memcpy(fusedRotRateData->rotRateParallel.value,
-                fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double));
-    fusedRotRateData->rotRateParallel.setValid(
-        fusedRotRateSourcesData->rotRateParallelSusMgm.isValid());
-    std::memcpy(fusedRotRateData->rotRateTotal.value,
-                fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
-    fusedRotRateData->rotRateTotal.setValid(true);
-    fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
-    fusedRotRateData->rotRateSource.setValid(true);
-  } else {
-    PoolReadGuard pg(fusedRotRateData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
-      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateData->setValidity(false, true);
-      fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
-      fusedRotRateData->rotRateSource.setValid(true);
-    }
-  }
-}
-
-void FusedRotationEstimation::estimateFusedRotationRateStr(
-    ACS::SensorValues *sensorValues, const double timeDelta,
-    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
-  if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
-          sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
-    {
-      PoolReadGuard pg(fusedRotRateSourcesData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
-      }
-    }
-    std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
-    return;
-  }
-
-  double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
-                       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
-  if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
-    double quatOldInv[4] = {0, 0, 0, 0};
-    double quatDelta[4] = {0, 0, 0, 0};
-
-    QuaternionOperations::inverse(quatOldStr, quatOldInv);
-    QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
-    if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
-      QuaternionOperations::normalize(quatDelta);
-    }
-
-    double rotVec[3] = {0, 0, 0};
-    double angle = QuaternionOperations::getAngle(quatDelta);
-    if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
-      {
-        PoolReadGuard pg(fusedRotRateSourcesData);
-        if (pg.getReadResult() == returnvalue::OK) {
-          std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
-                      3 * sizeof(double));
-          fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
-        }
-      }
-      std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
-      return;
-    }
-    VectorOperations<double>::normalize(quatDelta, rotVec, 3);
-    VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
-    {
-      PoolReadGuard pg(fusedRotRateSourcesData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
-      }
-    }
-    std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
-    return;
-  }
-  {
-    PoolReadGuard pg(fusedRotRateSourcesData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
-    }
-  }
-  std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
-  return;
-}
-
-void FusedRotationEstimation::estimateFusedRotationRateQuest(
-    acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
-    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
-  if (not attitudeEstimationData->quatQuest.isValid()) {
-    {
-      PoolReadGuard pg(fusedRotRateSourcesData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
-                    3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
-      }
-    }
-    std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
-  }
-
-  if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
-    double quatOldInv[4] = {0, 0, 0, 0};
-    double quatDelta[4] = {0, 0, 0, 0};
-
-    QuaternionOperations::inverse(quatOldQuest, quatOldInv);
-    QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta);
-    if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
-      QuaternionOperations::normalize(quatDelta);
-    }
-
-    double rotVec[3] = {0, 0, 0};
-    double angle = QuaternionOperations::getAngle(quatDelta);
-    if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
-      {
-        PoolReadGuard pg(fusedRotRateSourcesData);
-        if (pg.getReadResult() == returnvalue::OK) {
-          std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
-                      3 * sizeof(double));
-          fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
-        }
-      }
-      std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
-      return;
-    }
-    VectorOperations<double>::normalize(quatDelta, rotVec, 3);
-    VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
-    {
-      PoolReadGuard pg(fusedRotRateSourcesData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
-      }
-    }
-    std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
-    return;
-  }
-  {
-    PoolReadGuard pg(fusedRotRateSourcesData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
-    }
-  }
-  std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
-  return;
-}
-
-void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
+void FusedRotationEstimation::estimateFusedRotationRateSafe(
     acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
-    acsctrl::GyrDataProcessed *gyrDataProcessed,
-    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
+    acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
   if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
-       not fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) or
+       not fusedRotRateData->rotRateTotal.isValid()) or
       (not susDataProcessed->susVecTotDerivative.isValid() and
        not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
     {
-      PoolReadGuard pg(fusedRotRateSourcesData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
-                    3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
-        std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
-                    3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
-        std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
-                    3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
-      }
+      PoolReadGuard pg(fusedRotRateData);
+      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
+      fusedRotRateData->setValidity(false, true);
     }
     // store for calculation of angular acceleration
     if (gyrDataProcessed->gyrVecTot.isValid()) {
@@ -227,7 +25,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
     return;
   }
   if (not susDataProcessed->susVecTot.isValid()) {
-    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
+    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
     // store for calculation of angular acceleration
     if (gyrDataProcessed->gyrVecTot.isValid()) {
       std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@@ -251,7 +49,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
     VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
                                         fusedRotRateParallel, 3);
   } else {
-    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
+    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
     // store for calculation of angular acceleration
     if (gyrDataProcessed->gyrVecTot.isValid()) {
       std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@@ -273,18 +71,12 @@ void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
   VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
 
   {
-    PoolReadGuard pg(fusedRotRateSourcesData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, fusedRotRateOrthogonal,
-                  3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(true);
-      std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, fusedRotRateParallel,
-                  3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(true);
-      std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
-                  3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
-    }
+    PoolReadGuard pg(fusedRotRateData);
+    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
+                3 * sizeof(double));
+    std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
+    std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
+    fusedRotRateData->setValidity(true, true);
   }
 
   // store for calculation of angular acceleration
@@ -294,44 +86,31 @@ void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
 }
 
 void FusedRotationEstimation::estimateFusedRotationRateEclipse(
-    acsctrl::GyrDataProcessed *gyrDataProcessed,
-    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
+    acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
   if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or
       not gyrDataProcessed->gyrVecTot.isValid() or
-      VectorOperations<double>::norm(fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3) == 0) {
+      VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
     {
-      PoolReadGuard pg(fusedRotRateSourcesData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
-                    3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
-        std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
-                    3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
-        std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
-                    3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
-      }
+      PoolReadGuard pg(fusedRotRateData);
+      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
+      fusedRotRateData->setValidity(false, true);
     }
     return;
   }
   double angAccelB[3] = {0, 0, 0};
   VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
   double fusedRotRateTotal[3] = {0, 0, 0};
-  VectorOperations<double>::add(fusedRotRateSourcesData->rotRateTotalSusMgm.value, angAccelB,
-                                fusedRotRateTotal, 3);
+  VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal,
+                                3);
   {
-    PoolReadGuard pg(fusedRotRateSourcesData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
-                  3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
-      std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
-                  3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
-      std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
-                  3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
-    }
+    PoolReadGuard pg(fusedRotRateData);
+    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+    fusedRotRateData->rotRateOrthogonal.setValid(false);
+    std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+    fusedRotRateData->rotRateParallel.setValid(false);
+    std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
+    fusedRotRateData->rotRateTotal.setValid(true);
   }
 }

mission/controller/acs/FusedRotationEstimation.h

@@ -2,46 +2,28 @@
 #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
 
 #include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <mission/controller/acs/AcsParameters.h>
-#include <mission/controller/acs/SensorValues.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class FusedRotationEstimation {
  public:
   FusedRotationEstimation(AcsParameters *acsParameters_);
 
-  void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
-                                 acsctrl::MgmDataProcessed *mgmDataProcessed,
-                                 acsctrl::GyrDataProcessed *gyrDataProcessed,
-                                 ACS::SensorValues *sensorValues,
-                                 acsctrl::AttitudeEstimationData *attitudeEstimationData,
-                                 const double timeDelta,
-                                 acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
-                                 acsctrl::FusedRotRateData *fusedRotRateData);
+  void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
+                                     acsctrl::MgmDataProcessed *mgmDataProcessed,
+                                     acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                     acsctrl::FusedRotRateData *fusedRotRateData);
 
  protected:
  private:
-  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
-  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
+  static constexpr double ZERO_VEC[3] = {0, 0, 0};
 
   AcsParameters *acsParameters;
-  double quatOldQuest[4] = {0, 0, 0, 0};
-  double quatOldStr[4] = {0, 0, 0, 0};
   double rotRateOldB[3] = {0, 0, 0};
 
-  void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed,
-                                       acsctrl::MgmDataProcessed *mgmDataProcessed,
-                                       acsctrl::GyrDataProcessed *gyrDataProcessed,
-                                       acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
   void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
-                                        acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
-  void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData,
-                                      const double timeDelta,
-                                      acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
-  void estimateFusedRotationRateStr(ACS::SensorValues *sensorValues, const double timeDelta,
-                                    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
+                                        acsctrl::FusedRotRateData *fusedRotRateData);
 };
 
 #endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */

mission/controller/acs/Guidance.cpp

@@ -1,304 +1,422 @@
 #include "Guidance.h"
 
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <math.h>
+
+#include <filesystem>
+
+#include "string.h"
+#include "util/CholeskyDecomposition.h"
+#include "util/MathOperations.h"
+
 Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
 Guidance::~Guidance() {}
 
-void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
-                                 const double sunDirI[3], const double posSatF[4],
-                                 double targetQuat[4], double targetSatRotRate[3]) {
-  // positive z-Axis of EIVE in direction of sun
-  double zAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(sunDirI, zAxisIX, 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 ECEF
+  double targetE[3] = {0, 0, 0};
 
-  // determine helper vector to point x-Axis and therefore the STR away from Earth
-  double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
-  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
-  VectorOperations<double>::normalize(posSatI, helperXI, 3);
+  MathOperations<double>::cartesianFromLatLongAlt(
+      acsParameters->targetModeControllerParameters.latitudeTgt,
+      acsParameters->targetModeControllerParameters.longitudeTgt,
+      acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
 
-  // construct y-axis from helper vector and z-axis
-  double yAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxisIX, helperXI, yAxisIX);
-  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
+  // target direction in the ECEF frame
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
 
-  // x-axis completes RHS
-  double xAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::cross(yAxisIX, zAxisIX, xAxisIX);
-  VectorOperations<double>::normalize(xAxisIX, xAxisIX, 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);
 
-  // join transformation matrix
-  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
-                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
-                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
-  QuaternionOperations::fromDcm(dcmIX, targetQuat);
+  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
 
-  // calculate of reference rotation rate
-  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
+  // 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->targetModeControllerParameters.refDirection[0];
+  refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1];
+  refDir[2] = acsParameters->targetModeControllerParameters.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
+  //-------------------------------------------------------------------------------------
+  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
+  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(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3);
+  MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
+  VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
+
+  double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
+  double normRefSatRate = normVelSatB / normTargetDirB;
+
+  double satRateDir[3] = {0, 0, 0};
+  VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
+  VectorOperations<double>::normalize(satRateDir, satRateDir, 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};
+    MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
+
+    double exclAngle = acsParameters->strParameters.exclusionAngle,
+           blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
+           blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
+    double sightAngleSun =
+        VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
+
+    if (!(strBlindAvoidFlag)) {
+      double critSightAngle = blindStart * exclAngle;
+      if (sightAngleSun < critSightAngle) {
+        strBlindAvoidFlag = true;
+      }
+    } 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;
+          blindRefRate[2] = 0;
+        } else {
+          blindRefRate[0] = -normBlindRefRate;
+          blindRefRate[1] = 0;
+          blindRefRate[2] = 0;
+        }
+        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::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
-                                   const double posSatF[3], const double velSatF[3],
-                                   double targetQuat[4], double targetSatRotRate[3]) {
+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 targetF[3] = {0, 0, 0};
-  CoordinateTransformations::cartesianFromLatLongAlt(
+  double targetE[3] = {0, 0, 0};
+  MathOperations<double>::cartesianFromLatLongAlt(
       acsParameters->targetModeControllerParameters.latitudeTgt,
       acsParameters->targetModeControllerParameters.longitudeTgt,
-      acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
+      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};
-  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
-  CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute);
+  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 xAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirI, xAxisIX, 3);
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirI, xAxis, 3);
 
   // transform velocity into inertial frame
-  double velSatI[3] = {0, 0, 0};
-  CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute);
+  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, velSatI, orbitalNormalI);
+  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 yAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::cross(orbitalNormalI, xAxisIX, yAxisIX);
-  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
+  double yAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
 
   // z-axis completes RHS
-  double zAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::cross(xAxisIX, yAxisIX, zAxisIX);
+  double zAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(xAxis, yAxis, zAxis);
 
   // join transformation matrix
-  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
-                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
-                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
+  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);
 
-  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
+  int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
+  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
-                               const double posSatF[3], const double sunDirI[3],
+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 posGroundStationF[3] = {0, 0, 0};
-  CoordinateTransformations::cartesianFromLatLongAlt(
+  double groundStationE[3] = {0, 0, 0};
+
+  MathOperations<double>::cartesianFromLatLongAlt(
       acsParameters->gsTargetModeControllerParameters.latitudeTgt,
       acsParameters->gsTargetModeControllerParameters.longitudeTgt,
-      acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
+      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}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
-  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
-  CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute);
-  VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
+  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 xAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
-  VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
+  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
 
-  // get earth vector in ECI
-  double earthDirI[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(posSatI, earthDirI, 3);
-  VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
+  // get sun vector model in ECI
+  VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
 
-  // sun avoidance calculations
-  double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
-                                      sunPerpendicularX, 3);
-  VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
-  VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
-  VectorOperations<double>::mulScalar(sunFloorYZ, -1, zAxisSun, 3);
-  double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
-         strVecSunZ[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
-                                      strVecSunX, 3);
-  VectorOperations<double>::mulScalar(zAxisSun, acsParameters->strParameters.boresightAxis[2],
-                                      strVecSunZ, 3);
-  VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
-  VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
-  sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
+  // 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);
 
-  // earth avoidance calculations
-  double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
-                                      earthPerpendicularX, 3);
-  VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
-  VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
-  VectorOperations<double>::mulScalar(earthFloorYZ, -1, zAxisEarth, 3);
-  double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
-         strVecEarthZ[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
-                                      strVecEarthX, 3);
-  VectorOperations<double>::mulScalar(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
-                                      strVecEarthZ, 3);
-  VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
-  VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
-  earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirI);
-
-  if ((sunWeight == 0.0) and (earthWeight == 0.0)) {
-    // if this actually ever happens i will eat a broom
-    sunWeight = 0.5;
-    earthWeight = 0.5;
-  }
-
-  // normalize weights for convenience
-  double normFactor = 1. / (std::abs(sunWeight) + std::abs(earthWeight));
-  sunWeight *= normFactor;
-  earthWeight *= normFactor;
-
-  // calculate z-axis for str blinding avoidance
-  double zAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
-  VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
-  VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
-  VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
-  VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
-
-  // calculate y-axis
-  double yAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
-  VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
+  // y-axis completes RHS
+  double yAxis[3];
+  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
 
   // join transformation matrix
-  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
-                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
-                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
-  QuaternionOperations::fromDcm(dcmIX, targetQuat);
+  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);
 
-  limitReferenceRotation(xAxisIX, targetQuat);
-  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
-
-  std::memcpy(xAxisIXprev, xAxisIX, sizeof(xAxisIXprev));
+  int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
+  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
-                                  const double posSatE[3], const double velSatE[3],
-                                  double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
+                                double targetSatRotRate[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
+  //----------------------------------------------------------------------------
+  int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
+  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
+}
+
+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};
-  CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute);
+  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 xAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(posSatI, xAxisIX, 3);
-  VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
+  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 zAxisIX[3] = {0, 0, 0};
-  double velSatI[3] = {0, 0, 0};
-  CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
-  VectorOperations<double>::cross(xAxisIX, velSatI, zAxisIX);
-  VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
+  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 yAxisIX[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
+  double yAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
 
   // join transformation matrix
-  double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
-                        {xAxisIX[1], yAxisIX[1], zAxisIX[1]},
-                        {xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
-  QuaternionOperations::fromDcm(dcmIX, targetQuat);
+  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);
 
-  targetRotationRate(timeDelta, targetQuat, refSatRate);
-}
-
-void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) {
-  if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
-    std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
-  }
-  if (timeDelta != 0.0) {
-    QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
-  } else {
-    std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
-  }
-  std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
-}
-
-void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) {
-  if ((VectorOperations<double>::norm(quatIXprev, 4) == 0) or
-      (VectorOperations<double>::norm(xAxisIXprev, 3) == 0)) {
-    return;
-  }
-
-  // check required rotation and return if below limit
-  double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0};
-  QuaternionOperations::inverse(quatIXprev, quatXprevI);
-  QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX);
-  QuaternionOperations::normalize(quatXprevX);
-  double phiMax = acsParameters->gsTargetModeControllerParameters.omMax *
-                  acsParameters->onBoardParams.sampleTime;
-  if (2 * std::acos(quatXprevX[3]) < phiMax) {
-    return;
-  }
-
-  // x-axis always needs full rotation
-  double phiX = 0, phiXvec[3] = {0, 0, 0};
-  phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));
-  VectorOperations<double>::cross(xAxisIXprev, xAxisIX, phiXvec);
-  VectorOperations<double>::normalize(phiXvec, phiXvec, 3);
-
-  double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0};
-  VectorOperations<double>::mulScalar(phiXvec, -std::sin(phiX / 2.), phiXvec, 3);
-  std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec));
-  quatXprevXtilde[3] = cos(phiX / 2.);
-  QuaternionOperations::normalize(quatXprevXtilde);
-  QuaternionOperations::multiply(quatXprevXtilde, quatIXprev, quatIXtilde);
-
-  // use the residual rotation up to the maximum
-  double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0};
-  QuaternionOperations::inverse(quatIX, quatXI);
-  QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde);
-
-  double phiResidual = 0, phiResidualVec[3] = {0, 0, 0};
-  phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX));
-  std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec));
-  VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);
-
-  double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0};
-  VectorOperations<double>::mulScalar(phiResidualVec, std::sin(phiResidual / 2.), phiResidualVec,
-                                      3);
-  std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec));
-  quatXhatXTilde[3] = std::cos(phiResidual / 2.);
-  QuaternionOperations::normalize(quatXhatXTilde);
-
-  // calculate final quaternion
-  QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat);
-  QuaternionOperations::multiply(quatXTildeXhat, quatIXtilde, quatIX);
-  QuaternionOperations::normalize(quatIX);
+  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 without reference
-  // quaternion
-  double errorQuatWoRef[4] = {0, 0, 0, 0};
-  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
-  // Then add rotation from reference quaternion
-  QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
+  // 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);
@@ -307,11 +425,7 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
   errorAngle = QuaternionOperations::getAngle(errorQuat, true);
 
   // Calculate error satellite rotational rate
-  // Convert target rotational rate into body RF
-  double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0};
-  QuaternionOperations::inverse(errorQuat, errorQuatInv);
-  QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB);
-  // Combine the target and reference satellite rotational rates
+  // First combine the target and reference satellite rotational rates
   double combinedRefSatRotRate[3] = {0, 0, 0};
   VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
   // Then subtract the combined required satellite rotational rates from the actual rate
@@ -321,48 +435,91 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                           double targetSatRotRate[3], double errorQuat[4],
                           double errorSatRotRate[3], double &errorAngle) {
-  double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0};
-  comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate,
-             errorQuat, errorSatRotRate, 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);
+
+  // Calculate error satellite rotational rate
+  VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
+}
+
+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 * 1e-6 -
+                       (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
+  if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
+    std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
+  }
+  if (timeElapsed < timeElapsedMax) {
+    double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
+    QuaternionOperations::inverse(quatInertialTarget, q);
+    QuaternionOperations::inverse(savedQuaternion, qS);
+    double qDiff[4] = {0, 0, 0, 0};
+    VectorOperations<double>::subtract(q, qS, qDiff, 4);
+    VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
+
+    double tgtQuatVec[3] = {q[0], q[1], q[2]};
+    double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
+    double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
+    VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
+    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
+    VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
+    VectorOperations<double>::add(sum1, sum2, sum, 3);
+    VectorOperations<double>::subtract(sum, sum3, sum, 3);
+    double omegaRefNew[3] = {0, 0, 0};
+    VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
+
+    VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
+    VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
+    omegaRefSaved[0] = omegaRefNew[0];
+    omegaRefSaved[1] = omegaRefNew[1];
+    omegaRefSaved[2] = omegaRefNew[2];
+  } else {
+    refSatRate[0] = 0;
+    refSatRate[1] = 0;
+    refSatRate[2] = 0;
+  }
+
+  timeSavedQuaternion = now;
+  savedQuaternion[0] = quatInertialTarget[0];
+  savedQuaternion[1] = quatInertialTarget[1];
+  savedQuaternion[2] = quatInertialTarget[2];
+  savedQuaternion[3] = quatInertialTarget[3];
 }
 
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
-                                                double *rwPseudoInv, acsctrl::RwAvail *rwAvail) {
-  rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
-  rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
-  rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
-  rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
+                                                double *rwPseudoInv) {
+  bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
+  bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
+  bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
+  bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
 
-  if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) {
+  if (rw1valid && rw2valid && rw3valid && rw4valid) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
     return returnvalue::OK;
-  } else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
-             rwAvail->rw4avail) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
-                12 * sizeof(double));
-    return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and
-             rwAvail->rw4avail) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
-                12 * sizeof(double));
-    return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and
-             rwAvail->rw4avail) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
-                12 * sizeof(double));
-    return acsctrl::SINGLE_RW_UNAVAILABLE;
-  } else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and
-             not rwAvail->rw4avail) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
-                12 * sizeof(double));
-    return acsctrl::SINGLE_RW_UNAVAILABLE;
+  } else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
+    return returnvalue::OK;
+  } else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
+    return returnvalue::OK;
+  } else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
+    return returnvalue::OK;
+  } else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
+    return returnvalue::OK;
+  } else {
+    return returnvalue::FAILED;
   }
-  return acsctrl::MULTIPLE_RW_UNAVAILABLE;
-}
-
-void Guidance::resetValues() {
-  std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev));
-  std::memcpy(xAxisIXprev, ZERO_VEC3, sizeof(xAxisIXprev));
 }
 
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {

mission/controller/acs/Guidance.h

@@ -1,18 +1,11 @@
 #ifndef GUIDANCE_H_
 #define GUIDANCE_H_
 
-#include <fsfw/coordinates/CoordinateTransformations.h>
-#include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <mission/controller/acs/AcsParameters.h>
-#include <mission/controller/acs/SensorValues.h>
-#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
+#include <time.h>
 
-#include <cmath>
-#include <filesystem>
-#include <string>
+#include "../controllerdefinitions/AcsCtrlDefinitions.h"
+#include "AcsParameters.h"
+#include "SensorValues.h"
 
 class Guidance {
  public:
@@ -21,22 +14,33 @@ class Guidance {
 
   void getTargetParamsSafe(double sunTargetSafe[3]);
   ReturnValue_t solarArrayDeploymentComplete();
-  void resetValues();
 
-  void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3],
-                         const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
-  void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
-                           const double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
-  void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
-                       const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
-  void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
-                          const double velSatF[3], double targetQuat[4], double refSatRate[3]);
+  // Function to get the target quaternion and reference rotation rate from gps position and
+  // position of the ground station
+  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]);
 
-  void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
-                          double *targetSatRotRate);
+  // Function to get the target quaternion and reference rotation rate for sun pointing after ground
+  // station
+  void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
+                        double targetSatRotRate[3]);
 
-  void limitReferenceRotation(const double xAxisIX[3], double quatIX[4]);
+  // Function to get the target quaternion and refence rotation rate from gps position for Nadir
+  // pointing
+  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]);
 
+  // @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);
@@ -44,18 +48,20 @@ class Guidance {
                   double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
                   double &errorAngle);
 
-  ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv,
-                                        acsctrl::RwAvail *rwAvail);
+  void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+                          double *targetSatRotRate);
+
+  // @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);
 
  private:
   const AcsParameters *acsParameters;
 
-  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
-  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
-
   bool strBlindAvoidFlag = false;
-  double quatIXprev[4] = {0, 0, 0, 0};
-  double xAxisIXprev[3] = {0, 0, 0};
+  timeval timeSavedQuaternion;
+  double savedQuaternion[4] = {0, 0, 0, 0};
+  double omegaRefSaved[3] = {0, 0, 0};
 
   static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
   static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";

mission/controller/acs/Igrf13Model.cpp

@@ -1,5 +1,19 @@
 #include "Igrf13Model.h"
 
+#include <fsfw/src/fsfw/globalfunctions/constants.h>
+#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
+#include <stdint.h>
+#include <string.h>
+#include <time.h>
+
+#include <cmath>
+
+#include "util/MathOperations.h"
+
+using namespace Math;
+
 Igrf13Model::Igrf13Model() {}
 Igrf13Model::~Igrf13Model() {}
 
@@ -9,7 +23,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
   double magFieldModel[3] = {0, 0, 0};
   double phi = longitude, theta = gcLatitude;  // geocentric
   /* Here is the co-latitude needed*/
-  theta -= 90. * M_PI / 180.;
+  theta -= 90 * PI / 180;
   theta *= (-1);
 
   double rE = 6371200.0;  // radius earth [m]
@@ -69,13 +83,13 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
   magFieldModel[1] *= -1;
   magFieldModel[2] *= (-1 / sin(theta));
 
-  double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement);
+  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
   double UT1 = JD2000 / 36525.;
 
   double gst =
       280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
   gst = std::fmod(gst, 360.);
-  gst *= M_PI / 180.;
+  gst *= PI / 180.;
   double lst = gst + longitude;  // local sidereal time [rad]
 
   magFieldModelInertial[0] =
@@ -93,7 +107,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
 
 void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
   double JD2000Igrf = (2458850.0 - 2451545);  // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
-  double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement);
+  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
   double days = ceil(JD2000 - JD2000Igrf);
   for (int i = 0; i <= igrfOrder; i++) {
     for (int j = 0; j <= (igrfOrder - 1); j++) {

mission/controller/acs/Igrf13Model.h

@@ -16,11 +16,10 @@
 #ifndef IGRF13MODEL_H_
 #define IGRF13MODEL_H_
 
-#include <fsfw/src/fsfw/globalfunctions/TimeSystems.h>
-#include <fsfw/src/fsfw/globalfunctions/constants.h>
-#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/parameters/HasParametersIF.h>
+#include <stdint.h>
+#include <string.h>
+#include <time.h>
 
 #include <cmath>
 

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -1,16 +1,14 @@
 #ifndef MULTIPLICATIVEKALMANFILTER_H_
 #define MULTIPLICATIVEKALMANFILTER_H_
 
-#include <common/config/eive/resultClassIds.h>
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <mission/controller/acs/AcsParameters.h>
-#include <mission/controller/acs/SensorValues.h>
-#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
+#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 gyroscope bias by
+  /* @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
@@ -20,26 +18,56 @@ class MultiplicativeKalmanFilter {
  public:
   /* @brief: Constructor
    */
-  MultiplicativeKalmanFilter(AcsParameters *acsParameters);
+  MultiplicativeKalmanFilter();
   virtual ~MultiplicativeKalmanFilter();
 
-  ReturnValue_t reset(acsctrl::AttitudeEstimationData *attitudeEstimationData);
+  ReturnValue_t reset(acsctrl::MekfData *mekfData);
 
-  ReturnValue_t init(const acsctrl::SusDataProcessed *susData,
-                     const acsctrl::MgmDataProcessed *mgmData,
-                     const acsctrl::GyrDataProcessed *gyrData,
-                     acsctrl::AttitudeEstimationData *attitudeEstimationData);
+  /* @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
+   */
+  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,
+                     AcsParameters *acsParameters);
 
-  ReturnValue_t mekfEst(const acsctrl::SusDataProcessed *susData,
-                        const acsctrl::MgmDataProcessed *mgmData,
-                        const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
-                        acsctrl::AttitudeEstimationData *attitudeEstimationData);
+  /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
+   * for the current step after the initalization
+   * @param:  quaternionSTR   Star Tracker Quaternion between from body to ECI frame
+   * 			rateGYRs_ 		Estimated satellite rotation rate from the
+   * Gyroscopes [rad/s] 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
+   * 			outputQuat 		Stores the calculated quaternion
+   * 			outputSatRate 	Stores the adjusted satellite rate
+   * @return 	ReturnValue_t	Feedback of this class, KALMAN_NO_GYR_MEAS if no satellite rate from
+   * the sensors was provided, KALMAN_NO_MODEL if no sunDirJ or magFieldJ was given from the model
+   * calculations, KALMAN_INVERSION_FAILED if the calculation of the Gain matrix was not possible,
+   * 							RETURN_OK else
+   */
+  ReturnValue_t mekfEst(const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
+                        const bool validGYRs_, 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,
+                        AcsParameters *acsParameters);
 
-  void updateStandardDeviations(const AcsParameters *acsParameters);
-
-  void setStrData(const double qX, const double qY, const double qZ, const double qW,
-                  const bool valid);
+  enum MekfStatus : uint8_t {
+    UNINITIALIZED = 0,
+    NO_GYR_DATA = 1,
+    NO_MODEL_VECTORS = 2,
+    NO_SUS_MGM_STR_DATA = 3,
+    COVARIANCE_INVERSION_FAILED = 4,
+    NOT_FINITE = 5,
+    INITIALIZED = 10,
+    RUNNING = 11,
+  };
 
+  // resetting Mekf
   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);
@@ -54,93 +82,26 @@ class MultiplicativeKalmanFilter {
  private:
   static constexpr double ZERO_VEC3[3] = {0, 0, 0};
   static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
-  static constexpr double ZERO_MAT66[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}};
-  static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1};
-  static constexpr double EYE3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
-  static constexpr double EYE6[6][6] = {{1, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0},
-                                        {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
 
-  enum MekfStatus : uint8_t {
-    UNINITIALIZED = 0,
-    NO_GYR_DATA = 1,
-    NO_MODEL_VECTORS = 2,
-    NO_SUS_MGM_STR_DATA = 3,
-    COVARIANCE_INVERSION_FAILED = 4,
-    NOT_FINITE = 5,
-    INITIALIZED = 10,
-    RUNNING = 11,
-  };
+  /*Parameters*/
+  double quaternion_STR_SB[4];
 
-  enum SensorAvailability : uint8_t {
-    NONE = 0,
-    SUS_MGM_STR = 1,
-    SUS_MGM = 2,
-    SUS_STR = 3,
-    MGM_STR = 4,
-    SUS = 5,
-    MGM = 6,
-    STR = 7,
-  };
+  /*States*/
+  double initialQuaternion[4] = {0, 0, 0, 1}; /*after reset?QUEST*/
+  double initialCovarianceMatrix[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}};
+  double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
+  uint8_t sensorsAvail = 0;
 
-  MekfStatus mekfStatus = MekfStatus::UNINITIALIZED;
-
-  struct StrData {
-    struct StrQuat {
-      double value[4] = {0, 0, 0, 0};
-      bool valid = false;
-    } strQuat;
-  } strData;
-
-  // Standard Deviations
-  double sigmaSus = 0;
-  double sigmaMgm = 0;
-  double sigmaStr = 0;
-  double sigmaGyr = 0;
-  // sigmaV
-  double sigmaGyrArw = 0;
-  // sigmaU
-  double sigmaGyrBs = 0;
-
-  // Covariance Matrices
-  double covSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double covMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double covStr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-
-  double covAposteriori[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}};
-
-  // Sensor Availability
-  SensorAvailability sensorsAvailable = SensorAvailability::NONE;
-  uint8_t matrixDimensionFactor = 0;
-
-  // Estimated States
-  double estimatedQuaternionBI[4] = {0, 0, 0, 1};
-  double estimatedBiasGyr[3] = {0, 0, 0};
-  double estimatedRotRate[3] = {0, 0, 0};
-  double estimatedCovarianceMatrix[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}};
-
-  // Functions
-  ReturnValue_t checkAvailableSensors(const acsctrl::SusDataProcessed *susData,
-                                      const acsctrl::MgmDataProcessed *mgmData,
-                                      const acsctrl::GyrDataProcessed *gyrData,
-                                      acsctrl::AttitudeEstimationData *attitudeEstimationData);
-
-  void kfUpdate(const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
-                double *measSensMatrix, double *measCovMatrix, double *measVec, double *measEstVec);
-  ReturnValue_t kfGain(double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
-                       acsctrl::AttitudeEstimationData *attitudeEstimationData);
-  void kfCovAposteriori(double *kalmanGain, double *measSensMatrix);
-  void kfStateAposteriori(double *kalmanGain, double *measVec, double *estVec);
-  void kfPropagate(const acsctrl::GyrDataProcessed *gyrData, const double timeDiff);
-
-  ReturnValue_t kfFiniteCheck(acsctrl::AttitudeEstimationData *attitudeEstimationData);
-
-  void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *attitudeEstimationData);
-  void updateDataSet(acsctrl::AttitudeEstimationData *attitudeEstimationData);
+  /*Outputs*/
+  double quatBJ[4];  /* Output Quaternion */
+  double biasGYR[3]; /*Between measured and estimated sat Rate*/
+                     /*Parameter INIT*/
+  /*Functions*/
+  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,32 +1,47 @@
 #include "Navigation.h"
 
-Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <math.h>
+
+#include "util/CholeskyDecomposition.h"
+#include "util/MathOperations.h"
+
+Navigation::Navigation() {}
 
 Navigation::~Navigation() {}
 
-ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
-                                  const acsctrl::GyrDataProcessed *gyrDataProcessed,
-                                  const acsctrl::MgmDataProcessed *mgmDataProcessed,
-                                  const acsctrl::SusDataProcessed *susDataProcessed,
-                                  const double timeDelta,
-                                  acsctrl::AttitudeEstimationData *attitudeEstimationData) {
-  multiplicativeKalmanFilter.setStrData(
-      sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
-      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
-      sensorValues->strSet.caliQx.isValid());
+ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
+                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
+                                  acsctrl::SusDataProcessed *susDataProcessed,
+                                  acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
+  double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
+                      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
+  bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
 
   if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
-    mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
-                                                 gyrDataProcessed, attitudeEstimationData);
+    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,
+        acsParameters);
     return mekfStatus;
   } else {
     mekfStatus = multiplicativeKalmanFilter.mekfEst(
-        susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
+        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(), mekfData, acsParameters);
     return mekfStatus;
   }
 }
 
-void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
+void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
   mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
 }
 
@@ -39,7 +54,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
     {
       PoolReadGuard pg(gpsDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        gpsDataProcessed->source = acs::gps::Source::SPG4;
+        gpsDataProcessed->source = acs::GpsSource::SPG4;
         gpsDataProcessed->source.setValid(true);
         std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
         gpsDataProcessed->gpsPosition.setValid(true);
@@ -51,7 +66,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
     {
       PoolReadGuard pg(gpsDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        gpsDataProcessed->source = acs::gps::Source::NONE;
+        gpsDataProcessed->source = acs::GpsSource::NONE;
         gpsDataProcessed->source.setValid(true);
         std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
         gpsDataProcessed->gpsPosition.setValid(false);
@@ -66,7 +81,3 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
 ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
   return sgp4Propagator.initialize(line1, line2);
 }
-
-void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
-  multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
-}

mission/controller/acs/Navigation.h

@@ -5,24 +5,24 @@
 #include <mission/acs/defs.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/MultiplicativeKalmanFilter.h>
+#include <mission/controller/acs/SensorProcessing.h>
 #include <mission/controller/acs/SensorValues.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class Navigation {
  public:
-  Navigation(AcsParameters *acsParameters);
+  Navigation();
   virtual ~Navigation();
 
-  ReturnValue_t useMekf(const ACS::SensorValues *sensorValues,
-                        const acsctrl::GyrDataProcessed *gyrDataProcessed,
-                        const acsctrl::MgmDataProcessed *mgmDataProcessed,
-                        const acsctrl::SusDataProcessed *susDataProcessed, const double timeDelta,
-                        acsctrl::AttitudeEstimationData *attitudeEstimationData);
-  void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
+  ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
+                        acsctrl::GyrDataProcessed *gyrDataProcessed,
+                        acsctrl::MgmDataProcessed *mgmDataProcessed,
+                        acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
+                        AcsParameters *acsParameters);
+  void resetMekf(acsctrl::MekfData *mekfData);
 
   ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
   ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
-  void updateMekfStandardDeviations(const AcsParameters *acsParameters);
 
  protected:
  private:

mission/controller/acs/SensorProcessing.cpp

@@ -7,7 +7,7 @@ SensorProcessing::~SensorProcessing() {}
 void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value,
                                   bool mgm1valid, const float *mgm2Value, bool mgm2valid,
                                   const float *mgm3Value, bool mgm3valid, const float *mgm4Value,
-                                  bool mgm4valid, timeval timeAbsolute, double timeDelta,
+                                  bool mgm4valid, timeval timeOfMgmMeasurement,
                                   const AcsParameters::MgmHandlingParameters *mgmParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed,
                                   acsctrl::MgmDataProcessed *mgmDataProcessed) {
@@ -15,14 +15,14 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   // ------------------------------------------------
   double magIgrfModel[3] = {0.0, 0.0, 0.0};
   bool gpsValid = false;
-  if (gpsDataProcessed->source.value != acs::gps::Source::NONE) {
-    // There seems to be a bug here, which causes the model vector to drift until infinity, if the
-    // model class is not initialized new every time. Works for now, but should be investigated.
+  if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
     Igrf13Model igrf13;
     igrf13.schmidtNormalization();
-    igrf13.updateCoeffGH(timeAbsolute);
+    igrf13.updateCoeffGH(timeOfMgmMeasurement);
+    // maybe put a condition here, to only update after a full day, this
+    // class function has around 700 steps to perform
     igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
-                        gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel);
+                        gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel);
     gpsValid = true;
   }
   if (not mgm0valid and not mgm1valid and not mgm2valid and not mgm3valid and
@@ -129,12 +129,14 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   //-----------------------Mgm Rate Computation ---------------------------------------------------
   double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
   bool mgmVecTotDerivativeValid = false;
-  if (timeDelta > 0 and VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
+  double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
+  if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
+      VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
     VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
-    VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDelta, mgmVecTotDerivative,
-                                        3);
+    VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3);
     mgmVecTotDerivativeValid = true;
   }
+  timeOfSavedMagFieldEst = timeOfMgmMeasurement;
   std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
 
   if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
@@ -175,12 +177,11 @@ void SensorProcessing::processSus(
     const uint16_t *sus6Value, bool sus6valid, const uint16_t *sus7Value, bool sus7valid,
     const uint16_t *sus8Value, bool sus8valid, const uint16_t *sus9Value, bool sus9valid,
     const uint16_t *sus10Value, bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
-    timeval timeAbsolute, double timeDelta,
-    const AcsParameters::SusHandlingParameters *susParameters,
+    timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
     const AcsParameters::SunModelParameters *sunModelParameters,
     acsctrl::SusDataProcessed *susDataProcessed) {
   /* -------- Sun Model Direction (IJK frame) ------- */
-  double JD2000 = TimeSystems::convertUnixToJD2000(timeAbsolute);
+  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
 
   // Julean Centuries
   double sunIjkModel[3] = {0.0, 0.0, 0.0};
@@ -198,7 +199,6 @@ void SensorProcessing::processSus(
   sunIjkModel[0] = cos(eclipticLongitude);
   sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
   sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
-  VectorOperations<double>::normalize(sunIjkModel, sunIjkModel, 3);
 
   uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
   if (sus0valid) {
@@ -354,10 +354,11 @@ void SensorProcessing::processSus(
 
   double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
   bool susVecTotDerivativeValid = false;
-  if (timeDelta > 0 and VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
+  double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
+  if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
+      VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
     VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
-    VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDelta, susVecTotDerivative,
-                                        3);
+    VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3);
     susVecTotDerivativeValid = true;
   }
   std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
@@ -366,6 +367,7 @@ void SensorProcessing::processSus(
     lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
                   susParameters->susRateFilterWeight);
   }
+  timeOfSavedSusDirEst = timeOfSusMeasurement;
   {
     PoolReadGuard pg(susDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -412,7 +414,7 @@ void SensorProcessing::processGyr(
     const double gyr2axXvalue, bool gyr2axXvalid, const double gyr2axYvalue, bool gyr2axYvalid,
     const double gyr2axZvalue, bool gyr2axZvalid, const double gyr3axXvalue, bool gyr3axXvalid,
     const double gyr3axYvalue, bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
-    const AcsParameters::GyrHandlingParameters *gyrParameters,
+    timeval timeOfGyrMeasurement, const AcsParameters::GyrHandlingParameters *gyrParameters,
     acsctrl::GyrDataProcessed *gyrDataProcessed) {
   bool gyr0valid = (gyr0axXvalid && gyr0axYvalid && gyr0axZvalid);
   bool gyr1valid = (gyr1axXvalid && gyr1axYvalid && gyr1axZvalid);
@@ -519,18 +521,18 @@ void SensorProcessing::processGyr(
 }
 
 void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
-                                  const double gpsAltitude, const double timeDelta,
+                                  const double gpsAltitude, const double gpsUnixSeconds,
                                   const bool validGps,
                                   const AcsParameters::GpsParameters *gpsParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed) {
   // init variables
   double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
          gpsVelocityE[3] = {0, 0, 0};
-  uint8_t gpsSource = acs::gps::Source::NONE;
+  uint8_t gpsSource = acs::GpsSource::NONE;
   // We do not trust the GPS and therefore it shall die here if SPG4 is running
-  if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
-    CoordinateTransformations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value,
-                                                       gdLatitude, gdLongitude, altitude);
+  if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) {
+    MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
+                                                    gdLongitude, altitude);
     double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
     gcLatitude = atan(factor * tan(gdLatitude));
     {
@@ -560,18 +562,22 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
 
     // Calculation of the satellite velocity in earth fixed frame
     double deltaDistance[3] = {0, 0, 0};
-    CoordinateTransformations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
-    if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
+    MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
+    if (validSavedPosSatE and
+        (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
+        (gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
       VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
-      VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);
+      double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
+      VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
     }
     savedPosSatE[0] = posSatE[0];
     savedPosSatE[1] = posSatE[1];
     savedPosSatE[2] = posSatE[2];
 
+    timeOfSavedPosSatE = gpsUnixSeconds;
     validSavedPosSatE = true;
 
-    gpsSource = acs::gps::Source::GPS;
+    gpsSource = acs::GpsSource::GPS;
   }
   {
     PoolReadGuard pg(gpsDataProcessed);
@@ -588,15 +594,13 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
   }
 }
 
-void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
-                               ACS::SensorValues *sensorValues,
+void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
                                acsctrl::MgmDataProcessed *mgmDataProcessed,
                                acsctrl::SusDataProcessed *susDataProcessed,
                                acsctrl::GyrDataProcessed *gyrDataProcessed,
                                acsctrl::GpsDataProcessed *gpsDataProcessed,
                                const AcsParameters *acsParameters) {
   sensorValues->update();
-
   processGps(
       sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
       sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
@@ -613,8 +617,7 @@ void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
              sensorValues->mgm3Rm3100Set.fieldStrengths.value,
              sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
              sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
-             timeAbsolute, timeDelta, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
-             mgmDataProcessed);
+             now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed);
 
   processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
              sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),
@@ -628,8 +631,8 @@ void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
              sensorValues->susSets[9].channels.value, sensorValues->susSets[9].channels.isValid(),
              sensorValues->susSets[10].channels.value, sensorValues->susSets[10].channels.isValid(),
              sensorValues->susSets[11].channels.value, sensorValues->susSets[11].channels.isValid(),
-             timeAbsolute, timeDelta, &acsParameters->susHandlingParameters,
-             &acsParameters->sunModelParameters, susDataProcessed);
+             now, &acsParameters->susHandlingParameters, &acsParameters->sunModelParameters,
+             susDataProcessed);
 
   processGyr(
       sensorValues->gyr0AdisSet.angVelocX.value, sensorValues->gyr0AdisSet.angVelocX.isValid(),
@@ -643,7 +646,7 @@ void SensorProcessing::process(timeval timeAbsolute, double timeDelta,
       sensorValues->gyr2AdisSet.angVelocZ.value, sensorValues->gyr2AdisSet.angVelocZ.isValid(),
       sensorValues->gyr3L3gSet.angVelocX.value, sensorValues->gyr3L3gSet.angVelocX.isValid(),
       sensorValues->gyr3L3gSet.angVelocY.value, sensorValues->gyr3L3gSet.angVelocY.isValid(),
-      sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(),
+      sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now,
       &acsParameters->gyrHandlingParameters, gyrDataProcessed);
 }
 

mission/controller/acs/SensorProcessing.h

@@ -2,9 +2,7 @@
 #define SENSORPROCESSING_H_
 
 #include <common/config/eive/resultClassIds.h>
-#include <fsfw/coordinates/CoordinateTransformations.h>
 #include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/TimeSystems.h>
 #include <fsfw/globalfunctions/constants.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
@@ -16,6 +14,7 @@
 #include <mission/controller/acs/Igrf13Model.h>
 #include <mission/controller/acs/SensorValues.h>
 #include <mission/controller/acs/SusConverter.h>
+#include <mission/controller/acs/util/MathOperations.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 #include <cmath>
@@ -25,21 +24,22 @@ class SensorProcessing {
   SensorProcessing();
   virtual ~SensorProcessing();
 
-  void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues,
+  void process(timeval now, ACS::SensorValues *sensorValues,
                acsctrl::MgmDataProcessed *mgmDataProcessed,
                acsctrl::SusDataProcessed *susDataProcessed,
                acsctrl::GyrDataProcessed *gyrDataProcessed,
-               acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters);
-
+               acsctrl::GpsDataProcessed *gpsDataProcessed,
+               const AcsParameters *acsParameters);  // Will call protected functions
  private:
   static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
   static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
   static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
 
  protected:
+  // short description needed for every function
   void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
                   const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
-                  const float *mgm4Value, bool mgm4valid, timeval timeAbsolute, double timeDelta,
+                  const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
                   const AcsParameters::MgmHandlingParameters *mgmParameters,
                   acsctrl::GpsDataProcessed *gpsDataProcessed,
                   acsctrl::MgmDataProcessed *mgmDataProcessed);
@@ -52,7 +52,7 @@ class SensorProcessing {
                   bool sus7valid, const uint16_t *sus8Value, bool sus8valid,
                   const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value,
                   bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
-                  timeval timeAbsolute, double timeDelta,
+                  timeval timeOfSusMeasurement,
                   const AcsParameters::SusHandlingParameters *susParameters,
                   const AcsParameters::SunModelParameters *sunModelParameters,
                   acsctrl::SusDataProcessed *susDataProcessed);
@@ -65,6 +65,7 @@ class SensorProcessing {
                   bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid,
                   const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
                   bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
+                  timeval timeOfGyrMeasurement,
                   const AcsParameters::GyrHandlingParameters *gyrParameters,
                   acsctrl::GyrDataProcessed *gyrDataProcessed);
 
@@ -76,9 +77,13 @@ class SensorProcessing {
   void lowPassFilter(double *newValue, double *oldValue, const double weight);
 
   double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
+  timeval timeOfSavedMagFieldEst;
   double savedSusVecTot[3] = {0.0, 0.0, 0.0};
+  timeval timeOfSavedSusDirEst;
+  bool validMagField = false;
 
   double savedPosSatE[3] = {0.0, 0.0, 0.0};
+  double timeOfSavedPosSatE = 0.0;
   bool validSavedPosSatE = false;
 
   SusConverter susConverter;

mission/controller/acs/control/Detumble.cpp

@@ -7,18 +7,18 @@ Detumble::Detumble() {}
 
 Detumble::~Detumble() {}
 
-acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
-                                                    const bool satRotRateValid,
-                                                    const bool magFieldRateValid,
-                                                    const bool useFullDetumbleLaw) {
+acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
+                                                 const bool satRotRateValid,
+                                                 const bool magFieldRateValid,
+                                                 const bool useFullDetumbleLaw) {
   if (not magFieldValid) {
-    return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
+    return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (satRotRateValid and useFullDetumbleLaw) {
-    return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
+    return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
   } else if (magFieldRateValid) {
-    return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
+    return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
   } else {
-    return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
+    return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
   }
 }
 

mission/controller/acs/control/Detumble.h

@@ -11,9 +11,9 @@ class Detumble {
   Detumble();
   virtual ~Detumble();
 
-  acs::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
-                                            const bool magFieldRateValid,
-                                            const bool useFullDetumbleLaw);
+  acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
+                                         const bool magFieldRateValid,
+                                         const bool useFullDetumbleLaw);
 
   void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
                    double gain);

mission/controller/acs/control/PtgCtrl.cpp

@@ -1,24 +1,15 @@
 #include "PtgCtrl.h"
 
+#include <fsfw/globalfunctions/constants.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/globalfunctions/sign.h>
+
 PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
 PtgCtrl::~PtgCtrl() {}
 
-acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
-    const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid,
-    const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) {
-  if (not magFieldValid) {
-    return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
-  } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
-    return acs::ControlModeStrategy::PTGCTRL_STR;
-  } else if (mekfEnabled and mekfValid) {
-    return acs::ControlModeStrategy::PTGCTRL_MEKF;
-  } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
-    return acs::ControlModeStrategy::PTGCTRL_QUEST;
-  }
-  return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
-}
-
 void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
                      const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
                      double *torqueRws) {
@@ -33,7 +24,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
 
   double cInt = 2 * om * zeta;
-  double kInt = 2 * om * om;
+  double kInt = 2 * pow(om, 2);
 
   double qErrorLaw[3] = {0, 0, 0};
 
@@ -62,9 +53,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
 
   // Inverse of gainMatrix
   double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  gainMatrixInverse[0][0] = 1. / gainMatrix[0][0];
-  gainMatrixInverse[1][1] = 1. / gainMatrix[1][1];
-  gainMatrixInverse[2][2] = 1. / gainMatrix[2][2];
+  gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
+  gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
+  gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];
 
   double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MatrixOperations<double>::multiply(
@@ -105,13 +96,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
 }
 
-void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
-                           AcsParameters::PointingLawParameters *pointingLawParameters,
+void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
                            const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                            const int32_t speedRw3, double *rwTrqNs) {
-  if (not allRwAvabilable) {
-    return;
-  }
   // concentrate RW speeds as vector and convert to double
   double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
                         static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
@@ -140,10 +127,9 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
                                       4);
 }
 
-void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail *rwAvail,
-                              AcsParameters::PointingLawParameters *pointingLawParameters,
+void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
                               const double *magFieldB, const bool magFieldBValid,
-                              const int32_t speedRw0, const int32_t speedRw1,
+                              const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
                               const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
   if (not magFieldBValid or not pointingLawParameters->desatOn) {
     return;
@@ -157,24 +143,17 @@ void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail
   double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
+  // calculate angular momentum of the satellite
+  double angMomentumSat[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
+                                     angMomentumSat, 3, 3, 1);
+
   // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
   // relocate RW speed zero to nullspace RW speed
   double refSpeedRws[4] = {0, 0, 0, 0};
   VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
                                       pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
-  if (not allRwAvailable) {
-    if (not rwAvail->rw1avail) {
-      refSpeedRws[0] = 0.0;
-    } else if (not rwAvail->rw2avail) {
-      refSpeedRws[1] = 0.0;
-    } else if (not rwAvail->rw3avail) {
-      refSpeedRws[2] = 0.0;
-    } else if (not rwAvail->rw4avail) {
-      refSpeedRws[3] = 0.0;
-    }
-  }
   VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
-
   // convert speed from 10 RPM to 1 RPM
   VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
   // convert to rad/s
@@ -190,12 +169,16 @@ void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail
 
   // calculate total angular momentum
   double angMomentumTotal[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef,
-                                     angMomentumTotal, 3);
+  VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
+
+  // calculating momentum error
+  double deltaAngMomentum[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
+                                     deltaAngMomentum, 3);
 
   // resulting magnetic dipole command
   double crossAngMomentumMagField[3] = {0, 0, 0};
-  VectorOperations<double>::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField);
+  VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
   double factor =
       pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
   VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
@@ -219,13 +202,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
             rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
           } else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
             rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
-          } else {
-            rwCmdSpeeds[i] = 0;
           }
         }
       }
-    } else {
-      rwCmdSpeeds[i] = 0;
     }
   }
 }

mission/controller/acs/control/PtgCtrl.h

@@ -1,21 +1,15 @@
 #ifndef PTGCTRL_H_
 #define PTGCTRL_H_
 
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <fsfw/globalfunctions/sign.h>
-#include <mission/acs/defs.h>
+#include <math.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/SensorValues.h>
-#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
-
-#include <cmath>
+#include <stdio.h>
 
 class PtgCtrl {
   /*
    * @brief: This class handles the pointing control mechanism. Calculation of an commanded
-   * torque for the reaction wheels, and magnetic Field strength for magnetorquer for desaturation
+   * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
    *
    * @note: A description of the used algorithms can be found in
    * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
@@ -27,30 +21,22 @@ class PtgCtrl {
   PtgCtrl(AcsParameters *acsParameters_);
   virtual ~PtgCtrl();
 
-  acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
-                                                const bool strValid, const bool questValid,
-                                                const bool fusedRateValid,
-                                                const uint8_t rotRateSource,
-                                                const uint8_t mekfEnabled);
-
   /* @brief: Calculates the needed torque for the pointing control mechanism
    */
   void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
               const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
 
-  void ptgNullspace(const bool allRwAvabilable,
-                    AcsParameters::PointingLawParameters *pointingLawParameters,
+  void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
                     const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                     const int32_t speedRw3, double *rwTrqNs);
 
-  void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail,
-                       AcsParameters::PointingLawParameters *pointingLawParameters,
-                       const double *magFieldB, const bool magFieldBValid, const int32_t speedRw0,
-                       const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3,
-                       double *mgtDpDes);
+  void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
+                       const double *magFieldB, const bool magFieldBValid, const double *satRate,
+                       const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+                       const int32_t speedRw3, double *mgtDpDes);
 
   /* @brief: Commands the stiction torque in case wheel speed is to low
-   * 		 torqueCommand  modified torque after anti-stiction
+   * 		 torqueCommand  modified torque after antistiction
    */
   void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);
 

mission/controller/acs/control/SafeCtrl.cpp

@@ -9,38 +9,40 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
 
 SafeCtrl::~SafeCtrl() {}
 
-acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(
-    const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
-    const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled,
-    const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
+acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+                                                 const bool satRotRateValid, const bool sunDirValid,
+                                                 const bool fusedRateTotalValid,
+                                                 const uint8_t mekfEnabled,
+                                                 const uint8_t gyrEnabled,
+                                                 const uint8_t dampingEnabled) {
   if (not magFieldValid) {
-    return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
+    return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (mekfEnabled and mekfValid) {
-    return acs::ControlModeStrategy::SAFECTRL_MEKF;
+    return acs::SafeModeStrategy::SAFECTRL_MEKF;
   } else if (sunDirValid) {
     if (gyrEnabled and satRotRateValid) {
-      return acs::ControlModeStrategy::SAFECTRL_GYR;
+      return acs::SafeModeStrategy::SAFECTRL_GYR;
     } else if (not gyrEnabled and fusedRateTotalValid) {
-      return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
+      return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
     } else {
-      return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
+      return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
     }
   } else if (not sunDirValid) {
     if (dampingEnabled) {
       if (gyrEnabled and satRotRateValid) {
-        return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
+        return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
       } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
-        return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
+        return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
       } else {
-        return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
+        return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
       }
     } else if (not dampingEnabled and satRotRateValid) {
-      return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
+      return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
     } else {
-      return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
+      return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
     }
   } else {
-    return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
+    return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
   }
 }
 

mission/controller/acs/control/SafeCtrl.h

@@ -12,11 +12,10 @@ class SafeCtrl {
   SafeCtrl(AcsParameters *acsParameters_);
   virtual ~SafeCtrl();
 
-  acs::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
-                                            const bool satRotRateValid, const bool sunDirValid,
-                                            const bool fusedRateTotalValid,
-                                            const uint8_t mekfEnabled, const uint8_t gyrEnabled,
-                                            const uint8_t dampingEnabled);
+  acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+                                         const bool satRotRateValid, const bool sunDirValid,
+                                         const bool fusedRateTotalValid, const uint8_t mekfEnabled,
+                                         const uint8_t gyrEnabled, const uint8_t dampingEnabled);
 
   void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
                 const double *quatBI, const double *sunDirRefB, double *magMomB,

mission/controller/acs/util/CholeskyDecomposition.h

@@ -0,0 +1,98 @@
+/*
+ * TinyEKF: Extended Kalman Filter for embedded processors
+ *
+ * Copyright (C) 2015 Simon D. Levy
+ *
+ * MIT License
+ */
+#ifndef CHOLESKYDECOMPOSITION_H_
+#define CHOLESKYDECOMPOSITION_H_
+#include <math.h>
+// typedef unsigned int uint8_t;
+
+template <typename T1, typename T2 = T1, typename T3 = T2>
+class CholeskyDecomposition {
+ public:
+  static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
+    // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
+    return cholsl(matrix, result, tempMatrix, dimension);
+  }
+
+ private:
+  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
+  static uint8_t choldc1(double *a, double *p, uint8_t n) {
+    int8_t i, j, k;
+    double sum;
+
+    for (i = 0; i < n; i++) {
+      for (j = i; j < n; j++) {
+        sum = a[i * n + j];
+        for (k = i - 1; k >= 0; k--) {
+          sum -= a[i * n + k] * a[j * n + k];
+        }
+        if (i == j) {
+          if (sum <= 0) {
+            return 1; /* error */
+          }
+          p[i] = sqrt(sum);
+        } else {
+          a[j * n + i] = sum / p[i];
+        }
+      }
+    }
+
+    return 0; /* success */
+  }
+
+  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
+  static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
+    uint8_t i, j, k;
+    double sum;
+    for (i = 0; i < n; i++)
+      for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
+    if (choldc1(a, p, n)) return 1;
+    for (i = 0; i < n; i++) {
+      a[i * n + i] = 1 / p[i];
+      for (j = i + 1; j < n; j++) {
+        sum = 0;
+        for (k = i; k < j; k++) {
+          sum -= a[j * n + k] * a[k * n + i];
+        }
+        a[j * n + i] = sum / p[j];
+      }
+    }
+
+    return 0; /* success */
+  }
+
+  // https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
+  static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
+    uint8_t i, j, k;
+    if (choldcsl(A, a, p, n)) return 1;
+    for (i = 0; i < n; i++) {
+      for (j = i + 1; j < n; j++) {
+        a[i * n + j] = 0.0;
+      }
+    }
+    for (i = 0; i < n; i++) {
+      a[i * n + i] *= a[i * n + i];
+      for (k = i + 1; k < n; k++) {
+        a[i * n + i] += a[k * n + i] * a[k * n + i];
+      }
+      for (j = i + 1; j < n; j++) {
+        for (k = j; k < n; k++) {
+          a[i * n + j] += a[k * n + i] * a[k * n + j];
+        }
+      }
+    }
+    for (i = 0; i < n; i++) {
+      for (j = 0; j < i; j++) {
+        a[i * n + j] = a[j * n + i];
+      }
+    }
+
+    return 0; /* success */
+  }
+};
+
+#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */

mission/controller/acs/util/MathOperations.h

@@ -0,0 +1,468 @@
+#ifndef MATH_MATHOPERATIONS_H_
+#define MATH_MATHOPERATIONS_H_
+
+#include <fsfw/src/fsfw/globalfunctions/constants.h>
+#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/src/fsfw/globalfunctions/sign.h>
+#include <stdint.h>
+#include <string.h>
+#include <sys/time.h>
+
+#include <cmath>
+#include <iostream>
+
+#include "fsfw/serviceinterface.h"
+
+template <typename T1, typename T2 = T1>
+class MathOperations {
+ public:
+  static void skewMatrix(const T1 vector[], T2 *result) {
+    // Input Dimension [3], Output [3][3]
+    result[0] = 0;
+    result[1] = -vector[2];
+    result[2] = vector[1];
+    result[3] = vector[2];
+    result[4] = 0;
+    result[5] = -vector[0];
+    result[6] = -vector[1];
+    result[7] = vector[0];
+    result[8] = 0;
+  }
+  static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
+                                    uint8_t size = 3) {
+    // Looks like MatrixOpertions::multiply is able to do the same thing
+    for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
+      for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
+        result[resultColumn + size * resultRow] =
+            vector1[resultRow] * transposeVector2[resultColumn];
+      }
+    }
+    /*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
+     matrixMag[i][j] = magEstB[i] * magEstB[j];
+     matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
+     matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
+  }
+
+  static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
+    int min_idx;
+    T1 temp;
+    memcpy(result, matrix, rowSize * colSize * sizeof(*result));
+    // One by one move boundary of unsorted subarray
+    for (int k = 0; k < rowSize; k++) {
+      for (int i = 0; i < colSize - 1; i++) {
+        // Find the minimum element in unsorted array
+        min_idx = i;
+        for (int j = i + 1; j < colSize; j++) {
+          if (result[j + k * colSize] < result[min_idx + k * colSize]) {
+            min_idx = j;
+          }
+        }
+        // Swap the found minimum element with the first element
+        temp = result[i + k * colSize];
+        result[i + k * colSize] = result[min_idx + k * colSize];
+        result[min_idx + k * colSize] = temp;
+      }
+    }
+  }
+
+  static void convertDateToJD2000(const T1 time, T2 julianDate) {
+    // time = { Y, M, D, h, m,s}
+    // time in sec and microsec -> The Epoch (unixtime)
+    julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
+                 floor(275 * time[1] / 9) + time[2] +
+                 (60 * time[3] + time[4] + (time(5) / 60)) / 1440;
+  }
+
+  static T1 convertUnixToJD2000(timeval time) {
+    // time = {{s},{us}}
+    T1 julianDate2000;
+    julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
+    return julianDate2000;
+  }
+
+  static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
+    // convention q = [qx,qy,qz, qw]
+    outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
+    outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
+    outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);
+
+    outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
+    outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
+    outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
+
+    outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
+    outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
+    outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
+  }
+
+  static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
+                                      T2 *cartesianOutput) {
+    /* @brief:  cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
+     * 								longitude and altitude
+     * @param:  lat   				geodetic latitude [rad]
+     * 			longi 				longitude [rad]
+     * 			alt					altitude [m]
+     * 			cartesianOutput 	Cartesian Coordinates in ECEF (3x1)
+     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
+     * 			Landis Markley and John L. Crassidis*/
+    double radiusPolar = 6356752.314;
+    double radiusEqua = 6378137;
+
+    double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
+    double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
+
+    cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
+    cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
+    cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
+  }
+
+  static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
+    /* @brief:  latLongAltFromCartesian() - calculates latitude, longitude and altitude from
+     * cartesian coordinates in ECEF
+     * @param:  x   			x-value of position vector [m]
+     * 			y 				y-value of position vector [m]
+     * 			z				z-value of position vector [m]
+     * 			latitude 		geodetic latitude [rad]
+     * 			longitude		longitude [rad]
+     * 			altitude		altitude [m]
+     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
+     * 			Landis Markley and John L. Crassidis*/
+    // From World Geodetic System the Earth Radii
+    double a = 6378137.0;     // semimajor axis [m]
+    double b = 6356752.3142;  // semiminor axis [m]
+
+    // Calculation
+    double e2 = 1 - pow(b, 2) / pow(a, 2);
+    double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
+    double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
+    double p = std::abs(vector[2]) / epsilon2;
+    double s = pow(rho, 2) / (e2 * epsilon2);
+    double q = pow(p, 2) - pow(b, 2) + s;
+    double u = p / sqrt(q);
+    double v = pow(b, 2) * pow(u, 2) / q;
+    double P = 27 * v * s / q;
+    double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
+    double t = (1 + Q + 1 / Q) / 6;
+    double c = sqrt(pow(u, 2) - 1 + 2 * t);
+    double w = (c - u) / 2;
+    double d =
+        sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
+    double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
+    latitude = asin((epsilon2 + 1) * d / N);
+    altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
+    longitude = atan2(vector[1], vector[0]);
+  }
+
+  static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
+    /* @brief:  dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
+     * @param:  time   			Current time
+     * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
+     * 			outputDotDcmEJ	Derivative of transformation matrix [3][3]
+     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
+     * 			Landis Markley and John L. Crassidis*/
+    double JD2000Floor = 0;
+    double JD2000 = convertUnixToJD2000(time);
+    // Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
+    JD2000Floor = floor(JD2000);
+    if ((JD2000 - JD2000Floor) < 0.5) {
+      JD2000Floor -= 0.5;
+    } else {
+      JD2000Floor += 0.5;
+    }
+
+    double JC2000 = JD2000Floor / 36525;
+    double sec = (JD2000 - JD2000Floor) * 86400;
+    double gmst = 0;  // greenwich mean sidereal time
+    gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
+           0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
+    double rest = gmst / 86400;
+    double FloorRest = floor(rest);
+    double secOfDay = rest - FloorRest;
+    secOfDay *= 86400;
+    gmst = secOfDay / 240 * M_PI / 180;
+
+    outputDcmEJ[0] = cos(gmst);
+    outputDcmEJ[1] = sin(gmst);
+    outputDcmEJ[2] = 0;
+    outputDcmEJ[3] = -sin(gmst);
+    outputDcmEJ[4] = cos(gmst);
+    outputDcmEJ[5] = 0;
+    outputDcmEJ[6] = 0;
+    outputDcmEJ[7] = 0;
+    outputDcmEJ[8] = 1;
+
+    //	Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
+    double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
+                              {outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
+                              {outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
+    double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
+    double omegaEarth = 0.000072921158553;
+    double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+    MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
+    MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
+  }
+
+  /* @brief:  ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
+   * 			give also the back the derivative of this matrix
+   * @param:  unixTime   		Current time in Unix format
+   * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
+   * 			outputDotDcmEJ	Derivative of transformation matrix [3][3]
+   * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
+                          Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
+   * 			Oliver Zeile
+   *
+   https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
+  static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
+    //		TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
+    //(initial Offset) 		International Atomic Time (TAI)
+
+    double JD2000UTC1 = convertUnixToJD2000(unixTime);
+
+    //		Julian Date / century from TT
+    timeval terestrialTime = unixTime;
+    terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
+    double JD2000TT = convertUnixToJD2000(terestrialTime);
+    double JC2000TT = JD2000TT / 36525;
+
+    //-------------------------------------------------------------------------------------
+    //		Calculation of Transformation from earth rotation Theta
+    //-------------------------------------------------------------------------------------
+    double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+    //		Earth Rotation angle
+    double era = 0;
+    era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
+    //		Greenwich Mean Sidereal Time
+    double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
+                      0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
+    double arcsecFactor = 1 * M_PI / (180 * 3600);
+    gmst2000 *= arcsecFactor;
+    gmst2000 += era;
+
+    theta[0][0] = cos(gmst2000);
+    theta[0][1] = sin(gmst2000);
+    theta[0][2] = 0;
+    theta[1][0] = -sin(gmst2000);
+    theta[1][1] = cos(gmst2000);
+    theta[1][2] = 0;
+    theta[2][0] = 0;
+    theta[2][1] = 0;
+    theta[2][2] = 1;
+
+    //-------------------------------------------------------------------------------------
+    //		Calculation of Transformation from earth Precession P
+    //-------------------------------------------------------------------------------------
+    double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+
+    double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
+    double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
+    double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);
+
+    zeta *= arcsecFactor;
+    theta2 *= arcsecFactor;
+    ze *= arcsecFactor;
+
+    precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
+    precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
+    precession[2][0] = sin(theta2) * cos(zeta);
+    precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta);
+    precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta);
+    precession[2][1] = -sin(theta2) * sin(zeta);
+    precession[0][2] = -cos(ze) * sin(theta2);
+    precession[1][2] = -sin(ze) * sin(theta2);
+    precession[2][2] = cos(theta2);
+
+    //-------------------------------------------------------------------------------------
+    //		Calculation of Transformation from earth Nutation N
+    //-------------------------------------------------------------------------------------
+    double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+    //      lunar asc node
+    double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
+                7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
+    Om *= arcsecFactor;
+    //      delta psi approx
+    double dp = -17.2 * arcsecFactor * sin(Om);
+
+    //      delta eps approx
+    double de = 9.203 * arcsecFactor * cos(Om);
+
+    //        % true obliquity of the ecliptic eps p.71 (simplified)
+    double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;
+
+    nutation[0][0] = cos(dp);
+    nutation[1][0] = cos(e + de) * sin(dp);
+    nutation[2][0] = sin(e + de) * sin(dp);
+    nutation[0][1] = -cos(e) * sin(dp);
+    nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
+    nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
+    nutation[0][2] = -sin(e) * sin(dp);
+    nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
+    nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);
+
+    //-------------------------------------------------------------------------------------
+    //		Calculation of Derivative of rotation matrix from earth
+    //-------------------------------------------------------------------------------------
+    double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+    double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
+    double omegaEarth = 0.000072921158553;
+    MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
+    MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
+
+    //-------------------------------------------------------------------------------------
+    //		Calculation of transformation matrix and Derivative of transformation matrix
+    //-------------------------------------------------------------------------------------
+    double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+    MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
+    MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
+
+    MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
+  }
+  static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
+    int i, j;
+    double determinant = 0;
+    double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
+                        {matrix[3], matrix[4], matrix[5]},
+                        {matrix[6], matrix[7], matrix[8]}};
+
+    for (i = 0; i < 3; i++) {
+      determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
+                                                mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
+    }
+    //		cout<<"\n\ndeterminant: "<<determinant;
+    //		cout<<"\n\nInverse of matrix is: \n";
+    for (i = 0; i < 3; i++) {
+      for (j = 0; j < 3; j++) {
+        output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
+                             (mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
+                            determinant;
+      }
+    }
+  }
+
+  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++) {
+      for (uint8_t col = 0; col < size; col++) {
+        matrix[row][col] = inputMatrix[row * size + col];
+      }
+    }
+    if (size == 2)
+      return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
+    else {
+      for (uint8_t col = 0; col < size; col++) {
+        int subRow = 0;
+        for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
+          int subCol = 0;
+          for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
+            if (colIndex == col) continue;
+            submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
+            subCol++;
+          }
+          subRow++;
+        }
+        det += (pow(-1, col) * matrix[0][col] *
+                MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
+      }
+    }
+    return det;
+  }
+
+  static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
+    // Stopwatch stopwatch;
+    T1 matrix[size][size], identity[size][size];
+    // reformat array to matrix
+    for (uint8_t row = 0; row < size; row++) {
+      for (uint8_t col = 0; col < size; col++) {
+        matrix[row][col] = inputMatrix[row * size + col];
+      }
+    }
+    // init identity matrix
+    std::memset(identity, 0.0, sizeof(identity));
+    for (uint8_t diag = 0; diag < size; diag++) {
+      identity[diag][diag] = 1;
+    }
+    // gauss-jordan algo
+    // sort matrix such as no diag entry shall be 0
+    for (uint8_t row = 0; row < size; row++) {
+      if (matrix[row][row] == 0.0) {
+        bool swaped = false;
+        uint8_t rowIndex = 0;
+        while ((rowIndex < size) && !swaped) {
+          if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
+            for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
+              std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
+              std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
+            }
+            swaped = true;
+          }
+          rowIndex++;
+        }
+        if (!swaped) {
+          return 1;  // matrix not invertible
+        }
+      }
+    }
+
+    for (int row = 0; row < size; row++) {
+      if (matrix[row][row] == 0.0) {
+        uint8_t rowIndex;
+        if (row == 0) {
+          rowIndex = size - 1;
+        } else {
+          rowIndex = row - 1;
+        }
+        for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
+          std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
+          std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
+        }
+        row--;
+        if (row < 0) {
+          return 1;  // Matrix is not invertible
+        }
+      }
+    }
+    // remove non diag elements in matrix (jordan)
+    for (int row = 0; row < size; row++) {
+      for (int rowIndex = 0; rowIndex < size; rowIndex++) {
+        if (row != rowIndex) {
+          double ratio = matrix[rowIndex][row] / matrix[row][row];
+          for (int colIndex = 0; colIndex < size; colIndex++) {
+            matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
+            identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
+          }
+        }
+      }
+    }
+    // normalize rows in matrix (gauss)
+    for (int row = 0; row < size; row++) {
+      for (int col = 0; col < size; col++) {
+        identity[row][col] = identity[row][col] / matrix[row][row];
+      }
+    }
+    std::memcpy(inverse, identity, sizeof(identity));
+    return 0;  // successful inversion
+  }
+
+  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
+    for (uint8_t i = 0; i < size; i++) {
+      if (not isfinite(inputVector[i])) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
+    for (uint8_t col = 0; col < cols; col++) {
+      for (uint8_t row = 0; row < rows; row++) {
+        if (not isfinite(inputMatrix[row * cols + cols])) {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
+};
+
+#endif /* ACS_MATH_MATHOPERATIONS_H_ */

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -1,7 +1,6 @@
 #ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
 #define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_
 
-#include <common/config/eive/resultClassIds.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
 #include <fsfw/datapoollocal/localPoolDefinitions.h>
 
@@ -9,25 +8,6 @@
 
 namespace acsctrl {
 
-static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
-//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
-static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0xA0);
-//! [EXPORT] : [COMMENT] Writing the TLE to the file has failed.
-static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(0xA1);
-//! [EXPORT] : [COMMENT] Reading the TLE to the file has failed.
-static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(0xA2);
-//! [EXPORT] : [COMMENT] A single RW has failed.
-static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
-//! [EXPORT] : [COMMENT] Multiple RWs have failed.
-static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4);
-
-struct RwAvail {
-  bool rw1avail = false;
-  bool rw2avail = false;
-  bool rw3avail = false;
-  bool rw4avail = false;
-};
-
 enum SetIds : uint32_t {
   MGM_SENSOR_DATA,
   MGM_PROCESSED_DATA,
@@ -40,7 +20,6 @@ enum SetIds : uint32_t {
   CTRL_VAL_DATA,
   ACTUATOR_CMD_DATA,
   FUSED_ROTATION_RATE_DATA,
-  FUSED_ROTATION_RATE_SOURCES_DATA,
   TLE_SET,
 };
 
@@ -117,7 +96,6 @@ enum PoolIds : lp_id_t {
   SAT_ROT_RATE_MEKF,
   QUAT_MEKF,
   MEKF_STATUS,
-  QUAT_QUEST,
   // Ctrl Values
   SAFE_STRAT,
   TGT_QUAT,
@@ -132,13 +110,9 @@ enum PoolIds : lp_id_t {
   ROT_RATE_ORTHOGONAL,
   ROT_RATE_PARALLEL,
   ROT_RATE_TOTAL,
-  ROT_RATE_SOURCE,
-  // Fused Rotation Rate Sources
-  ROT_RATE_ORTHOGONAL_SUSMGM,
-  ROT_RATE_PARALLEL_SUSMGM,
-  ROT_RATE_TOTAL_SUSMGM,
-  ROT_RATE_TOTAL_QUEST,
-  ROT_RATE_TOTAL_STR,
+  // TLE
+  TLE_LINE_1,
+  TLE_LINE_2,
 };
 
 static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@@ -148,11 +122,11 @@ 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 = 6;
-static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4;
+static constexpr uint8_t MEKF_SET_ENTRIES = 3;
 static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
-static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 4;
-static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
+static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
+static constexpr uint8_t TLE_SET_ENTRIES = 2;
 
 /**
  * @brief   Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@@ -276,14 +250,13 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
  private:
 };
 
-class AttitudeEstimationData : public StaticLocalDataSet<ATTITUDE_ESTIMATION_SET_ENTRIES> {
+class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
  public:
-  AttitudeEstimationData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
+  MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
 
   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);
-  lp_vec_t<double, 4> quatQuest = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
 
  private:
 };
@@ -322,25 +295,16 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
       lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
   lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
   lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
-  lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
 
  private:
 };
 
-class FusedRotRateSourcesData : public StaticLocalDataSet<FUSED_ROT_RATE_SOURCES_SET_ENTRIES> {
+class TleData : public StaticLocalDataSet<TLE_SET_ENTRIES> {
  public:
-  FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner)
-      : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {}
+  TleData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, TLE_SET) {}
 
-  lp_vec_t<double, 3> rotRateOrthogonalSusMgm =
-      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL_SUSMGM, this);
-  lp_vec_t<double, 3> rotRateParallelSusMgm =
-      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL_SUSMGM, this);
-  lp_vec_t<double, 3> rotRateTotalSusMgm =
-      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this);
-  lp_vec_t<double, 3> rotRateTotalQuest =
-      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_QUEST, this);
-  lp_vec_t<double, 3> rotRateTotalStr = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_STR, this);
+  lp_vec_t<uint8_t, 69> line1 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
+  lp_vec_t<uint8_t, 69> line2 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
 
  private:
 };

mission/genericFactory.cpp

@@ -250,30 +250,20 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
                            pus::PUS_SERVICE_2, 3, 10);
   new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID,
                            pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH, 16);
-
-  auto psbParamsService5 =
-      PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5);
-  psbParamsService5.requestQueueDepth = 50;
-  psbParamsService5.maxPacketsPerCycle = 50;
-  new Service5EventReporting(psbParamsService5, 80, 160);
+  new Service5EventReporting(
+      PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5),
+      80, 160);
   new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID,
                                  pus::PUS_SERVICE_8, config::ACTION_SERVICE_QUEUE_DEPTH, 16, 60);
   new Service9TimeManagement(
       PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9));
 
-  auto psbParamsService11 =
-      PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11);
-  psbParamsService11.requestQueueDepth = 100;
-  psbParamsService11.maxPacketsPerCycle = 100;
-  new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(psbParamsService11,
-                                                                     ccsdsDistrib);
-
+  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);
-  auto psbParamsService17 =
-      PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17);
-  psbParamsService17.requestQueueDepth = 50;
-  psbParamsService17.maxPacketsPerCycle = 50;
-  new Service17Test(psbParamsService17);
+  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,
                                    pus::PUS_SERVICE_20);
   new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT, config::EIVE_PUS_APID,

mission/payload/PayloadPcduHandler.cpp

@@ -4,7 +4,6 @@
 
 #include "OBSWConfig.h"
 #include "fsfw/thermal/tcsDefinitions.h"
-#include "mission/payload/payloadPcduDefinitions.h"
 
 #ifdef XIPHOS_Q7S
 #include <fsfw_hal/linux/UnixFileGuard.h>
@@ -429,20 +428,20 @@ void PayloadPcduHandler::checkAdcValues() {
     params.getValue(PARAM_KEY_MAP[NEG_V_UPPER_BOUND], upperBound);
     if (not checkVoltage(adcSet.processed[U_NEG_V_FB], lowerBound, upperBound,
                          NEG_V_OUT_OF_BOUNDS)) {
-      sif::warning << "Negative voltage was out of bounds, went back to OFF" << std::endl;
       return;
     }
     params.getValue(PARAM_KEY_MAP[DRO_U_LOWER_BOUND], lowerBound);
     params.getValue(PARAM_KEY_MAP[DRO_U_UPPER_BOUND], upperBound);
     if (not checkVoltage(adcSet.processed[U_DRO_DIV_6], lowerBound, upperBound,
                          U_DRO_OUT_OF_BOUNDS)) {
-      sif::warning << "DRO voltage was out of bounds, went back to OFF" << std::endl;
       return;
     }
     params.getValue(PARAM_KEY_MAP[DRO_I_UPPER_BOUND], upperBound);
     if (not checkCurrent(adcSet.processed[I_DRO], upperBound, I_DRO_OUT_OF_BOUNDS)) {
+#if OBSW_VERBOSE_LEVEL >= 1
       sif::warning << "Detected out of bounds current for DRO: " << adcSet.processed[I_DRO]
                    << ", Raw: " << adcSet.channels[I_DRO] << std::endl;
+#endif
       return;
     }
   }
@@ -456,12 +455,10 @@ void PayloadPcduHandler::checkAdcValues() {
     params.getValue(PARAM_KEY_MAP[X8_U_UPPER_BOUND], upperBound);
     if (not checkVoltage(adcSet.processed[U_X8_DIV_6], lowerBound, upperBound,
                          U_X8_OUT_OF_BOUNDS)) {
-      sif::warning << "X8 voltage was out of bounds, went back to OFF" << std::endl;
       return;
     }
     params.getValue(PARAM_KEY_MAP[X8_I_UPPER_BOUND], upperBound);
     if (not checkCurrent(adcSet.processed[I_X8], upperBound, I_X8_OUT_OF_BOUNDS)) {
-      sif::warning << "X8 current was out of bounds, went back to OFF" << std::endl;
       return;
     }
   }
@@ -475,12 +472,10 @@ void PayloadPcduHandler::checkAdcValues() {
     params.getValue(PARAM_KEY_MAP[TX_U_UPPER_BOUND], upperBound);
     if (not checkVoltage(adcSet.processed[U_TX_DIV_6], lowerBound, upperBound,
                          U_TX_OUT_OF_BOUNDS)) {
-      sif::warning << "TX voltage was out of bounds, went back to OFF" << std::endl;
       return;
     }
     params.getValue(PARAM_KEY_MAP[TX_I_UPPER_BOUND], upperBound);
     if (not checkCurrent(adcSet.processed[I_TX], upperBound, I_TX_OUT_OF_BOUNDS)) {
-      sif::warning << "TX current was out of bounds, went back to OFF" << std::endl;
       return;
     }
   }
@@ -494,12 +489,10 @@ void PayloadPcduHandler::checkAdcValues() {
     params.getValue(PARAM_KEY_MAP[MPA_U_UPPER_BOUND], upperBound);
     if (not checkVoltage(adcSet.processed[U_MPA_DIV_6], lowerBound, upperBound,
                          U_MPA_OUT_OF_BOUNDS)) {
-      sif::warning << "MPA voltage was out of bounds, went back to OFF" << std::endl;
       return;
     }
     params.getValue(PARAM_KEY_MAP[MPA_I_UPPER_BOUND], upperBound);
     if (not checkCurrent(adcSet.processed[I_MPA], upperBound, I_MPA_OUT_OF_BOUNDS)) {
-      sif::warning << "MPA current was out of bounds, went back to OFF" << std::endl;
       return;
     }
   }
@@ -513,7 +506,6 @@ void PayloadPcduHandler::checkAdcValues() {
     params.getValue(PARAM_KEY_MAP[HPA_U_UPPER_BOUND], upperBound);
     if (not checkVoltage(adcSet.processed[U_HPA_DIV_6], lowerBound, upperBound,
                          U_HPA_OUT_OF_BOUNDS)) {
-      sif::warning << "HPA voltage was out of bounds, went back to OFF" << std::endl;
       return;
     }
     params.getValue(PARAM_KEY_MAP[HPA_I_UPPER_BOUND], upperBound);
@@ -584,7 +576,6 @@ void PayloadPcduHandler::performOperationHook() { checkJsonFileInit(); }
 
 ReturnValue_t PayloadPcduHandler::checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
                                                    uint32_t* msToReachTheMode) {
-  using namespace plpcdu;
   if (commandedMode != MODE_OFF) {
     PoolReadGuard pg(&enablePl);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -593,59 +584,45 @@ ReturnValue_t PayloadPcduHandler::checkModeCommand(Mode_t commandedMode, Submode
       }
     }
   }
-  if (commandedMode == MODE_NORMAL) {
+  return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
+}
+
+ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
+  using namespace plpcdu;
+  if (mode == MODE_NORMAL) {
     uint8_t dhbSubmode = getSubmode();
-    diffMask = commandedSubmode ^ dhbSubmode;
-    // For all higher level modes, SSR needs to be on. This is to ensure we have valid ADC
-    // measurements
-    if ((droOnForSubmode(commandedSubmode) or x8OnForSubmode(commandedSubmode) or
-         txOnForSubmode(commandedSubmode) or mpaOnForSubmode(commandedSubmode) or
-         hpaOnForSubmode(commandedSubmode)) and
-        not ssrOnForSubmode(dhbSubmode)) {
+    diffMask = submode ^ dhbSubmode;
+    // Also deals with the case where the mode is MODE_ON, submode should be 0 here
+    if ((((submode >> SOLID_STATE_RELAYS_ADC_ON) & 0b1) == SOLID_STATE_RELAYS_ADC_ON) and
+        (getMode() == MODE_NORMAL and dhbSubmode != ALL_OFF_SUBMODE)) {
       return TRANS_NOT_ALLOWED;
     }
-    if (disableChannelOrderCheck) {
-      return returnvalue::OK;
-    }
-    if (x8OnForSubmode(commandedSubmode) and not droOnForSubmode(dhbSubmode)) {
+    if (((((submode >> DRO_ON) & 1) == 1) and
+         ((dhbSubmode & 0b1) != (1 << SOLID_STATE_RELAYS_ADC_ON)))) {
       return TRANS_NOT_ALLOWED;
     }
-    if (txOnForSubmode(commandedSubmode) and
-        (not droOnForSubmode(dhbSubmode) or not x8OnForSubmode(dhbSubmode))) {
+    if ((((submode >> X8_ON) & 1) == 1) and
+        ((dhbSubmode & 0b11) != ((1 << SOLID_STATE_RELAYS_ADC_ON) | (1 << DRO_ON)))) {
       return TRANS_NOT_ALLOWED;
     }
-    if (mpaOnForSubmode(commandedSubmode) and
-        (not droOnForSubmode(dhbSubmode) or not x8OnForSubmode(dhbSubmode) or
-         not txOnForSubmode(dhbSubmode))) {
+    if (((((submode >> TX_ON) & 1) == 1) and
+         ((dhbSubmode & 0b111) !=
+          ((1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
       return TRANS_NOT_ALLOWED;
     }
-    if (hpaOnForSubmode(commandedSubmode) and
-        (not droOnForSubmode(dhbSubmode) or not x8OnForSubmode(dhbSubmode) or
-         not txOnForSubmode(dhbSubmode) or not mpaOnForSubmode(dhbSubmode))) {
+    if ((((submode >> MPA_ON) & 1) == 1 and
+         ((dhbSubmode & 0b1111) !=
+          ((1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) | (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
+      return TRANS_NOT_ALLOWED;
+    }
+    if ((((submode >> HPA_ON) & 1) == 1 and
+         ((dhbSubmode & 0b11111) != ((1 << MPA_ON) | (1 << TX_ON) | (1 << X8_ON) | (1 << DRO_ON) |
+                                     (1 << SOLID_STATE_RELAYS_ADC_ON))))) {
       return TRANS_NOT_ALLOWED;
     }
     return returnvalue::OK;
   }
-  return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
-}
-
-bool PayloadPcduHandler::ssrOnForSubmode(uint8_t submode) {
-  return submode & (1 << plpcdu::SOLID_STATE_RELAYS_ADC_ON);
-}
-bool PayloadPcduHandler::droOnForSubmode(uint8_t submode) {
-  return submode & (1 << plpcdu::DRO_ON);
-}
-
-bool PayloadPcduHandler::x8OnForSubmode(uint8_t submode) { return submode & (1 << plpcdu::X8_ON); }
-
-bool PayloadPcduHandler::txOnForSubmode(uint8_t submode) { return submode & (1 << plpcdu::TX_ON); }
-
-bool PayloadPcduHandler::mpaOnForSubmode(uint8_t submode) {
-  return submode & (1 << plpcdu::MPA_ON);
-}
-
-bool PayloadPcduHandler::hpaOnForSubmode(uint8_t submode) {
-  return submode & (1 << plpcdu::HPA_ON);
+  return DeviceHandlerBase::isModeCombinationValid(mode, submode);
 }
 
 ReturnValue_t PayloadPcduHandler::serializeFloat(uint32_t& param, float val) {
@@ -660,68 +637,56 @@ ReturnValue_t PayloadPcduHandler::getParameter(uint8_t domainId, uint8_t uniqueI
                                                uint16_t startAtIndex) {
   using namespace plpcdu;
   switch (uniqueId) {
-    case (PlPcduParamId::NEG_V_LOWER_BOUND):
-    case (PlPcduParamId::NEG_V_UPPER_BOUND):
-    case (PlPcduParamId::DRO_U_LOWER_BOUND):
-    case (PlPcduParamId::DRO_U_UPPER_BOUND):
-    case (PlPcduParamId::DRO_I_UPPER_BOUND):
-    case (PlPcduParamId::X8_U_LOWER_BOUND):
-    case (PlPcduParamId::X8_U_UPPER_BOUND):
-    case (PlPcduParamId::X8_I_UPPER_BOUND):
-    case (PlPcduParamId::TX_U_LOWER_BOUND):
-    case (PlPcduParamId::TX_U_UPPER_BOUND):
-    case (PlPcduParamId::TX_I_UPPER_BOUND):
-    case (PlPcduParamId::MPA_U_LOWER_BOUND):
-    case (PlPcduParamId::MPA_U_UPPER_BOUND):
-    case (PlPcduParamId::MPA_I_UPPER_BOUND):
-    case (PlPcduParamId::HPA_U_LOWER_BOUND):
-    case (PlPcduParamId::HPA_U_UPPER_BOUND):
-    case (PlPcduParamId::HPA_I_UPPER_BOUND):
-    case (PlPcduParamId::SSR_TO_DRO_WAIT_TIME):
-    case (PlPcduParamId::DRO_TO_X8_WAIT_TIME):
-    case (PlPcduParamId::X8_TO_TX_WAIT_TIME):
-    case (PlPcduParamId::TX_TO_MPA_WAIT_TIME):
-    case (PlPcduParamId::MPA_TO_HPA_WAIT_TIME): {
-      handleDoubleParamUpdate(PARAM_KEY_MAP[static_cast<PlPcduParamId>(uniqueId)], parameterWrapper,
-                              newValues);
+    case (PlPcduParamIds::NEG_V_LOWER_BOUND):
+    case (PlPcduParamIds::NEG_V_UPPER_BOUND):
+    case (PlPcduParamIds::DRO_U_LOWER_BOUND):
+    case (PlPcduParamIds::DRO_U_UPPER_BOUND):
+    case (PlPcduParamIds::DRO_I_UPPER_BOUND):
+    case (PlPcduParamIds::X8_U_LOWER_BOUND):
+    case (PlPcduParamIds::X8_U_UPPER_BOUND):
+    case (PlPcduParamIds::X8_I_UPPER_BOUND):
+    case (PlPcduParamIds::TX_U_LOWER_BOUND):
+    case (PlPcduParamIds::TX_U_UPPER_BOUND):
+    case (PlPcduParamIds::TX_I_UPPER_BOUND):
+    case (PlPcduParamIds::MPA_U_LOWER_BOUND):
+    case (PlPcduParamIds::MPA_U_UPPER_BOUND):
+    case (PlPcduParamIds::MPA_I_UPPER_BOUND):
+    case (PlPcduParamIds::HPA_U_LOWER_BOUND):
+    case (PlPcduParamIds::HPA_U_UPPER_BOUND):
+    case (PlPcduParamIds::HPA_I_UPPER_BOUND):
+    case (PlPcduParamIds::SSR_TO_DRO_WAIT_TIME):
+    case (PlPcduParamIds::DRO_TO_X8_WAIT_TIME):
+    case (PlPcduParamIds::X8_TO_TX_WAIT_TIME):
+    case (PlPcduParamIds::TX_TO_MPA_WAIT_TIME):
+    case (PlPcduParamIds::MPA_TO_HPA_WAIT_TIME): {
+      handleDoubleParamUpdate(PARAM_KEY_MAP[static_cast<PlPcduParamIds>(uniqueId)],
+                              parameterWrapper, newValues);
       break;
     }
-    case (PlPcduParamId::INJECT_SSR_TO_DRO_FAILURE): {
+    case (PlPcduParamIds::INJECT_SSR_TO_DRO_FAILURE): {
       ssrToDroInjectionRequested = true;
       break;
     }
-    case (PlPcduParamId::INJECT_DRO_TO_X8_FAILURE): {
+    case (PlPcduParamIds::INJECT_DRO_TO_X8_FAILURE): {
       droToX8InjectionRequested = true;
       break;
     }
-    case (PlPcduParamId::INJECT_X8_TO_TX_FAILURE): {
+    case (PlPcduParamIds::INJECT_X8_TO_TX_FAILURE): {
       x8ToTxInjectionRequested = true;
       break;
     }
-    case (PlPcduParamId::INJECT_TX_TO_MPA_FAILURE): {
+    case (PlPcduParamIds::INJECT_TX_TO_MPA_FAILURE): {
       txToMpaInjectionRequested = true;
       break;
     }
-    case (PlPcduParamId::INJECT_MPA_TO_HPA_FAILURE): {
+    case (PlPcduParamIds::INJECT_MPA_TO_HPA_FAILURE): {
       mpaToHpaInjectionRequested = true;
       break;
     }
-    case (PlPcduParamId::INJECT_ALL_ON_FAILURE): {
+    case (PlPcduParamIds::INJECT_ALL_ON_FAILURE): {
       allOnInjectRequested = true;
       break;
     }
-    case (PlPcduParamId::DISABLE_ORDER_CHECK_CHANNELS): {
-      uint8_t newValue = 0;
-      ReturnValue_t result = newValues->getElement(&newValue);
-      if (result != returnvalue::OK) {
-        return result;
-      }
-      if (newValue > 1) {
-        return HasParametersIF::INVALID_VALUE;
-      }
-      parameterWrapper->set(disableChannelOrderCheck);
-      break;
-    }
     default: {
       return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
                                              startAtIndex);

mission/payload/PayloadPcduHandler.h

@@ -137,12 +137,6 @@ class PayloadPcduHandler : public DeviceHandlerBase {
       PoolEntry<float>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
   PoolEntry<float> tempC = PoolEntry<float>({0.0});
 
-  /**
-   * This parameter disables all checks for the channels except the SSR on check. The SSR on check
-   * is kept to ensure that there is a common start point where the ADC is enabled.
-   */
-  uint8_t disableChannelOrderCheck = false;
-
   void updateSwitchGpio(gpioId_t id, gpio::Levels level);
 
   void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override;
@@ -161,6 +155,7 @@ class PayloadPcduHandler : public DeviceHandlerBase {
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                         LocalDataPoolManager& poolManager) override;
+  ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
   ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
                              const ParameterWrapper* newValues, uint16_t startAtIndex) override;
 
@@ -182,12 +177,6 @@ class PayloadPcduHandler : public DeviceHandlerBase {
   pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
   ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
                                  uint32_t* msToReachTheMode) override;
-  static bool ssrOnForSubmode(uint8_t submode);
-  static bool droOnForSubmode(uint8_t submode);
-  static bool x8OnForSubmode(uint8_t submode);
-  static bool txOnForSubmode(uint8_t submode);
-  static bool mpaOnForSubmode(uint8_t submode);
-  static bool hpaOnForSubmode(uint8_t submode);
 };
 
 #endif /* LINUX_DEVICES_PLPCDUHANDLER_H_ */

mission/payload/ScexDeviceHandler.cpp

@@ -307,7 +307,7 @@ void ScexDeviceHandler::performOperationHook() {
   }
 }
 
-uint32_t ScexDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
+uint32_t ScexDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return OK; }
 
 ReturnValue_t ScexDeviceHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
   if (switchId) {

mission/payload/payloadPcduDefinitions.h

@@ -35,7 +35,7 @@ enum PlPcduAdcChannels : uint8_t {
   NUM_CHANNELS = 12
 };
 
-enum PlPcduParamId : uint8_t {
+enum PlPcduParamIds : uint8_t {
   NEG_V_LOWER_BOUND = 0,
   NEG_V_UPPER_BOUND = 1,
   DRO_U_LOWER_BOUND = 2,
@@ -65,12 +65,10 @@ enum PlPcduParamId : uint8_t {
   INJECT_X8_TO_TX_FAILURE = 32,
   INJECT_TX_TO_MPA_FAILURE = 33,
   INJECT_MPA_TO_HPA_FAILURE = 34,
-  INJECT_ALL_ON_FAILURE = 35,
-
-  DISABLE_ORDER_CHECK_CHANNELS = 40
+  INJECT_ALL_ON_FAILURE = 35
 };
 
-static std::map<PlPcduParamId, std::string> PARAM_KEY_MAP = {
+static std::map<PlPcduParamIds, std::string> PARAM_KEY_MAP = {
     {NEG_V_LOWER_BOUND, "negVoltLowerBound"}, {NEG_V_UPPER_BOUND, "negVoltUpperBound"},
     {DRO_U_LOWER_BOUND, "droVoltLowerBound"}, {DRO_U_UPPER_BOUND, "droVoltUpperBound"},
     {DRO_I_UPPER_BOUND, "droCurrUpperBound"}, {X8_U_LOWER_BOUND, "x8VoltLowerBound"},