diff --git a/CHANGELOG.md b/CHANGELOG.md
index c5eceb87..c545da36 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -17,6 +17,61 @@ change warranting a new major release:
 
 # [unreleased]
 
+# [v1.26.1] 2023-02-08
+
+- Initialize parameter helper in ACS controller.
+
+# [v1.26.0] 2023-02-08
+
+eive-tmtc v2.12.1
+
+## Changed
+
+### ACS
+
+- Readded calibration matrices for MGM calibration.
+- Added calculation of satellite velocity vector from GPS position data
+- Added detumble mode using GYR values
+- Added inertial pointing mode
+- Added nadir pointing mode
+- Added ground station target mode
+- Added antistiction for RWs
+- Added `sunTargetSafe` differentiation for LEOP
+- Added check for existance of `SD_0_SKEWED_PTG_FILE` and `SD_1_SKEWED_PTG_FILE` to determine
+  which `sunTargetSafe` to use
+- Added `gpsVelocity` and `gpsPosition` to `gpsProcessed`
+- Removed deprecated `OutputValues`
+- Added `HasParametersIF` to `AcsParameters`
+- Added `ReceivesParameterMessagesIF` and `ParameterHelper` to `AcsController`
+- Updated `AcsParameters` with actual values and changed structure
+- Sun vector model and magnetic field vector model calculations are always executed now
+- `domainId` is now used as identifier for parameter structs
+- Changed onboard GYR value handling from deg/s to rad/s
+
+## Fixed
+
+- Single sourcing the version information into `CMakeLists.txt`. The `git describe` functionality
+  is only used to retrieve the git SHA hash now. Also removed `OBSWVersion.h` accordingly.
+- Build system: Fixed small bug, where the version itself was
+  stored as the git SHA hash in `commonConfig.h`. This will be
+  an empty string now for regular versions.
+- Bump FSFW for important fix in PUS mode service.
+
+### ACS
+
+- Bugfixes in 'SensorProcessing' where previously MGM values would be calibrated before being
+  transformed in body RF. However, the calibration values are in the body RF. Also fixed the
+  validity flag of 'mgmVecTotDerivative'.
+- Fixed calculation of model sun vector
+- Fixed calculation of model magnetic field vector
+- Fixed MEKF algorithm
+- Fixed several variable initializations
+- Fixed several variable types
+- Fixed use of `sunMagAngleMin` for safe mode
+- Fixed MEKF not using correct `sampleTime`
+- Fixed assignment of `SUS0` and `SUS6` calibration matrices due to wiring being mixed up
+- Various smaller bugfixes
+
 # [v1.25.0] 2023-02-06
 
 eive-tmtc version: v2.12.0
@@ -52,6 +107,10 @@ eive-tmtc version: v2.12.0
 
 ## Changed
 
+- Update ACS scheduling to represent the actual ACS design. There is one ACS PST now for all
+  timing sensitive ACS operations. In the debug builds, the new ACS polling sequence table
+  will have a period of 0.6 seconds, but will remain 0.4 seconds for the release build.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/365
 - `ACS::SensorValues` is now an ACS controller member to reduce the risk of stack overflow.
 - ACS Subsystem Sequence Mode IDs updated.
   PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/365
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 24d73550..6bb34ed7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,9 +9,9 @@
 # ##############################################################################
 cmake_minimum_required(VERSION 3.13)
 
-set(OBSW_VERSION_MAJOR_IF_GIT_FAILS 1)
-set(OBSW_VERSION_MINOR_IF_GIT_FAILS 25)
-set(OBSW_VERSION_REVISION_IF_GIT_FAILS 0)
+set(OBSW_VERSION_MAJOR 1)
+set(OBSW_VERSION_MINOR 26)
+set(OBSW_VERSION_REVISION 1)
 
 # set(CMAKE_VERBOSE TRUE)
 
@@ -168,10 +168,13 @@ if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/.git)
     set(GIT_INFO
         ${GIT_INFO}
         CACHE STRING "Version information retrieved with git describe")
-    list(GET GIT_INFO 1 OBSW_VERSION_MAJOR)
-    list(GET GIT_INFO 2 OBSW_VERSION_MINOR)
-    list(GET GIT_INFO 3 OBSW_VERSION_REVISION)
-    list(GET GIT_INFO 4 OBSW_VERSION_CST_GIT_SHA1)
+    # CMakeLists.txt is now single source of information. list(GET GIT_INFO 1
+    # OBSW_VERSION_MAJOR) list(GET GIT_INFO 2 OBSW_VERSION_MINOR) list(GET
+    # GIT_INFO 3 OBSW_VERSION_REVISION)
+    list(LENGTH GIT_INFO LIST_LEN)
+    if(LIST_LEN GREATER 4)
+      list(GET GIT_INFO 4 OBSW_VERSION_CST_GIT_SHA1)
+    endif()
     if(NOT OBSW_VERSION_MAJOR)
       set(OBSW_VERSION_MAJOR ${OBSW_VERSION_MAJOR_IF_GIT_FAILS})
     endif()
@@ -302,6 +305,9 @@ else()
   endif()
 endif()
 
+include(BuildType)
+set_build_type()
+
 # Configuration files
 configure_file(${COMMON_CONFIG_PATH}/commonConfig.h.in commonConfig.h)
 configure_file(${FSFW_CONFIG_PATH}/FSFWConfig.h.in FSFWConfig.h)
@@ -560,6 +566,3 @@ add_custom_command(
   POST_BUILD
   COMMAND ${CMAKE_SIZE} ${OBSW_BIN_NAME}${FILE_SUFFIX}
   COMMENT ${POST_BUILD_COMMENT})
-
-include(BuildType)
-set_build_type()
diff --git a/bsp_hosted/OBSWConfig.h.in b/bsp_hosted/OBSWConfig.h.in
index e58e534d..5e6fcc2c 100644
--- a/bsp_hosted/OBSWConfig.h.in
+++ b/bsp_hosted/OBSWConfig.h.in
@@ -7,7 +7,6 @@
 #define FSFWCONFIG_OBSWCONFIG_H_
 
 #include "commonConfig.h"
-#include "OBSWVersion.h"
     
 /*******************************************************************/
 /** All of the following flags should be enabled for mission code  */
diff --git a/bsp_q7s/OBSWConfig.h.in b/bsp_q7s/OBSWConfig.h.in
index 5dff001c..faee473f 100644
--- a/bsp_q7s/OBSWConfig.h.in
+++ b/bsp_q7s/OBSWConfig.h.in
@@ -8,7 +8,8 @@
 
 #include "commonConfig.h"
 #include "q7sConfig.h"
-#include "OBSWVersion.h"
+
+#cmakedefine RELEASE_BUILD
 
 /*******************************************************************/
 /** All of the following flags should be enabled for mission code  */
diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp
index 23c6cc4f..116b4fc9 100644
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@@ -5,7 +5,7 @@
 #include <fsfw/ipc/QueueFactory.h>
 #include <fsfw/tasks/TaskFactory.h>
 
-#include "OBSWVersion.h"
+#include "commonConfig.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/timemanager/Stopwatch.h"
 #include "fsfw/version.h"
@@ -179,6 +179,26 @@ ReturnValue_t CoreController::initializeAfterTaskCreation() {
 ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                             const uint8_t *data, size_t size) {
   switch (actionId) {
+    case (ANNOUNCE_VERSION): {
+      uint32_t p1 = (common::OBSW_VERSION_MAJOR << 24) | (common::OBSW_VERSION_MINOR << 16) |
+                    (common::OBSW_VERSION_REVISION << 8);
+      uint32_t p2 = 0;
+      if (strcmp("", common::OBSW_VERSION_CST_GIT_SHA1) != 0) {
+        p1 |= 1;
+        auto shaAsStr = std::string(common::OBSW_VERSION_CST_GIT_SHA1);
+        size_t posDash = shaAsStr.find("-");
+        auto gitHash = shaAsStr.substr(posDash + 2, 4);
+        // Only copy first 4 letters of git hash
+        memcpy(&p2, gitHash.c_str(), 4);
+      }
+
+      triggerEvent(VERSION_INFO, p1, p2);
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
+    case (ANNOUNCE_CURRENT_IMAGE): {
+      triggerEvent(CURRENT_IMAGE_INFO, CURRENT_CHIP, CURRENT_COPY);
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
     case (LIST_DIRECTORY_INTO_FILE): {
       return actionListDirectoryIntoFile(actionId, commandedBy, data, size);
     }
@@ -673,9 +693,9 @@ ReturnValue_t CoreController::initVersionFile() {
     sif::warning << "CoreController::versionFileInit: Retrieving uname line failed" << std::endl;
   }
 
-  std::string fullObswVersionString = "OBSW: v" + std::to_string(SW_VERSION) + "." +
-                                      std::to_string(SW_SUBVERSION) + "." +
-                                      std::to_string(SW_REVISION);
+  std::string fullObswVersionString = "OBSW: v" + std::to_string(common::OBSW_VERSION_MAJOR) + "." +
+                                      std::to_string(common::OBSW_VERSION_MINOR) + "." +
+                                      std::to_string(common::OBSW_VERSION_REVISION);
   char versionString[16] = {};
   fsfw::FSFW_VERSION.getVersion(versionString, sizeof(versionString));
   std::string fullFsfwVersionString = "FSFW: v" + std::string(versionString);
diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h
index 4d9d5ee8..9cbc6340 100644
--- a/bsp_q7s/core/CoreController.h
+++ b/bsp_q7s/core/CoreController.h
@@ -74,6 +74,8 @@ class CoreController : public ExtendedControllerBase {
   static constexpr dur_millis_t DEFAULT_SD_CARD_CHECK_TIMEOUT = 60000;
 
   static constexpr ActionId_t LIST_DIRECTORY_INTO_FILE = 0;
+  static constexpr ActionId_t ANNOUNCE_VERSION = 1;
+  static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2;
   static constexpr ActionId_t SWITCH_REBOOT_FILE_HANDLING = 5;
   static constexpr ActionId_t RESET_REBOOT_COUNTERS = 6;
   static constexpr ActionId_t SWITCH_IMG_LOCK = 7;
@@ -109,6 +111,12 @@ class CoreController : public ExtendedControllerBase {
   //! [EXPORT] : [COMMENT] No SD card was active. Core controller will attempt to re-initialize
   //! a SD card.
   static constexpr Event NO_SD_CARD_ACTIVE = event::makeEvent(SUBSYSTEM_ID, 4, severity::HIGH);
+  //! [EXPORT] : [COMMENT]
+  //! P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash
+  //! P2: First four letters of Git SHA is the last byte of P1 is set.
+  static constexpr Event VERSION_INFO = event::makeEvent(SUBSYSTEM_ID, 5, severity::INFO);
+  //! [EXPORT] : [COMMENT] P1: Current Chip, P2: Current Copy
+  static constexpr Event CURRENT_IMAGE_INFO = event::makeEvent(SUBSYSTEM_ID, 6, severity::INFO);
 
   CoreController(object_id_t objectId);
   virtual ~CoreController();
diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp
index 43c50c02..79390b92 100644
--- a/bsp_q7s/core/scheduling.cpp
+++ b/bsp_q7s/core/scheduling.cpp
@@ -144,11 +144,12 @@ void scheduling::initTasks() {
 #endif
 
   PeriodicTaskIF* comTask = factory->createPeriodicTask(
-      "CCSDS_HANDLER", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
+      "COM_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc);
   result = comTask->addComponent(objects::COM_SUBSYSTEM);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("COM subsystem", objects::COM_SUBSYSTEM);
   }
+
 #if OBSW_ADD_CCSDS_IP_CORES == 1
   result = comTask->addComponent(objects::CCSDS_HANDLER);
   if (result != returnvalue::OK) {
@@ -183,15 +184,8 @@ void scheduling::initTasks() {
 #endif /* OBSW_ADD_GPS_CTRL */
 
   PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
-      "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+      "SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
   static_cast<void>(acsSysTask);
-  // To be removed soon because it will be part of the ACS PST.
-#if OBSW_ADD_ACS_CTRL == 1
-  acsSysTask->addComponent(objects::ACS_CONTROLLER);
-  if (result != returnvalue::OK) {
-    scheduling::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER);
-  }
-#endif
 #if OBSW_ADD_ACS_BOARD == 1
   result = acsSysTask->addComponent(objects::ACS_BOARD_ASS);
   if (result != returnvalue::OK) {
@@ -215,35 +209,6 @@ void scheduling::initTasks() {
     scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM);
   }
 
-#if OBSW_Q7S_EM == 1
-  acsSysTask->addComponent(objects::MGM_0_LIS3_HANDLER);
-  acsSysTask->addComponent(objects::MGM_1_RM3100_HANDLER);
-  acsSysTask->addComponent(objects::MGM_2_LIS3_HANDLER);
-  acsSysTask->addComponent(objects::MGM_3_RM3100_HANDLER);
-  acsSysTask->addComponent(objects::IMTQ_HANDLER);
-  acsSysTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
-  acsSysTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
-  acsSysTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
-  acsSysTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
-  acsSysTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
-  acsSysTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
-  acsSysTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
-  acsSysTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
-  acsSysTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
-  acsSysTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
-  acsSysTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
-  acsSysTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
-  acsSysTask->addComponent(objects::GYRO_0_ADIS_HANDLER);
-  acsSysTask->addComponent(objects::GYRO_1_L3G_HANDLER);
-  acsSysTask->addComponent(objects::GYRO_2_ADIS_HANDLER);
-  acsSysTask->addComponent(objects::GYRO_3_L3G_HANDLER);
-  acsSysTask->addComponent(objects::GPS_CONTROLLER);
-  acsSysTask->addComponent(objects::STAR_TRACKER);
-  acsSysTask->addComponent(objects::RW1);
-  acsSysTask->addComponent(objects::RW2);
-  acsSysTask->addComponent(objects::RW3);
-  acsSysTask->addComponent(objects::RW4);
-#endif
 #if OBSW_ADD_RTD_DEVICES == 1
   PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask(
       "TCS_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc);
@@ -440,11 +405,30 @@ void scheduling::initTasks() {
 void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction missedDeadlineFunc,
                                 std::vector<PeriodicTaskIF*>& taskVec) {
   ReturnValue_t result = returnvalue::OK;
+
+#ifdef RELEASE_BUILD
+  static constexpr float acsPstPeriod = 0.4;
+#else
+  static constexpr float acsPstPeriod = 0.6;
+#endif
+  FixedTimeslotTaskIF* acsPst = factory.createFixedTimeslotTask(
+      "ACS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc);
+  result = pst::pstAcs(acsPst);
+  if (result != returnvalue::OK) {
+    if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
+      sif::warning << "scheduling::initTasks: ACS PST is empty" << std::endl;
+    } else {
+      sif::error << "scheduling::initTasks: Creating ACS PST failed!" << std::endl;
+    }
+  } else {
+    taskVec.push_back(acsPst);
+  }
+
   /* Polling Sequence Table Default */
 #if OBSW_ADD_SPI_TEST_CODE == 0
   FixedTimeslotTaskIF* spiPst = factory.createFixedTimeslotTask(
-      "MAIN_SPI", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc);
-  result = pst::pstSpi(spiPst);
+      "MAIN_SPI", 45, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc);
+  result = pst::pstSpiAndSyrlinks(spiPst);
   if (result != returnvalue::OK) {
     if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
       sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl;
@@ -456,37 +440,9 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
   }
 #endif
 
-#if OBSW_ADD_RW == 1
-  FixedTimeslotTaskIF* rwPstTask = factory.createFixedTimeslotTask(
-      "RW_SPI", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 1.0, missedDeadlineFunc);
-  result = pst::pstSpiRw(rwPstTask);
-  if (result != returnvalue::OK) {
-    if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
-      sif::warning << "scheduling::initTasks: SPI PST is empty" << std::endl;
-    } else {
-      sif::error << "scheduling::initTasks: Creating SPI PST failed!" << std::endl;
-    }
-  } else {
-    taskVec.push_back(rwPstTask);
-  }
-#endif
-
-  FixedTimeslotTaskIF* uartPst = factory.createFixedTimeslotTask(
-      "UART_PST", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
-  result = pst::pstUart(uartPst);
-  if (result != returnvalue::OK) {
-    if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
-      sif::warning << "scheduling::initTasks: UART PST is empty" << std::endl;
-    } else {
-      sif::error << "scheduling::initTasks: Creating UART PST failed!" << std::endl;
-    }
-  } else {
-    taskVec.push_back(uartPst);
-  }
-
 #if OBSW_ADD_I2C_TEST_CODE == 0
   FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask(
-      "I2C_PST", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
+      "I2C_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
   result = pst::pstI2c(i2cPst);
   if (result != returnvalue::OK) {
     if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
@@ -501,7 +457,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
 
 #if OBSW_ADD_GOMSPACE_PCDU == 1
   FixedTimeslotTaskIF* gomSpacePstTask = factory.createFixedTimeslotTask(
-      "GS_PST_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc);
+      "GS_PST_TASK", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc);
   result = pst::pstGompaceCan(gomSpacePstTask);
   if (result != returnvalue::OK) {
     if (result != FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
diff --git a/cmake/BuildType.cmake b/cmake/BuildType.cmake
index e078e5c7..ee027d13 100644
--- a/cmake/BuildType.cmake
+++ b/cmake/BuildType.cmake
@@ -21,10 +21,13 @@ if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
 	)
 endif()
 
+set(RELEASE_BUILD 1 PARENT_SCOPE)
+
 if(${CMAKE_BUILD_TYPE} MATCHES "Debug")
 	message(STATUS 
 		"Building Debug application with flags: ${CMAKE_C_FLAGS_DEBUG}"
 	)
+	set(RELEASE_BUILD 0 PARENT_SCOPE)
 elseif(${CMAKE_BUILD_TYPE} MATCHES "RelWithDebInfo")
 	message(STATUS 
 		"Building Release (Debug) application with "
diff --git a/cmake/EiveHelpers.cmake b/cmake/EiveHelpers.cmake
index 1a717552..b210739e 100644
--- a/cmake/EiveHelpers.cmake
+++ b/cmake/EiveHelpers.cmake
@@ -4,7 +4,7 @@
 # 2. Major version
 # 3. Minor version
 # 4. Revision
-# 5. git SHA hash and commits since tag
+# 5. (Optional) git SHA hash and commits since tag when applicable
 function(determine_version_with_git)
     include(GetGitRevisionDescription)
     git_describe(VERSION ${ARGN})
@@ -22,7 +22,9 @@ function(determine_version_with_git)
     list(APPEND GIT_INFO ${_VERSION_MAJOR})
     list(APPEND GIT_INFO ${_VERSION_MINOR})
     list(APPEND GIT_INFO ${_VERSION_PATCH})
-    list(APPEND GIT_INFO ${VERSION_SHA1})
+    if(NOT VERSION_SHA1 STREQUAL VERSION)
+        list(APPEND GIT_INFO ${VERSION_SHA1})
+    endif()
     set(GIT_INFO ${GIT_INFO} PARENT_SCOPE)
     message(STATUS "eive | Set git version info into GIT_INFO from the git tag ${VERSION}")
 endfunction()
diff --git a/common/config/OBSWVersion.h b/common/config/OBSWVersion.h
deleted file mode 100644
index 36233c3b..00000000
--- a/common/config/OBSWVersion.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef COMMON_CONFIG_OBSWVERSION_H_
-#define COMMON_CONFIG_OBSWVERSION_H_
-
-const char* const SW_NAME = "eive";
-
-#define SW_VERSION 1
-#define SW_SUBVERSION 12
-#define SW_REVISION 1
-
-#endif /* COMMON_CONFIG_OBSWVERSION_H_ */
diff --git a/common/config/eive/resultClassIds.h b/common/config/eive/resultClassIds.h
index aeb05815..e4dfb927 100644
--- a/common/config/eive/resultClassIds.h
+++ b/common/config/eive/resultClassIds.h
@@ -35,6 +35,11 @@ enum commonClassIds : uint8_t {
   SA_DEPL_HANDLER,          // SADPL
   MPSOC_RETURN_VALUES_IF,   // MPSOCRTVIF
   SUPV_RETURN_VALUES_IF,    // SPVRTVIF
+  ACS_KALMAN,               // ACSKAL
+  ACS_SAFE,                 // ACSSAF
+  ACS_PTG,                  // ACSPTG
+  ACS_DETUMBLE,             // ACSDTB
+  ACS_MEKF,                 // ACSMEK
   COMMON_CLASS_ID_END       // [EXPORT] : [END]
 };
 
diff --git a/fsfw b/fsfw
index a1567de9..7f907fb9 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit a1567de9e8d84a044b4a33ccdaffd15f0f4f54f1
+Subproject commit 7f907fb9d36be411d98ff05f3efc8cbd642904f2
diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv
index 2e39b641..9f477ca2 100644
--- a/generators/bsp_q7s_events.csv
+++ b/generators/bsp_q7s_events.csv
@@ -198,6 +198,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 12903;0x3267;SIDE_SWITCH_TRANSITION_NOT_ALLOWED;LOW;Not implemented, would increase already high complexity. Operator should instead command the assembly off first and then command the assembly on into the desired mode/submode combination;mission/system/objects/SusAssembly.h
 13000;0x32c8;CHILDREN_LOST_MODE;MEDIUM;;mission/system/objects/TcsBoardAssembly.h
 13100;0x332c;GPS_FIX_CHANGE;INFO;Fix has changed. P1: Old fix. P2: New fix 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix;mission/devices/devicedefinitions/GPSDefinitions.h
+13101;0x332d;CANT_GET_FIX;LOW;Could not get fix in maximum allowed time. P1: Maximum allowed time to get a fix after the GPS was switched on.;mission/devices/devicedefinitions/GPSDefinitions.h
 13200;0x3390;P60_BOOT_COUNT;INFO;P60 boot count is broadcasted once at SW startup. P1: Boot count;mission/devices/P60DockHandler.h
 13201;0x3391;BATT_MODE;INFO;Battery mode is broadcasted at startup. P1: Mode;mission/devices/P60DockHandler.h
 13202;0x3392;BATT_MODE_CHANGED;MEDIUM;Battery mode has changed. P1: Old mode. P2: New mode;mission/devices/P60DockHandler.h
@@ -244,3 +245,5 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 14002;0x36b2;REBOOT_MECHANISM_TRIGGERED;MEDIUM;The reboot mechanism was triggered. P1: First 16 bits: Last Chip, Last 16 bits: Last Copy, P2: Each byte is the respective reboot count for the slots;bsp_q7s/core/CoreController.h
 14003;0x36b3;REBOOT_HW;MEDIUM;;bsp_q7s/core/CoreController.h
 14004;0x36b4;NO_SD_CARD_ACTIVE;HIGH;No SD card was active. Core controller will attempt to re-initialize a SD card.;bsp_q7s/core/CoreController.h
+14005;0x36b5;VERSION_INFO;INFO;P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;bsp_q7s/core/CoreController.h
+14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;bsp_q7s/core/CoreController.h
diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv
index 82f44173..0581c352 100644
--- a/generators/bsp_q7s_returnvalues.csv
+++ b/generators/bsp_q7s_returnvalues.csv
@@ -51,9 +51,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x4fa4;HEATER_MainSwitchSetTimeout;;164;HEATER_HANDLER;mission/devices/HeaterHandler.h
 0x4fa5;HEATER_CommandAlreadyWaiting;;165;HEATER_HANDLER;mission/devices/HeaterHandler.h
 0x60a0;CCSDS_CommandNotImplemented;Received action message with unknown action id;160;CCSDS_HANDLER;mission/tmtc/CcsdsIpCoreHandler.h
-0x6001;CCSDS_KalmanNoGyrMeas;;1;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6002;CCSDS_KalmanNoModel;;2;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6003;CCSDS_KalmanInversionFailed;;3;CCSDS_HANDLER;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a01;ACSSAF_SafectrlMekfInputInvalid;;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
+0x6b01;ACSPTG_PtgctrlMekfInputInvalid;;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
+0x6c01;ACSDTB_DetumbleNoSensordata;;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
+0x6901;ACSKAL_KalmanNoGyrMeas;;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6902;ACSKAL_KalmanNoModel;;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6903;ACSKAL_KalmanInversionFailed;;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x4500;HSPI_OpeningFileFailed;;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
 0x4501;HSPI_FullDuplexTransferFailed;;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
 0x4502;HSPI_HalfDuplexTransferFailed;;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
@@ -468,16 +471,16 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x1d03;ATC_IllegalApplicationData;;3;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
 0x1d04;ATC_SendTmFailed;;4;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
 0x1d05;ATC_Timeout;;5;ACCEPTS_TELECOMMANDS_IF;fsfw/src/fsfw/tmtcservices/AcceptsTelecommandsIF.h
-0x6b00;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h
+0x7000;SCBU_KeyNotFound;;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h
 0x64a0;FSHLP_SdNotMounted;SD card specified with path string not mounted;160;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h
 0x64a1;FSHLP_FileNotExists;Specified file does not exist on filesystem;161;FILE_SYSTEM_HELPER;bsp_q7s/fs/FilesystemHelper.h
-0x6a00;SDMA_OpOngoing;;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a01;SDMA_AlreadyOn;;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a02;SDMA_AlreadyMounted;;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a03;SDMA_AlreadyOff;;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a0a;SDMA_StatusFileNexists;;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a0b;SDMA_StatusFileFormatInvalid;;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a0c;SDMA_MountError;;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a0d;SDMA_UnmountError;;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a0e;SDMA_SystemCallError;;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
-0x6a0f;SDMA_PopenCallError;;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f00;SDMA_OpOngoing;;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f01;SDMA_AlreadyOn;;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f02;SDMA_AlreadyMounted;;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f03;SDMA_AlreadyOff;;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f0a;SDMA_StatusFileNexists;;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f0b;SDMA_StatusFileFormatInvalid;;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f0c;SDMA_MountError;;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f0d;SDMA_UnmountError;;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f0e;SDMA_SystemCallError;;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
+0x6f0f;SDMA_PopenCallError;;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h
diff --git a/generators/events/event_parser.py b/generators/events/event_parser.py
index 8c5ab35b..8dd4952b 100644
--- a/generators/events/event_parser.py
+++ b/generators/events/event_parser.py
@@ -29,6 +29,7 @@ GENERATE_CSV = True
 COPY_CPP_FILE = True
 COPY_CPP_H_FILE = True
 MOVE_CSV_FILE = True
+PRINT_EVENTS = False
 
 PARSE_HOST_BSP = True
 
@@ -80,13 +81,13 @@ LOGGER = get_console_logger()
 
 
 def parse_events(
-    generate_csv: bool = True, generate_cpp: bool = True, print_events: bool = True
+    generate_csv: bool = True, generate_cpp: bool = True
 ):
     LOGGER.info("EventParser: Parsing events: ")
     # Small delay for clean printout
     time.sleep(0.01)
     event_list = generate_event_list()
-    if print_events:
+    if PRINT_EVENTS:
         PrettyPrinter.pprint(event_list)
         # Delay for clean printout
         time.sleep(0.1)
diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp
index e810f4d1..a13005e1 100644
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 245 translations.
+ * @brief    Auto-generated event translation file. Contains 248 translations.
  * @details
- * Generated on: 2023-02-03 10:52:53
+ * Generated on: 2023-02-08 14:09:40
  */
 #include "translateEvents.h"
 
@@ -200,6 +200,7 @@ const char *POWER_STATE_MACHINE_TIMEOUT_STRING = "POWER_STATE_MACHINE_TIMEOUT";
 const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED";
 const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE";
 const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE";
+const char *CANT_GET_FIX_STRING = "CANT_GET_FIX";
 const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT";
 const char *BATT_MODE_STRING = "BATT_MODE";
 const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED";
@@ -245,6 +246,8 @@ const char *REBOOT_SW_STRING = "REBOOT_SW";
 const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED";
 const char *REBOOT_HW_STRING = "REBOOT_HW";
 const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE";
+const char *VERSION_INFO_STRING = "VERSION_INFO";
+const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO";
 
 const char *translateEvents(Event event) {
   switch ((event & 0xFFFF)) {
@@ -638,6 +641,8 @@ const char *translateEvents(Event event) {
       return CHILDREN_LOST_MODE_STRING;
     case (13100):
       return GPS_FIX_CHANGE_STRING;
+    case (13101):
+      return CANT_GET_FIX_STRING;
     case (13200):
       return P60_BOOT_COUNT_STRING;
     case (13201):
@@ -728,6 +733,10 @@ const char *translateEvents(Event event) {
       return REBOOT_HW_STRING;
     case (14004):
       return NO_SD_CARD_ACTIVE_STRING;
+    case (14005):
+      return VERSION_INFO_STRING;
+    case (14006):
+      return CURRENT_IMAGE_INFO_STRING;
     default:
       return "UNKNOWN_EVENT";
   }
diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp
index 9707189d..0e1c12dc 100644
--- a/generators/objects/translateObjects.cpp
+++ b/generators/objects/translateObjects.cpp
@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 152 translations.
- * Generated on: 2023-02-03 10:52:53
+ * Generated on: 2023-02-08 14:09:40
  */
 #include "translateObjects.h"
 
diff --git a/generators/returnvalues/returnvalues_parser.py b/generators/returnvalues/returnvalues_parser.py
index 4f6f30e1..6ad893d1 100644
--- a/generators/returnvalues/returnvalues_parser.py
+++ b/generators/returnvalues/returnvalues_parser.py
@@ -22,7 +22,7 @@ LOGGER = get_console_logger()
 EXPORT_TO_FILE = True
 COPY_CSV_FILE = True
 EXPORT_TO_SQL = True
-PRINT_TABLES = True
+PRINT_TABLES = False
 
 
 FILE_SEPARATOR = ";"
diff --git a/linux/ObjectFactory.cpp b/linux/ObjectFactory.cpp
index 469187a6..2f04b294 100644
--- a/linux/ObjectFactory.cpp
+++ b/linux/ObjectFactory.cpp
@@ -363,6 +363,6 @@ void ObjectFactory::gpioChecker(ReturnValue_t result, std::string output) {
 
 void ObjectFactory::addTmtcIpCoresToFunnels(CcsdsIpCoreHandler& ipCoreHandler,
                                             PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel) {
-  cfdpFunnel.addDestination(ipCoreHandler, config::LIVE_TM);
-  pusFunnel.addDestination(ipCoreHandler, config::LIVE_TM);
+  cfdpFunnel.addDestination("PTME IP Core", ipCoreHandler, config::LIVE_TM);
+  pusFunnel.addDestination("PTME IP Core", ipCoreHandler, config::LIVE_TM);
 }
diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp
index 6bbbcfb7..cb0523d9 100644
--- a/linux/devices/GpsHyperionLinuxController.cpp
+++ b/linux/devices/GpsHyperionLinuxController.cpp
@@ -34,7 +34,6 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
                                                            uint32_t *msToReachTheMode) {
   if (not modeCommanded) {
     if (mode == MODE_ON or mode == MODE_OFF) {
-      gpsNotOpenSwitch = true;
       // 5h time to reach fix
       *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
       maxTimeToReachFix.resetTimer();
@@ -44,7 +43,12 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
     }
   }
   if (mode == MODE_OFF) {
+    PoolReadGuard pg(&gpsSet);
     gpsSet.setValidity(false, true);
+    // There can't be a fix with a device that is off.
+    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0);
+    oneShotSwitches.reset();
+    modeCommanded = false;
   }
   return returnvalue::OK;
 }
@@ -101,6 +105,7 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
     if (not callAgainImmediately) {
       handleQueue();
       poolManager.performHkOperation();
+      TaskFactory::delayTask(250);
     }
   }
   // Should never be reached.
@@ -113,25 +118,24 @@ ReturnValue_t GpsHyperionLinuxController::initialize() {
     return result;
   }
   auto openError = [&](const char *type, int error) {
-    if (gpsNotOpenSwitch) {
-      // Opening failed
+  // Opening failed
 #if FSFW_VERBOSE_LEVEL >= 1
-      sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
-                   << " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
+    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
+                 << " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
 #endif
-      gpsNotOpenSwitch = false;
-    }
   };
   if (readMode == ReadModes::SOCKET) {
     int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps);
     if (retval != 0) {
       openError("Socket", retval);
+      return ObjectManager::CHILD_INIT_FAILED;
     }
     gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
   } else if (readMode == ReadModes::SHM) {
     int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps);
     if (retval != 0) {
       openError("SHM", retval);
+      return ObjectManager::CHILD_INIT_FAILED;
     }
   }
   return result;
@@ -145,32 +149,33 @@ void GpsHyperionLinuxController::performControlOperation() {}
 
 bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
   auto readError = [&]() {
-    if (gpsReadFailedSwitch) {
-      gpsReadFailedSwitch = false;
+    if (oneShotSwitches.gpsReadFailedSwitch) {
+      oneShotSwitches.gpsReadFailedSwitch = false;
       sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | "
                       "Error "
                    << errno << " | " << gps_errstr(errno) << std::endl;
     }
   };
+  // GPS is off, no point in reading data from GPSD.
+  if (mode == MODE_OFF) {
+    return false;
+  }
   if (readMode == ReadModes::SOCKET) {
-    // Perform other necessary handling if not data seen for 0.2 seconds.
-    if (gps_waiting(&gps, 200000)) {
+    // Poll the GPS.
+    if (gps_waiting(&gps, 0)) {
       if (-1 == gps_read(&gps)) {
         readError();
         return false;
       }
+      oneShotSwitches.gpsReadFailedSwitch = true;
       if (MODE_SET != (MODE_SET & gps.set)) {
-        if (mode == MODE_ON) {
-          if (noModeSetCntr >= 0) {
-            noModeSetCntr++;
-          }
-          if (noModeSetCntr == 10) {
-            // TODO: Trigger event here
-            sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be "
-                            "read for 10 consecutive reads"
-                         << std::endl;
-            noModeSetCntr = -1;
-          }
+        if (mode != MODE_OFF and maxTimeToReachFix.hasTimedOut() and
+            oneShotSwitches.cantGetFixSwitch) {
+          sif::warning
+              << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed "
+              << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
+          triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
+          oneShotSwitches.cantGetFixSwitch = false;
           // did not event get mode, nothing to see.
           return false;
         }
@@ -198,51 +203,58 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
   }
 
   bool validFix = false;
-  static_cast<void>(validFix);
   // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
-  int newFixMode = gps.fix.mode;
-  if (newFixMode == 2 or newFixMode == 3) {
+  if (gps.fix.mode == 2 or gps.fix.mode == 3) {
     validFix = true;
   }
-  if (gpsSet.fixMode.value != newFixMode) {
-    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFixMode);
+  if (gpsSet.fixMode.value != gps.fix.mode) {
+    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode);
   }
-  gpsSet.fixMode.value = newFixMode;
+  gpsSet.fixMode.value = gps.fix.mode;
   if (gps.fix.mode == 0 or gps.fix.mode == 1) {
     if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
-      // We are supposed to be on and functioning, but not fix was found
+      // We are supposed to be on and functioning, but no fix was found
       if (mode == MODE_ON or mode == MODE_NORMAL) {
         mode = MODE_OFF;
       }
       modeCommanded = false;
     }
     gpsSet.setValidity(false, true);
-  } else if (gps.satellites_used > 0) {
+  } else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) {
     gpsSet.setValidity(true, true);
   }
 
   gpsSet.satInUse.value = gps.satellites_used;
   gpsSet.satInView.value = gps.satellites_visible;
 
+  bool latValid = false;
   if (std::isfinite(gps.fix.latitude)) {
     // Negative latitude -> South direction
     gpsSet.latitude.value = gps.fix.latitude;
-  } else {
-    gpsSet.latitude.setValid(false);
+    if (gps.fix.mode >= 2) {
+      latValid = true;
+    }
   }
+  gpsSet.latitude.setValid(latValid);
 
+  bool longValid = false;
   if (std::isfinite(gps.fix.longitude)) {
     // Negative longitude -> West direction
     gpsSet.longitude.value = gps.fix.longitude;
-  } else {
-    gpsSet.longitude.setValid(false);
+    if (gps.fix.mode >= 2) {
+      longValid = true;
+    }
   }
+  gpsSet.latitude.setValid(longValid);
 
+  bool altitudeValid = false;
   if (std::isfinite(gps.fix.altitude)) {
     gpsSet.altitude.value = gps.fix.altitude;
-  } else {
-    gpsSet.altitude.setValid(false);
+    if (gps.fix.mode == 3) {
+      altitudeValid = true;
+    }
   }
+  gpsSet.altitude.setValid(altitudeValid);
 
   if (std::isfinite(gps.fix.speed)) {
     gpsSet.speed.value = gps.fix.speed;
@@ -250,59 +262,44 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
     gpsSet.speed.setValid(false);
   }
 
+  if (TIME_SET == (TIME_SET & gps.set)) {
+    timeval time = {};
 #if LIBGPS_VERSION_MINOR <= 17
-  gpsSet.unixSeconds.value = gps.fix.time;
+    gpsSet.unixSeconds.value = std::floor(gps.fix.time);
+    double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value;
+    time.tv_usec = fractionalPart * 1000.0 * 1000.0;
 #else
-  gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
+    gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
+    time.tv_usec = gps.fix.time.tv_nsec / 1000;
 #endif
-  timeval time = {};
-  time.tv_sec = gpsSet.unixSeconds.value;
-#if LIBGPS_VERSION_MINOR <= 17
-  double fractionalPart = gps.fix.time - std::floor(gps.fix.time);
-  time.tv_usec = fractionalPart * 1000.0 * 1000.0;
-#else
-  time.tv_usec = gps.fix.time.tv_nsec / 1000;
-#endif
-  std::time_t t = std::time(nullptr);
-  if (time.tv_sec == t) {
-    timeIsConstantCounter++;
+    time.tv_sec = gpsSet.unixSeconds.value;
+    // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC
+    // and no time file available) we set it with the roughly valid time from the GPS.
+    // NTP might only work if the time difference between sys time and current time is not too
+    // large.
+    overwriteTimeIfNotSane(time, validFix);
+    Clock::TimeOfDay_t timeOfDay = {};
+    Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
+    gpsSet.year = timeOfDay.year;
+    gpsSet.month = timeOfDay.month;
+    gpsSet.day = timeOfDay.day;
+    gpsSet.hours = timeOfDay.hour;
+    gpsSet.minutes = timeOfDay.minute;
+    gpsSet.seconds = timeOfDay.second;
   } else {
-    timeIsConstantCounter = 0;
-  }
-  if (timeInit and validFix) {
-    if (not utility::timeSanityCheck()) {
-#if OBSW_VERBOSE_LEVEL >= 1
-      time_t timeRaw = time.tv_sec;
-      std::tm *timeTm = std::gmtime(&timeRaw);
-      sif::info << "Setting invalid system time from GPS data directly: "
-                << std::put_time(timeTm, "%c %Z") << std::endl;
-#endif
-      // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
-      Clock::setClock(&time);
-    }
-    timeInit = false;
-  }
-  // If the received time does not change anymore for whatever reason, do not set it here
-  // to avoid stale times. Also, don't do it too often often to avoid jumping times
-  if (timeIsConstantCounter < 20 and timeUpdateCd.hasTimedOut()) {
-    // Update the system time here for now. NTP seems to be unable to do so for whatever reason.
-    // Further tests have shown that the time seems to be set by NTPD after some time..
-    // Clock::setClock(&time);
-    timeUpdateCd.resetTimer();
+    gpsSet.unixSeconds.setValid(false);
+    gpsSet.year.setValid(false);
+    gpsSet.month.setValid(false);
+    gpsSet.day.setValid(false);
+    gpsSet.hours.setValid(false);
+    gpsSet.minutes.setValid(false);
+    gpsSet.seconds.setValid(false);
   }
 
-  Clock::TimeOfDay_t timeOfDay = {};
-  Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
-  gpsSet.year = timeOfDay.year;
-  gpsSet.month = timeOfDay.month;
-  gpsSet.day = timeOfDay.day;
-  gpsSet.hours = timeOfDay.hour;
-  gpsSet.minutes = timeOfDay.minute;
-  gpsSet.seconds = timeOfDay.second;
   if (debugHyperionGps) {
     sif::info << "-- Hyperion GPS Data --" << std::endl;
 #if LIBGPS_VERSION_MINOR <= 17
-    time_t timeRaw = gps.fix.time;
+    time_t timeRaw = gpsSet.unixSeconds.value;
 #else
     time_t timeRaw = gps.fix.time.tv_sec;
 #endif
@@ -325,3 +322,19 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
   }
   return returnvalue::OK;
 }
+
+void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) {
+  if (not timeInit and validFix) {
+    if (not utility::timeSanityCheck()) {
+#if OBSW_VERBOSE_LEVEL >= 1
+      time_t timeRaw = time.tv_sec;
+      std::tm *timeTm = std::gmtime(&timeRaw);
+      sif::info << "Overwriting invalid system time from GPS data directly: "
+                << std::put_time(timeTm, "%c %Z") << std::endl;
+#endif
+      // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
+      Clock::setClock(&time);
+    }
+    timeInit = true;
+  }
+}
diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h
index d2dc91b6..5d4a35ff 100644
--- a/linux/devices/GpsHyperionLinuxController.h
+++ b/linux/devices/GpsHyperionLinuxController.h
@@ -58,18 +58,30 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
   const char* currentClientBuf = nullptr;
   ReadModes readMode = ReadModes::SOCKET;
   Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);
-  bool modeCommanded = true;
-  bool timeInit = true;
-  bool gpsNotOpenSwitch = true;
-  bool gpsReadFailedSwitch = true;
+  bool modeCommanded = false;
+  bool timeInit = false;
+
+  struct OneShotSwitches {
+    void reset() {
+      gpsReadFailedSwitch = true;
+      cantGetFixSwitch = true;
+    }
+    bool gpsReadFailedSwitch = true;
+    bool cantGetFixSwitch = true;
+
+  } oneShotSwitches;
+
   bool debugHyperionGps = false;
   int32_t noModeSetCntr = 0;
-  uint32_t timeIsConstantCounter = 0;
   Countdown timeUpdateCd = Countdown(60);
 
   // Returns true if the function should be called again or false if other
   // controller handling can be done.
   bool readGpsDataFromGpsd();
+  // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC)
+  // we set it with the roughly valid time from the GPS. For some reason, NTP might only work
+  // if the time difference between sys time and current time is not too large
+  void overwriteTimeIfNotSane(timeval time, bool validFix);
 };
 
 #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */
diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp
index e810f4d1..a13005e1 100644
--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 245 translations.
+ * @brief    Auto-generated event translation file. Contains 248 translations.
  * @details
- * Generated on: 2023-02-03 10:52:53
+ * Generated on: 2023-02-08 14:09:40
  */
 #include "translateEvents.h"
 
@@ -200,6 +200,7 @@ const char *POWER_STATE_MACHINE_TIMEOUT_STRING = "POWER_STATE_MACHINE_TIMEOUT";
 const char *SIDE_SWITCH_TRANSITION_NOT_ALLOWED_STRING = "SIDE_SWITCH_TRANSITION_NOT_ALLOWED";
 const char *CHILDREN_LOST_MODE_STRING = "CHILDREN_LOST_MODE";
 const char *GPS_FIX_CHANGE_STRING = "GPS_FIX_CHANGE";
+const char *CANT_GET_FIX_STRING = "CANT_GET_FIX";
 const char *P60_BOOT_COUNT_STRING = "P60_BOOT_COUNT";
 const char *BATT_MODE_STRING = "BATT_MODE";
 const char *BATT_MODE_CHANGED_STRING = "BATT_MODE_CHANGED";
@@ -245,6 +246,8 @@ const char *REBOOT_SW_STRING = "REBOOT_SW";
 const char *REBOOT_MECHANISM_TRIGGERED_STRING = "REBOOT_MECHANISM_TRIGGERED";
 const char *REBOOT_HW_STRING = "REBOOT_HW";
 const char *NO_SD_CARD_ACTIVE_STRING = "NO_SD_CARD_ACTIVE";
+const char *VERSION_INFO_STRING = "VERSION_INFO";
+const char *CURRENT_IMAGE_INFO_STRING = "CURRENT_IMAGE_INFO";
 
 const char *translateEvents(Event event) {
   switch ((event & 0xFFFF)) {
@@ -638,6 +641,8 @@ const char *translateEvents(Event event) {
       return CHILDREN_LOST_MODE_STRING;
     case (13100):
       return GPS_FIX_CHANGE_STRING;
+    case (13101):
+      return CANT_GET_FIX_STRING;
     case (13200):
       return P60_BOOT_COUNT_STRING;
     case (13201):
@@ -728,6 +733,10 @@ const char *translateEvents(Event event) {
       return REBOOT_HW_STRING;
     case (14004):
       return NO_SD_CARD_ACTIVE_STRING;
+    case (14005):
+      return VERSION_INFO_STRING;
+    case (14006):
+      return CURRENT_IMAGE_INFO_STRING;
     default:
       return "UNKNOWN_EVENT";
   }
diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp
index 9707189d..0e1c12dc 100644
--- a/linux/fsfwconfig/objects/translateObjects.cpp
+++ b/linux/fsfwconfig/objects/translateObjects.cpp
@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 152 translations.
- * Generated on: 2023-02-03 10:52:53
+ * Generated on: 2023-02-08 14:09:40
  */
 #include "translateObjects.h"
 
diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
index 5fec0cd6..efbb9578 100644
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@@ -15,38 +15,17 @@
 #define RPI_TEST_GPS_HANDLER 0
 #endif
 
-ReturnValue_t pst::pstSpiRw(FixedTimeslotTaskIF *thisSequence) {
+ReturnValue_t pst::pstSpiAndSyrlinks(FixedTimeslotTaskIF *thisSequence) {
   uint32_t length = thisSequence->getPeriodMs();
-  static_cast<void>(length);
-  thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
 
-  thisSequence->addSlot(objects::RW1, length * 0.2, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::RW2, length * 0.2, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::RW3, length * 0.2, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::RW4, length * 0.2, DeviceHandlerIF::SEND_WRITE);
+#if OBSW_ADD_SYRLINKS == 1
+  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
+#endif
 
-  thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::GET_WRITE);
-
-  thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::SEND_READ);
-
-  thisSequence->addSlot(objects::RW1, length * 0.8, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::RW2, length * 0.8, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::RW3, length * 0.8, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::RW4, length * 0.8, DeviceHandlerIF::GET_READ);
-  return thisSequence->checkSequence();
-}
-
-ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
-  uint32_t length = thisSequence->getPeriodMs();
   static_cast<void>(length);
 #if OBSW_ADD_PL_PCDU == 1
   thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
@@ -56,271 +35,6 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
   thisSequence->addSlot(objects::PLPCDU_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
 #endif
 
-#if OBSW_ADD_SUN_SENSORS == 1
-
-  bool addSus0 = true;
-  bool addSus1 = true;
-  bool addSus2 = true;
-  bool addSus3 = true;
-  bool addSus4 = true;
-  bool addSus5 = true;
-  bool addSus6 = true;
-  bool addSus7 = true;
-  bool addSus8 = true;
-  bool addSus9 = true;
-  bool addSus10 = true;
-  bool addSus11 = true;
-
-  if (addSus0) {
-    /* Write setup */
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-  if (addSus1) {
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-  if (addSus2) {
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-  if (addSus3) {
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-  if (addSus4) {
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-  if (addSus5) {
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-
-  if (addSus6) {
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-
-  if (addSus7) {
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-
-  if (addSus8) {
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-
-  if (addSus9) {
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-
-  if (addSus10) {
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
-                          DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-
-  if (addSus11) {
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
-                          DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.4,
-                          DeviceHandlerIF::GET_READ);
-  }
-#endif /* OBSW_ADD_SUN_SENSORS == 1 */
-
 #if OBSW_ADD_RAD_SENSORS == 1
   /* Radiation sensor */
   thisSequence->addSlot(objects::RAD_SENSOR, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
@@ -330,74 +44,6 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
   thisSequence->addSlot(objects::RAD_SENSOR, length * 0.8, DeviceHandlerIF::GET_READ);
 #endif
 
-#if OBSW_ADD_ACS_BOARD == 1
-  bool enableAside = true;
-  bool enableBside = true;
-  if (enableAside) {
-    // A side
-    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.25,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
-  }
-
-  if (enableBside) {
-    // B side
-    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.25,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.25, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.6, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.7, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.85, DeviceHandlerIF::GET_READ);
-  }
-#endif /* OBSW_ADD_ACS_BOARD == 1 */
-
   return thisSequence->checkSequence();
 }
 
@@ -407,23 +53,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
   // Length of a communication cycle
   uint32_t length = thisSequence->getPeriodMs();
   static_cast<void>(length);
-#if OBSW_ADD_MGT == 1
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::GET_READ);
-#endif
 #if OBSW_ADD_BPX_BATTERY_HANDLER == 1
   thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2,
                         DeviceHandlerIF::PERFORM_OPERATION);
@@ -481,29 +110,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
   return thisSequence->checkSequence();
 }
 
-ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) {
-  // Length of a communication cycle
-  uint32_t length = thisSequence->getPeriodMs();
-  static_cast<void>(length);
-
-#if OBSW_ADD_SYRLINKS == 1
-  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::SYRLINKS_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
-#endif
-
-#if OBSW_ADD_STAR_TRACKER == 1
-  thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::STAR_TRACKER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::STAR_TRACKER, length * 0.4, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::STAR_TRACKER, length * 0.6, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::STAR_TRACKER, length * 0.8, DeviceHandlerIF::GET_READ);
-#endif
-  return thisSequence->checkSequence();
-}
-
 ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) {
   uint32_t length = thisSequence->getPeriodMs();
   // PCDU handlers receives two messages and both must be handled
@@ -576,3 +182,422 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) {
   }
   return returnvalue::OK;
 }
+
+ReturnValue_t pst::pstAcs(FixedTimeslotTaskIF *thisSequence) {
+  /* Length of a communication cycle */
+  uint32_t length = thisSequence->getPeriodMs();
+#if OBSW_ADD_ACS_BOARD == 1
+  bool enableAside = true;
+  bool enableBside = true;
+  if (enableAside) {
+    // A side
+    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+  }
+
+  if (enableBside) {
+    // B side
+    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+  }
+#endif /* OBSW_ADD_ACS_BOARD == 1 */
+  // SUS: 16 ms
+#if OBSW_ADD_SUN_SENSORS == 1
+
+  bool addSus0 = true;
+  bool addSus1 = true;
+  bool addSus2 = true;
+  bool addSus3 = true;
+  bool addSus4 = true;
+  bool addSus5 = true;
+  bool addSus6 = true;
+  bool addSus7 = true;
+  bool addSus8 = true;
+  bool addSus9 = true;
+  bool addSus10 = true;
+  bool addSus11 = true;
+
+  if (addSus0) {
+    /* Write setup */
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_0_N_LOC_XFYFZM_PT_XF, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+  if (addSus1) {
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_1_N_LOC_XBYFZM_PT_XB, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+  if (addSus2) {
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_2_N_LOC_XFYBZB_PT_YB, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+  if (addSus3) {
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_3_N_LOC_XFYBZF_PT_YF, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+  if (addSus4) {
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+  if (addSus5) {
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+
+  if (addSus6) {
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+
+  if (addSus7) {
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+
+  if (addSus8) {
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+
+  if (addSus9) {
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+
+  if (addSus10) {
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
+                          DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+
+  if (addSus11) {
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
+                          DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0.0325,
+                          DeviceHandlerIF::GET_READ);
+  }
+#endif /* OBSW_ADD_SUN_SENSORS == 1 */
+
+#if OBSW_ADD_STAR_TRACKER == 1
+  thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ);
+#endif
+
+#if OBSW_ADD_MGT == 1
+  // This is the MTM measurement cycle
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.0625, DeviceHandlerIF::GET_READ);
+#endif
+
+  thisSequence->addSlot(objects::ACS_CONTROLLER, length * 0.1, 0);
+
+#if OBSW_ADD_MGT == 1
+  // This is the torquing cycle.
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.1125, DeviceHandlerIF::GET_READ);
+#endif
+
+#if OBSW_ADD_RW == 1
+  thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::PERFORM_OPERATION);
+
+  thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::SEND_WRITE);
+
+  thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::GET_WRITE);
+
+  thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::SEND_READ);
+
+  thisSequence->addSlot(objects::RW1, length * 0.1125, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::RW2, length * 0.1125, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::RW3, length * 0.1125, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::RW4, length * 0.1125, DeviceHandlerIF::GET_READ);
+#endif
+  return returnvalue::OK;
+}
diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h
index 1b585d81..1dcfc30a 100644
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.h
@@ -39,11 +39,9 @@ namespace pst {
  */
 ReturnValue_t pstGompaceCan(FixedTimeslotTaskIF* thisSequence);
 
-ReturnValue_t pstUart(FixedTimeslotTaskIF* thisSequence);
+ReturnValue_t pstSpiAndSyrlinks(FixedTimeslotTaskIF* thisSequence);
 
-ReturnValue_t pstSpi(FixedTimeslotTaskIF* thisSequence);
-
-ReturnValue_t pstSpiRw(FixedTimeslotTaskIF* thisSequence);
+ReturnValue_t pstAcs(FixedTimeslotTaskIF* thisSequence);
 
 ReturnValue_t pstI2c(FixedTimeslotTaskIF* thisSequence);
 
diff --git a/mission/acsDefs.h b/mission/acsDefs.h
index 8869d6ff..ac093f6f 100644
--- a/mission/acsDefs.h
+++ b/mission/acsDefs.h
@@ -6,15 +6,16 @@
 
 namespace acs {
 
-enum CtrlSubmode {
+// These modes are the submodes of the ACS controller and the modes of the ACS subsystem.
+enum AcsMode {
   OFF = HasModesIF::MODE_OFF,
   SAFE = 10,
   DETUMBLE = 11,
-  IDLE = 12,
-  PTG_TARGET_NADIR = 13,
+  PTG_IDLE = 12,
+  PTG_NADIR = 13,
   PTG_TARGET = 14,
   PTG_TARGET_GS = 15,
-  PTG_TARGET_INERTIAL = 16,
+  PTG_INERTIAL = 16,
 };
 
 static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 8a58eb90..83088b41 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId)
       detumble(&acsParameters),
       ptgCtrl(&acsParameters),
       detumbleCounter{0},
+      parameterHelper(this),
       mgmDataRaw(this),
       mgmDataProcessed(this),
       susDataRaw(this),
@@ -27,7 +28,25 @@ AcsController::AcsController(object_id_t objectId)
       actuatorCmdData(this) {}
 
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
-  return returnvalue::OK;
+  ReturnValue_t result = actionHelper.handleActionMessage(message);
+  if (result == returnvalue::OK) {
+    return result;
+  }
+  result = parameterHelper.handleParameterMessage(message);
+  if (result == returnvalue::OK) {
+    return result;
+  }
+  return result;
+}
+
+MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }
+
+ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
+                                          ParameterWrapper *parameterWrapper,
+                                          const ParameterWrapper *newValues,
+                                          uint16_t startAtIndex) {
+  return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues,
+                                    startAtIndex);
 }
 
 void AcsController::performControlOperation() {
@@ -52,9 +71,11 @@ void AcsController::performControlOperation() {
           case acs::DETUMBLE:
             performDetumble();
             break;
+          case acs::PTG_IDLE:
           case acs::PTG_TARGET:
-          case acs::PTG_TARGET_NADIR:
-          case acs::PTG_TARGET_INERTIAL:
+          case acs::PTG_TARGET_GS:
+          case acs::PTG_NADIR:
+          case acs::PTG_INERTIAL:
             performPointingCtrl();
             break;
         }
@@ -86,10 +107,6 @@ void AcsController::performControlOperation() {
 }
 
 void AcsController::performSafe() {
-  //	Concept: SAFE MODE WITH MEKF
-  //	-do the sensor processing, maybe is does make more sense do call this class function in
-  //	another place since we have to do it for every mode regardless of safe or not
-
   ACS::SensorValues sensorValues;
 
   timeval now;
@@ -128,10 +145,10 @@ void AcsController::performSafe() {
   {
     PoolReadGuard pg(&ctrlValData);
     if (pg.getReadResult() == returnvalue::OK) {
-      double zeroQuat[4] = {0, 0, 0, 0};
-      std::memcpy(ctrlValData.tgtQuat.value, zeroQuat, 4 * sizeof(double));
+      double unitQuat[4] = {0, 0, 0, 1};
+      std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
       ctrlValData.tgtQuat.setValid(false);
-      std::memcpy(ctrlValData.errQuat.value, zeroQuat, 4 * sizeof(double));
+      std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
       ctrlValData.errQuat.setValid(false);
       ctrlValData.errAng.value = errAng;
       ctrlValData.errAng.setValid(true);
@@ -153,7 +170,7 @@ void AcsController::performSafe() {
   }
   if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
     detumbleCounter = 0;
-    // Triggers detubmle mode transition in subsystem
+    // Triggers detumble mode transition in subsystem
     triggerEvent(acs::SAFE_RATE_VIOLATION);
   }
 
@@ -174,7 +191,8 @@ void AcsController::performSafe() {
   //    PoolReadGuard pg(&dipoleSet);
   //    MutexGuard mg(torquer::lazyLock());
   //    torquer::NEW_ACTUATION_FLAG = true;
-  //    dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2], torqueDuration);
+  //    dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
+  //    torqueDuration);
   //  }
 }
 
@@ -252,29 +270,146 @@ void AcsController::performPointingCtrl() {
                      &mekfData, &validMekf);
 
   double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
-  guidance.targetQuatPtg(&sensorValues, &mekfData, &susDataProcessed, now, targetQuat, refSatRate);
+  double quatRef[4] = {0, 0, 0, 0};
+  uint8_t enableAntiStiction = true;
+
   double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
          deltaRate[3] = {0, 0, 0};  // ToDo: check if pointer needed
-  guidance.comparePtg(targetQuat, &mekfData, refSatRate, quatErrorComplete, quatError, deltaRate);
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
-  double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
-  ptgCtrl.ptgGroundstation(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
-  double rwTrqNs[4] = {0, 0, 0, 0};
-  ptgCtrl.ptgNullspace(
-      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
-  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
-  actuatorCmd.cmdSpeedToRws(
-      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
-      rwTrqNs, cmdSpeedRws);
+  double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
+  double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
   double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
-  ptgCtrl.ptgDesaturation(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);
+
+  switch (submode) {
+    case acs::PTG_IDLE:
+      guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
+                          targetQuat, refSatRate);
+      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
+                  4 * sizeof(double));
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      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, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &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);
+
+      break;
+
+    case acs::PTG_TARGET:
+      guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
+                                      refSatRate);
+      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
+                  4 * sizeof(double));
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      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, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &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);
+      break;
+
+    case acs::PTG_TARGET_GS:
+      guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
+                               targetQuat, refSatRate);
+      std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
+                  4 * sizeof(double));
+      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      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, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &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);
+      break;
+
+    case acs::PTG_NADIR:
+      guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
+                                     refSatRate);
+      std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
+      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      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, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &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);
+      break;
+
+    case acs::PTG_INERTIAL:
+      guidance.inertialQuatPtg(targetQuat, refSatRate);
+      std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
+                  4 * sizeof(double));
+      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
+      guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
+                          deltaRate);
+      ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
+                     *rwPseudoInv, torquePtgRws);
+      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, torqueRwsScaled);
+      ptgCtrl.ptgDesaturation(
+          &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);
+      break;
+  }
+
+  if (enableAntiStiction) {
+    bool rwAvailable[4] = {true, true, true, true};  // WHICH INPUT SENSOR SET?
+    int32_t rwSpeed[4] = {
+        (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value),
+        (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)};
+    ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
+  }
+
+  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
+  actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
+                            &(sensorValues.rw2Set.currSpeed.value),
+                            &(sensorValues.rw3Set.currSpeed.value),
+                            &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
   actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
 
   int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
@@ -371,6 +506,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   // GPS Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
   localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
+  localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
+  localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
   poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});
   // MEKF
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
@@ -426,7 +563,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
       return INVALID_SUBMODE;
     }
   } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
-    if ((submode > 6) || (submode < 2)) {
+    if ((submode < acs::AcsMode::SAFE) or (submode > acs::AcsMode::PTG_INERTIAL)) {
       return INVALID_SUBMODE;
     } else {
       return returnvalue::OK;
@@ -488,7 +625,6 @@ void AcsController::copyMgmData() {
 void AcsController::copySusData() {
   {
     PoolReadGuard pg(&sensorValues.susSets[0]);
-
     if (pg.getReadResult() == returnvalue::OK) {
       std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
                   6 * sizeof(uint16_t));
@@ -585,6 +721,14 @@ void AcsController::copySusData() {
   }
 }
 
+ReturnValue_t AcsController::initialize() {
+  ReturnValue_t result = parameterHelper.initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return ExtendedControllerBase::initialize();
+}
+
 void AcsController::copyGyrData() {
   ACS::SensorValues sensorValues;
   {
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 308700e4..e1e31a2a 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -3,6 +3,8 @@
 
 #include <fsfw/controller/ExtendedControllerBase.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/parameters/ParameterHelper.h>
+#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
 
 #include "acs/ActuatorCmd.h"
 #include "acs/Guidance.h"
@@ -18,12 +20,17 @@
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
 #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
 
-class AcsController : public ExtendedControllerBase {
+class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
  public:
   static constexpr dur_millis_t INIT_DELAY = 500;
 
   AcsController(object_id_t objectId);
 
+  MessageQueueId_t getCommandQueue() const;
+  ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
+                             ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
+                             uint16_t startAtIndex) override;
+
  protected:
   void performSafe();
   void performDetumble();
@@ -42,10 +49,13 @@ class AcsController : public ExtendedControllerBase {
 
   uint8_t detumbleCounter;
 
+  ParameterHelper parameterHelper;
+
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };
 
   InternalState internalState = InternalState::STARTUP;
 
+  ReturnValue_t initialize() override;
   ReturnValue_t handleCommandMessage(CommandMessage* message) override;
   void performControlOperation() override;
 
@@ -134,6 +144,8 @@ class AcsController : public ExtendedControllerBase {
   acsctrl::GpsDataProcessed gpsDataProcessed;
   PoolEntry<double> gcLatitude = PoolEntry<double>();
   PoolEntry<double> gdLongitude = PoolEntry<double>();
+  PoolEntry<double> gpsPosition = PoolEntry<double>(3);
+  PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
 
   // MEKF
   acsctrl::MekfData mekfData;
diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index cc9959ab..13642fe9 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -3,542 +3,611 @@
 #include <fsfw/src/fsfw/globalfunctions/constants.h>
 #include <stddef.h>
 
-AcsParameters::AcsParameters(){};  //(uint8_t parameterModuleId) :
-                                   // parameterModuleId(parameterModuleId) {}
+AcsParameters::AcsParameters() {}
 
 AcsParameters::~AcsParameters() {}
 
-/*ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint16_t parameterId,
+ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
                                           ParameterWrapper* parameterWrapper,
                                           const ParameterWrapper* newValues,
                                           uint16_t startAtIndex) {
-  if (domainId == parameterModuleId) {
-    switch (parameterId >> 8) {
-      case 0x0:  // direct members
-        switch (parameterId & 0xFF) {
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case 0x1:  // OnBoardParams
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(onBoardParams.sampleTime);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case 0x2:  // InertiaEIVE
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(inertiaEIVE.inertiaMatrix);
-            break;
-          case 0x1:
-            parameterWrapper->set(inertiaEIVE.inertiaMatrixInverse);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case 0x3:  // MgmHandlingParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix);
-            break;
-          case 0x1:
-            parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix);
-            break;
-          case 0x2:
-            parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix);
-            break;
-          case 0x3:
-            parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix);
-            break;
-          case 0x4:
-            parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix);
-            break;
-          case 0x5:
-            parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset);
-            break;
-          case 0x6:
-            parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset);
-            break;
-          case 0x7:
-            parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset);
-            break;
-          case 0x8:
-            parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset);
-            break;
-          case 0x9:
-            parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset);
-            break;
-          case 0xA:
-            parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse);
-            break;
-          case 0xB:
-            parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse);
-            break;
-          case 0xC:
-            parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse);
-            break;
-          case 0xD:
-            parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse);
-            break;
-          case 0xE:
-            parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case 0x4:  // SusHandlingParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(susHandlingParameters.sus0orientationMatrix);
-            break;
-          case 0x1:
-            parameterWrapper->set(susHandlingParameters.sus1orientationMatrix);
-            break;
-          case 0x2:
-            parameterWrapper->set(susHandlingParameters.sus2orientationMatrix);
-            break;
-          case 0x3:
-            parameterWrapper->set(susHandlingParameters.sus3orientationMatrix);
-            break;
-          case 0x4:
-            parameterWrapper->set(susHandlingParameters.sus4orientationMatrix);
-            break;
-          case 0x5:
-            parameterWrapper->set(susHandlingParameters.sus5orientationMatrix);
-            break;
-          case 0x6:
-            parameterWrapper->set(susHandlingParameters.sus6orientationMatrix);
-            break;
-          case 0x7:
-            parameterWrapper->set(susHandlingParameters.sus7orientationMatrix);
-            break;
-          case 0x8:
-            parameterWrapper->set(susHandlingParameters.sus8orientationMatrix);
-            break;
-          case 0x9:
-            parameterWrapper->set(susHandlingParameters.sus9orientationMatrix);
-            break;
-          case 0xA:
-            parameterWrapper->set(susHandlingParameters.sus10orientationMatrix);
-            break;
-          case 0xB:
-            parameterWrapper->set(susHandlingParameters.sus11orientationMatrix);
-            break;
-          case 0xC:
-            parameterWrapper->set(susHandlingParameters.sus0coeffAlpha);
-            break;
-          case 0xD:
-            parameterWrapper->set(susHandlingParameters.sus0coeffBeta);
-            break;
-          case 0xE:
-            parameterWrapper->set(susHandlingParameters.sus1coeffAlpha);
-            break;
-          case 0xF:
-            parameterWrapper->set(susHandlingParameters.sus1coeffBeta);
-            break;
-          case 0x10:
-            parameterWrapper->set(susHandlingParameters.sus2coeffAlpha);
-            break;
-          case 0x11:
-            parameterWrapper->set(susHandlingParameters.sus2coeffBeta);
-            break;
-          case 0x12:
-            parameterWrapper->set(susHandlingParameters.sus3coeffAlpha);
-            break;
-          case 0x13:
-            parameterWrapper->set(susHandlingParameters.sus3coeffBeta);
-            break;
-          case 0x14:
-            parameterWrapper->set(susHandlingParameters.sus4coeffAlpha);
-            break;
-          case 0x15:
-            parameterWrapper->set(susHandlingParameters.sus4coeffBeta);
-            break;
-          case 0x16:
-            parameterWrapper->set(susHandlingParameters.sus5coeffAlpha);
-            break;
-          case 0x17:
-            parameterWrapper->set(susHandlingParameters.sus5coeffBeta);
-            break;
-          case 0x18:
-            parameterWrapper->set(susHandlingParameters.sus6coeffAlpha);
-            break;
-          case 0x19:
-            parameterWrapper->set(susHandlingParameters.sus6coeffBeta);
-            break;
-          case 0x1A:
-            parameterWrapper->set(susHandlingParameters.sus7coeffAlpha);
-            break;
-          case 0x1B:
-            parameterWrapper->set(susHandlingParameters.sus7coeffBeta);
-            break;
-          case 0x1C:
-            parameterWrapper->set(susHandlingParameters.sus8coeffAlpha);
-            break;
-          case 0x1D:
-            parameterWrapper->set(susHandlingParameters.sus8coeffBeta);
-            break;
-          case 0x1E:
-            parameterWrapper->set(susHandlingParameters.sus9coeffAlpha);
-            break;
-          case 0x1F:
-            parameterWrapper->set(susHandlingParameters.sus9coeffBeta);
-            break;
-          case 0x20:
-            parameterWrapper->set(susHandlingParameters.sus10coeffAlpha);
-            break;
-          case 0x21:
-            parameterWrapper->set(susHandlingParameters.sus10coeffBeta);
-            break;
-          case 0x22:
-            parameterWrapper->set(susHandlingParameters.sus11coeffAlpha);
-            break;
-          case 0x23:
-            parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
-            break;
-          case 0x24:
-            parameterWrapper->set(susHandlingParameters.filterAlpha);
-            break;
-          case 0x25:
-            parameterWrapper->set(susHandlingParameters.sunThresh);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0x5):  // GyrHandlingParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix);
-            break;
-          case 0x1:
-            parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix);
-            break;
-          case 0x2:
-            parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix);
-            break;
-          case 0x3:
-            parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
-            break;
-          case 0x4:
-            parameterWrapper->set(gyrHandlingParameters.gyrFusionWeight);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0x6):  // RwHandlingParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix);
-            break;
-          case 0x1:
-            parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix);
-            break;
-          case 0x2:
-            parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix);
-            break;
-          case 0x3:
-            parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix);
-            break;
-          case 0x4:
-            parameterWrapper->set(rwHandlingParameters.inertiaWheel);
-            break;
-          case 0x5:
-            parameterWrapper->set(rwHandlingParameters.maxTrq);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0x7):  // RwMatrices
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(rwMatrices.alignmentMatrix);
-            break;
-          case 0x1:
-            parameterWrapper->set(rwMatrices.pseudoInverse);
-            break;
-          case 0x2:
-            parameterWrapper->set(rwMatrices.without0);
-            break;
-          case 0x3:
-            parameterWrapper->set(rwMatrices.without1);
-            break;
-          case 0x4:
-            parameterWrapper->set(rwMatrices.without2);
-            break;
-          case 0x5:
-            parameterWrapper->set(rwMatrices.without3);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0x8):  // SafeModeControllerParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(safeModeControllerParameters.k_rate_mekf);
-            break;
-          case 0x1:
-            parameterWrapper->set(safeModeControllerParameters.k_align_mekf);
-            break;
-          case 0x2:
-            parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf);
-            break;
-          case 0x3:
-            parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf);
-            break;
-          case 0x4:
-            parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
-            break;
-          case 0x5:
-            parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
-            break;
-          case 0x6:
-            parameterWrapper->set(safeModeControllerParameters.satRateRef);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0x9):  // DetumbleCtrlParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(detumbleCtrlParameters.gainD);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0xA):  // PointingModeControllerParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(targetModeControllerParameters.updtFlag);
-            break;
-          case 0x1:
-            parameterWrapper->set(targetModeControllerParameters.A_rw);
-            break;
-          case 0x2:
-            parameterWrapper->set(targetModeControllerParameters.refDirection);
-            break;
-          case 0x3:
-            parameterWrapper->set(targetModeControllerParameters.refRotRate);
-            break;
-          case 0x4:
-            parameterWrapper->set(targetModeControllerParameters.quatRef);
-            break;
-          case 0x5:
-            parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
-            break;
-          case 0x6:
-            parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
-            break;
-          case 0x7:
-            parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
-            break;
-          case 0x8:
-            parameterWrapper->set(targetModeControllerParameters.blindRotRate);
-            break;
-          case 0x9:
-            parameterWrapper->set(targetModeControllerParameters.zeta);
-            break;
-          case 0xA:
-            parameterWrapper->set(targetModeControllerParameters.zetaLow);
-            break;
-          case 0xB:
-            parameterWrapper->set(targetModeControllerParameters.om);
-            break;
-          case 0xC:
-            parameterWrapper->set(targetModeControllerParameters.omLow);
-            break;
-          case 0xD:
-            parameterWrapper->set(targetModeControllerParameters.omMax);
-            break;
-          case 0xE:
-            parameterWrapper->set(targetModeControllerParameters.qiMin);
-            break;
-          case 0xF:
-            parameterWrapper->set(targetModeControllerParameters.gainNullspace);
-            break;
-          case 0x10:
-            parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
-            break;
-          case 0x11:
-            parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
-            break;
-          case 0x12:
-            parameterWrapper->set(targetModeControllerParameters.desatOn);
-            break;
-          case 0x13:
-            parameterWrapper->set(targetModeControllerParameters.omegaEarth);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0xB):  // StrParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(strParameters.exclusionAngle);
-            break;
-          case 0x1:
-            parameterWrapper->set(strParameters.boresightAxis);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0xC):  // GpsParameters
-        switch (parameterId & 0xFF) {
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0xD):  // GroundStationParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(groundStationParameters.latitudeGs);
-            break;
-          case 0x1:
-            parameterWrapper->set(groundStationParameters.longitudeGs);
-            break;
-          case 0x2:
-            parameterWrapper->set(groundStationParameters.altitudeGs);
-            break;
-          case 0x3:
-            parameterWrapper->set(groundStationParameters.earthRadiusEquat);
-            break;
-          case 0x4:
-            parameterWrapper->set(groundStationParameters.earthRadiusPolar);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0xE):  // SunModelParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(sunModelParameters.useSunModel);
-            break;
-          case 0x1:
-            parameterWrapper->set(sunModelParameters.domega);
-            break;
-          case 0x2:
-            parameterWrapper->set(sunModelParameters.omega_0);
-            break;
-          case 0x3:
-            parameterWrapper->set(sunModelParameters.m_0);
-            break;
-          case 0x4:
-            parameterWrapper->set(sunModelParameters.dm);
-            break;
-          case 0x5:
-            parameterWrapper->set(sunModelParameters.e);
-            break;
-          case 0x6:
-            parameterWrapper->set(sunModelParameters.e1);
-            break;
-          case 0x7:
-            parameterWrapper->set(sunModelParameters.p1);
-            break;
-          case 0x8:
-            parameterWrapper->set(sunModelParameters.p2);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0xF):  // KalmanFilterParameters
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(kalmanFilterParameters.activateKalmanFilter);
-            break;
-          case 0x1:
-            parameterWrapper->set(kalmanFilterParameters.requestResetFlag);
-            break;
-          case 0x2:
-            parameterWrapper->set(
-                kalmanFilterParameters.maxToleratedTimeBetweenKalmanFilterExecutionSteps);
-            break;
-          case 0x3:
-            parameterWrapper->set(kalmanFilterParameters.processNoiseOmega);
-            break;
-          case 0x4:
-            parameterWrapper->set(kalmanFilterParameters.processNoiseQuaternion);
-            break;
-          case 0x5:
-            parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
-            break;
-          case 0x6:
-            parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
-            break;
-          case 0x7:
-            parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
-            break;
-          case 0x8:
-            parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
-            break;
-          case 0x9:
-            parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
-            break;
-          case 0xA:
-            parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0x10):  // MagnetorquesParameter
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
-            break;
-          case 0x1:
-            parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix);
-            break;
-          case 0x2:
-            parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix);
-            break;
-          case 0x3:
-            parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq);
-            break;
-          case 0x4:
-            parameterWrapper->set(magnetorquesParameter.inverseAlignment);
-            break;
-          case 0x5:
-            parameterWrapper->set(magnetorquesParameter.DipolMax);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      case (0x11):  // DetumbleParameter
-        switch (parameterId & 0xFF) {
-          case 0x0:
-            parameterWrapper->set(detumbleParameter.detumblecounter);
-            break;
-          case 0x1:
-            parameterWrapper->set(detumbleParameter.omegaDetumbleStart);
-            break;
-          case 0x2:
-            parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
-            break;
-          default:
-            return INVALID_IDENTIFIER_ID;
-        }
-        break;
-      default:
-        return INVALID_IDENTIFIER_ID;
-    }
-    return returnvalue::OK;
-  } else {
-    return INVALID_DOMAIN_ID;
+  switch (domainId) {
+    case 0x0:  // direct members
+      switch (parameterId) {
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case 0x1:  // OnBoardParams
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(onBoardParams.sampleTime);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case 0x2:  // InertiaEIVE
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(inertiaEIVE.inertiaMatrix);
+          break;
+        case 0x1:
+          parameterWrapper->set(inertiaEIVE.inertiaMatrixDeployed);
+          break;
+        case 0x2:
+          parameterWrapper->set(inertiaEIVE.inertiaMatrixUndeployed);
+          break;
+        case 0x3:
+          parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel1);
+          break;
+        case 0x4:
+          parameterWrapper->set(inertiaEIVE.inertiaMatrixPanel3);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case 0x3:  // MgmHandlingParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(mgmHandlingParameters.mgm0orientationMatrix);
+          break;
+        case 0x1:
+          parameterWrapper->set(mgmHandlingParameters.mgm1orientationMatrix);
+          break;
+        case 0x2:
+          parameterWrapper->set(mgmHandlingParameters.mgm2orientationMatrix);
+          break;
+        case 0x3:
+          parameterWrapper->set(mgmHandlingParameters.mgm3orientationMatrix);
+          break;
+        case 0x4:
+          parameterWrapper->set(mgmHandlingParameters.mgm4orientationMatrix);
+          break;
+        case 0x5:
+          parameterWrapper->set(mgmHandlingParameters.mgm0hardIronOffset);
+          break;
+        case 0x6:
+          parameterWrapper->set(mgmHandlingParameters.mgm1hardIronOffset);
+          break;
+        case 0x7:
+          parameterWrapper->set(mgmHandlingParameters.mgm2hardIronOffset);
+          break;
+        case 0x8:
+          parameterWrapper->set(mgmHandlingParameters.mgm3hardIronOffset);
+          break;
+        case 0x9:
+          parameterWrapper->set(mgmHandlingParameters.mgm4hardIronOffset);
+          break;
+        case 0xA:
+          parameterWrapper->set(mgmHandlingParameters.mgm0softIronInverse);
+          break;
+        case 0xB:
+          parameterWrapper->set(mgmHandlingParameters.mgm1softIronInverse);
+          break;
+        case 0xC:
+          parameterWrapper->set(mgmHandlingParameters.mgm2softIronInverse);
+          break;
+        case 0xD:
+          parameterWrapper->set(mgmHandlingParameters.mgm3softIronInverse);
+          break;
+        case 0xE:
+          parameterWrapper->set(mgmHandlingParameters.mgm4softIronInverse);
+          break;
+        case 0xF:
+          parameterWrapper->set(mgmHandlingParameters.mgm02variance);
+          break;
+        case 0x10:
+          parameterWrapper->set(mgmHandlingParameters.mgm13variance);
+          break;
+        case 0x11:
+          parameterWrapper->set(mgmHandlingParameters.mgm4variance);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case 0x4:  // SusHandlingParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(susHandlingParameters.sus0orientationMatrix);
+          break;
+        case 0x1:
+          parameterWrapper->set(susHandlingParameters.sus1orientationMatrix);
+          break;
+        case 0x2:
+          parameterWrapper->set(susHandlingParameters.sus2orientationMatrix);
+          break;
+        case 0x3:
+          parameterWrapper->set(susHandlingParameters.sus3orientationMatrix);
+          break;
+        case 0x4:
+          parameterWrapper->set(susHandlingParameters.sus4orientationMatrix);
+          break;
+        case 0x5:
+          parameterWrapper->set(susHandlingParameters.sus5orientationMatrix);
+          break;
+        case 0x6:
+          parameterWrapper->set(susHandlingParameters.sus6orientationMatrix);
+          break;
+        case 0x7:
+          parameterWrapper->set(susHandlingParameters.sus7orientationMatrix);
+          break;
+        case 0x8:
+          parameterWrapper->set(susHandlingParameters.sus8orientationMatrix);
+          break;
+        case 0x9:
+          parameterWrapper->set(susHandlingParameters.sus9orientationMatrix);
+          break;
+        case 0xA:
+          parameterWrapper->set(susHandlingParameters.sus10orientationMatrix);
+          break;
+        case 0xB:
+          parameterWrapper->set(susHandlingParameters.sus11orientationMatrix);
+          break;
+        case 0xC:
+          parameterWrapper->set(susHandlingParameters.sus0coeffAlpha);
+          break;
+        case 0xD:
+          parameterWrapper->set(susHandlingParameters.sus0coeffBeta);
+          break;
+        case 0xE:
+          parameterWrapper->set(susHandlingParameters.sus1coeffAlpha);
+          break;
+        case 0xF:
+          parameterWrapper->set(susHandlingParameters.sus1coeffBeta);
+          break;
+        case 0x10:
+          parameterWrapper->set(susHandlingParameters.sus2coeffAlpha);
+          break;
+        case 0x11:
+          parameterWrapper->set(susHandlingParameters.sus2coeffBeta);
+          break;
+        case 0x12:
+          parameterWrapper->set(susHandlingParameters.sus3coeffAlpha);
+          break;
+        case 0x13:
+          parameterWrapper->set(susHandlingParameters.sus3coeffBeta);
+          break;
+        case 0x14:
+          parameterWrapper->set(susHandlingParameters.sus4coeffAlpha);
+          break;
+        case 0x15:
+          parameterWrapper->set(susHandlingParameters.sus4coeffBeta);
+          break;
+        case 0x16:
+          parameterWrapper->set(susHandlingParameters.sus5coeffAlpha);
+          break;
+        case 0x17:
+          parameterWrapper->set(susHandlingParameters.sus5coeffBeta);
+          break;
+        case 0x18:
+          parameterWrapper->set(susHandlingParameters.sus6coeffAlpha);
+          break;
+        case 0x19:
+          parameterWrapper->set(susHandlingParameters.sus6coeffBeta);
+          break;
+        case 0x1A:
+          parameterWrapper->set(susHandlingParameters.sus7coeffAlpha);
+          break;
+        case 0x1B:
+          parameterWrapper->set(susHandlingParameters.sus7coeffBeta);
+          break;
+        case 0x1C:
+          parameterWrapper->set(susHandlingParameters.sus8coeffAlpha);
+          break;
+        case 0x1D:
+          parameterWrapper->set(susHandlingParameters.sus8coeffBeta);
+          break;
+        case 0x1E:
+          parameterWrapper->set(susHandlingParameters.sus9coeffAlpha);
+          break;
+        case 0x1F:
+          parameterWrapper->set(susHandlingParameters.sus9coeffBeta);
+          break;
+        case 0x20:
+          parameterWrapper->set(susHandlingParameters.sus10coeffAlpha);
+          break;
+        case 0x21:
+          parameterWrapper->set(susHandlingParameters.sus10coeffBeta);
+          break;
+        case 0x22:
+          parameterWrapper->set(susHandlingParameters.sus11coeffAlpha);
+          break;
+        case 0x23:
+          parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0x5):  // GyrHandlingParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(gyrHandlingParameters.gyr0orientationMatrix);
+          break;
+        case 0x1:
+          parameterWrapper->set(gyrHandlingParameters.gyr1orientationMatrix);
+          break;
+        case 0x2:
+          parameterWrapper->set(gyrHandlingParameters.gyr2orientationMatrix);
+          break;
+        case 0x3:
+          parameterWrapper->set(gyrHandlingParameters.gyr3orientationMatrix);
+          break;
+        case 0x4:
+          parameterWrapper->set(gyrHandlingParameters.gyr0bias);
+          break;
+        case 0x5:
+          parameterWrapper->set(gyrHandlingParameters.gyr1bias);
+          break;
+        case 0x6:
+          parameterWrapper->set(gyrHandlingParameters.gyr2bias);
+          break;
+        case 0x7:
+          parameterWrapper->set(gyrHandlingParameters.gyr3bias);
+          break;
+        case 0x8:
+          parameterWrapper->set(gyrHandlingParameters.gyr02variance);
+          break;
+        case 0x9:
+          parameterWrapper->set(gyrHandlingParameters.gyr13variance);
+          break;
+        case 0xA:
+          parameterWrapper->set(gyrHandlingParameters.preferAdis);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0x6):  // RwHandlingParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(rwHandlingParameters.inertiaWheel);
+          break;
+        case 0x1:
+          parameterWrapper->set(rwHandlingParameters.maxTrq);
+          break;
+        case 0x2:
+          parameterWrapper->set(rwHandlingParameters.stictionSpeed);
+          break;
+        case 0x3:
+          parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
+          break;
+        case 0x4:
+          parameterWrapper->set(rwHandlingParameters.stictionTorque);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0x7):  // RwMatrices
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(rwMatrices.alignmentMatrix);
+          break;
+        case 0x1:
+          parameterWrapper->set(rwMatrices.pseudoInverse);
+          break;
+        case 0x2:
+          parameterWrapper->set(rwMatrices.without0);
+          break;
+        case 0x3:
+          parameterWrapper->set(rwMatrices.without1);
+          break;
+        case 0x4:
+          parameterWrapper->set(rwMatrices.without2);
+          break;
+        case 0x5:
+          parameterWrapper->set(rwMatrices.without3);
+          break;
+        case 0x6:
+          parameterWrapper->set(rwMatrices.nullspace);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0x8):  // SafeModeControllerParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(safeModeControllerParameters.k_rate_mekf);
+          break;
+        case 0x1:
+          parameterWrapper->set(safeModeControllerParameters.k_align_mekf);
+          break;
+        case 0x2:
+          parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf);
+          break;
+        case 0x3:
+          parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf);
+          break;
+        case 0x4:
+          parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
+          break;
+        case 0x5:
+          parameterWrapper->set(safeModeControllerParameters.sunTargetDirLeop);
+          break;
+        case 0x6:
+          parameterWrapper->set(safeModeControllerParameters.sunTargetDir);
+          break;
+        case 0x7:
+          parameterWrapper->set(safeModeControllerParameters.satRateRef);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0x9):  // TargetModeControllerParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(targetModeControllerParameters.zeta);
+          break;
+        case 0x1:
+          parameterWrapper->set(targetModeControllerParameters.om);
+          break;
+        case 0x2:
+          parameterWrapper->set(targetModeControllerParameters.omMax);
+          break;
+        case 0x3:
+          parameterWrapper->set(targetModeControllerParameters.qiMin);
+          break;
+        case 0x4:
+          parameterWrapper->set(targetModeControllerParameters.gainNullspace);
+          break;
+        case 0x5:
+          parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
+          break;
+        case 0x6:
+          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
+          break;
+        case 0x7:
+          parameterWrapper->set(targetModeControllerParameters.desatOn);
+          break;
+        case 0x8:
+          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          break;
+        case 0x9:
+          parameterWrapper->set(targetModeControllerParameters.refDirection);
+          break;
+        case 0xA:
+          parameterWrapper->set(targetModeControllerParameters.refRotRate);
+          break;
+        case 0xB:
+          parameterWrapper->set(targetModeControllerParameters.quatRef);
+          break;
+        case 0xC:
+          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
+          break;
+        case 0xD:
+          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
+          break;
+        case 0xE:
+          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
+          break;
+        case 0xF:
+          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
+          break;
+        case 0x10:
+          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
+          break;
+        case 0x11:
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
+          break;
+        case 0x12:
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          break;
+        case 0x13:
+          parameterWrapper->set(targetModeControllerParameters.blindRotRate);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0xA):  // NadirModeControllerParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(nadirModeControllerParameters.zeta);
+          break;
+        case 0x1:
+          parameterWrapper->set(nadirModeControllerParameters.om);
+          break;
+        case 0x2:
+          parameterWrapper->set(nadirModeControllerParameters.omMax);
+          break;
+        case 0x3:
+          parameterWrapper->set(nadirModeControllerParameters.qiMin);
+          break;
+        case 0x4:
+          parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
+          break;
+        case 0x5:
+          parameterWrapper->set(nadirModeControllerParameters.desatMomentumRef);
+          break;
+        case 0x6:
+          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
+          break;
+        case 0x7:
+          parameterWrapper->set(nadirModeControllerParameters.desatOn);
+          break;
+        case 0x8:
+          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
+          break;
+        case 0x9:
+          parameterWrapper->set(nadirModeControllerParameters.refDirection);
+          break;
+        case 0xA:
+          parameterWrapper->set(nadirModeControllerParameters.quatRef);
+          break;
+        case 0xC:
+          parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0xB):  // InertialModeControllerParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(inertialModeControllerParameters.zeta);
+          break;
+        case 0x1:
+          parameterWrapper->set(inertialModeControllerParameters.om);
+          break;
+        case 0x2:
+          parameterWrapper->set(inertialModeControllerParameters.omMax);
+          break;
+        case 0x3:
+          parameterWrapper->set(inertialModeControllerParameters.qiMin);
+          break;
+        case 0x4:
+          parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
+          break;
+        case 0x5:
+          parameterWrapper->set(inertialModeControllerParameters.desatMomentumRef);
+          break;
+        case 0x6:
+          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
+          break;
+        case 0x7:
+          parameterWrapper->set(inertialModeControllerParameters.desatOn);
+          break;
+        case 0x8:
+          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
+          break;
+        case 0x9:
+          parameterWrapper->set(inertialModeControllerParameters.tgtQuat);
+          break;
+        case 0xA:
+          parameterWrapper->set(inertialModeControllerParameters.refRotRate);
+          break;
+        case 0xC:
+          parameterWrapper->set(inertialModeControllerParameters.quatRef);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0xC):  // StrParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(strParameters.exclusionAngle);
+          break;
+        case 0x1:
+          parameterWrapper->set(strParameters.boresightAxis);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0xD):  // GpsParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0xE):  // SunModelParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(sunModelParameters.domega);
+          break;
+        case 0x1:
+          parameterWrapper->set(sunModelParameters.omega_0);
+          break;
+        case 0x2:
+          parameterWrapper->set(sunModelParameters.m_0);
+          break;
+        case 0x3:
+          parameterWrapper->set(sunModelParameters.dm);
+          break;
+        case 0x4:
+          parameterWrapper->set(sunModelParameters.e);
+          break;
+        case 0x5:
+          parameterWrapper->set(sunModelParameters.e1);
+          break;
+        case 0x6:
+          parameterWrapper->set(sunModelParameters.p1);
+          break;
+        case 0x7:
+          parameterWrapper->set(sunModelParameters.p2);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0xF):  // KalmanFilterParameters
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
+          break;
+        case 0x1:
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
+          break;
+        case 0x2:
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
+          break;
+        case 0x3:
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
+          break;
+        case 0x4:
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
+          break;
+        case 0x5:
+          parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0x10):  // MagnetorquesParameter
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
+          break;
+        case 0x1:
+          parameterWrapper->set(magnetorquesParameter.mtq1orientationMatrix);
+          break;
+        case 0x2:
+          parameterWrapper->set(magnetorquesParameter.mtq2orientationMatrix);
+          break;
+        case 0x3:
+          parameterWrapper->set(magnetorquesParameter.alignmentMatrixMtq);
+          break;
+        case 0x4:
+          parameterWrapper->set(magnetorquesParameter.inverseAlignment);
+          break;
+        case 0x5:
+          parameterWrapper->set(magnetorquesParameter.DipolMax);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    case (0x11):  // DetumbleParameter
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(detumbleParameter.detumblecounter);
+          break;
+        case 0x1:
+          parameterWrapper->set(detumbleParameter.omegaDetumbleStart);
+          break;
+        case 0x2:
+          parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
+          break;
+        case 0x3:
+          parameterWrapper->set(detumbleParameter.gainD);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    default:
+      return INVALID_DOMAIN_ID;
   }
-}*/
+  return returnvalue::OK;
+}
diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index 5d40f752..f1c0fb63 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -12,22 +12,36 @@
 
 typedef unsigned char uint8_t;
 
-class AcsParameters /*: public HasParametersIF*/ {
+class AcsParameters : public HasParametersIF {
  public:
   AcsParameters();
   virtual ~AcsParameters();
-  /*
-          virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId,
-                                  ParameterWrapper *parameterWrapper,
-                                  const ParameterWrapper *newValues, uint16_t startAtIndex);
-  */
+
+  ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
+                             ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
+                             uint16_t startAtIndex) override;
+
   struct OnBoardParams {
-    double sampleTime = 0.1;  // [s]
+    double sampleTime = 0.4;  // [s]
   } onBoardParams;
 
   struct InertiaEIVE {
-    double inertiaMatrix[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.5, 1.0}};
-    double inertiaMatrixInverse[3][3];
+    double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
+                                  {-0.0001821456, 0.1701302, 0.0004748963},
+                                  {-0.0050135, 0.0004748963, 0.08374296}};  // 19.11.2021
+    // Possible inertia matrices
+    double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
+                                          {-0.0001821456, 0.1701302, 0.0004748963},
+                                          {-0.0050135, 0.0004748963, 0.08374296}};  // 19.11.2021
+    double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008},
+                                            {-0.0001798426, 0.162240, 0.000475596},
+                                            {-0.005008, 0.000475596, 0.060136}};  // 19.11.2021
+    double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207},
+                                        {-0.0001836122, 0.16619787, 0.0083537},
+                                        {-0.00501207, 0.0083537, 0.07192588}};  // 19.11.2021
+    double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767},
+                                        {-0.000178376, 0.166172, -0.007403},
+                                        {-0.005009767, -0.007403, 0.07195314}};
   } inertiaEIVE;
 
   struct MgmHandlingParameters {
@@ -37,40 +51,41 @@ class AcsParameters /*: public HasParametersIF*/ {
     float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
     float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
 
-    float mgm0hardIronOffset[3] = {0.0, 0.0, 0.0};  //{19.89364, -29.94111, -31.07508};
-    float mgm1hardIronOffset[3] = {0.0, 0.0, 0.0};  //{10.95500, -8.053403, -33.36383};
-    float mgm2hardIronOffset[3] = {0.0, 0.0, 0.0};  //{15.72181, -26.87090, -62.19010};
-    float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0};
-    float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0};
+    float mgm0hardIronOffset[3] = {6.116487, 6.796264, -19.188060};
+    float mgm1hardIronOffset[3] = {-1.077152, 2.080583, 1.974483};
+    float mgm2hardIronOffset[3] = {-19.285857, 5.401821, -16.096297};
+    float mgm3hardIronOffset[3] = {-0.634033, 2.787695, 0.092036};
+    float mgm4hardIronOffset[3] = {2.702743, 5.236043, 0.726229};
+
+    float mgm0softIronInverse[3][3] = {{0.910192, -0.188413, -0.161522},
+                                       {-0.188413, 1.642303, -0.033184},
+                                       {-0.161522, -0.033184, 0.943904}};
+    float mgm1softIronInverse[3][3] = {{1.053508, -0.170225, -0.041678},
+                                       {-0.170225, 1.274465, -0.040231},
+                                       {-0.041678, -0.040231, 1.086352}};
+    float mgm2softIronInverse[3][3] = {{0.931086, 0.172675, -0.043084},
+                                       {0.172675, 1.541296, 0.065489},
+                                       {-0.043084, 0.065489, 1.001238}};
+    float mgm3softIronInverse[3][3] = {{1.073353, 0.177266, -0.058832},
+                                       {0.177266, 1.262156, 0.010478},
+                                       {-0.058832, 0.010478, 1.068345}};
+    float mgm4softIronInverse[3][3] = {{1.114887, -0.007534, -0.037072},
+                                       {-0.007534, 1.253879, 0.006812},
+                                       {-0.037072, 0.006812, 1.313158}};
 
-    float mgm0softIronInverse[3][3] = {
-        {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{1420.727e-3, 9.352825e-3,
-         -127.1979e-3}, {9.352825e-3, 1031.965e-3, -80.02734e-3},
-         {-127.1979e-3, -80.02734e-3, 934.8899e-3}};*/
-    float mgm1softIronInverse[3][3] = {
-        {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{126.7325e-2, -4.146410e-2, -18.37963e-2},
-         {-4.146410e-2, 109.3310e-2, -5.246314e-2},
-         {-18.37963e-2, -5.246314e-2, 105.7300e-2}};*/
-    float mgm2softIronInverse[3][3] = {
-        {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{143.0438e-2, 7.095763e-2,
-         15.67482e-2}, {7.095763e-2, 99.65167e-2, -6.958760e-2},
-         {15.67482e-2, -6.958760e-2, 94.50124e-2}};*/
-    float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
-    float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
     float mgm02variance[3] = {1, 1, 1};
     float mgm13variance[3] = {1, 1, 1};
     float mgm4variance[3] = {1, 1, 1};
-
   } mgmHandlingParameters;
 
   struct SusHandlingParameters {
-    float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}};     // FM07
+    float sus0orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}};     // FM10
     float sus1orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};   // FM06
     float sus2orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}};  // FM13
     float sus3orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}};    // FM14
     float sus4orientationMatrix[3][3] = {{0, -1, 0}, {1, 0, 0}, {0, 0, 1}};    // FM05
     float sus5orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}};   // FM02
-    float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}};     // FM10
+    float sus6orientationMatrix[3][3] = {{0, 0, 1}, {1, 0, 0}, {0, 1, 0}};     // FM07
     float sus7orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};   // FM01
     float sus8orientationMatrix[3][3] = {{-1, 0, 0}, {0, 0, -1}, {0, -1, 0}};  // FM03
     float sus9orientationMatrix[3][3] = {{1, 0, 0}, {0, 0, 1}, {0, -1, 0}};    // FM11
@@ -78,61 +93,61 @@ class AcsParameters /*: public HasParametersIF*/ {
     float sus11orientationMatrix[3][3] = {{1, 0, 0}, {0, -1, 0}, {0, 0, -1}};  // FM08
 
     float sus0coeffAlpha[9][10] = {
-        {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707,
-         -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396,
-         -0.000124096561459262, 0.00040790566176981},
-        {6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417,
-         0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415,
-         1.63114413539632e-05, -2.0187452317724e-05},
-        {-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334,
-         0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995,
-         -0.000249094601806692, 0.000231466668650876},
-        {-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384,
-         -0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05,
-         -9.78534772816957e-06, -4.72083745991256e-05},
-        {0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541,
-         -0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463,
-         0.00010150571088124, -0.000367705461590854},
-        {3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526,
-         -0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05,
-         6.97244631313601e-05, 2.50519145070895e-05},
-        {-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191,
-         -0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928,
-         -1.78466217018177e-05, -0.00058976234038192},
-        {1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089,
-         -0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06,
-         -2.73407888222758e-05, 5.45131881032866e-06},
-        {43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675,
-         0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078,
-         -0.000889196131314391, -0.000509766491897672}};
+        {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293,
+         0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146,
+         0.00091428505258307, 0.000259857066722932},
+        {1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859,
+         0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05,
+         -1.06065583714065e-05, -1.43899892666699e-05},
+        {8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577,
+         0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412,
+         -4.88501228194031e-06, -0.000434861113274231},
+        {-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253,
+         0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05,
+         6.09619017310129e-05, 4.23908862836885e-05},
+        {-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607,
+         0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257,
+         0.000150795563555828, -0.000279246491927943},
+        {7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557,
+         -0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05,
+         9.13233708460098e-05, -0.000206717750924323},
+        {16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216,
+         -0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597,
+         -0.000532190902882765, 9.36018171121253e-05},
+        {-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645,
+         -0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05,
+         1.02609175615251e-05, -9.64717658954667e-06},
+        {-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358,
+         -0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05,
+         0.000868163357631254, 0.00120099606910313}};
     float sus0coeffBeta[9][10] = {
-        {1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235,
-         -0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05,
-         -5.52178824394723e-06, 5.73935295303589e-05},
-        {3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133,
-         0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05,
-         -7.58501977656551e-05, -8.79809730730992e-05},
-        {14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518,
-         0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341,
-         -0.00020861690864945},
-        {1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335,
-         -0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06,
-         -6.00508568552101e-05, 0.000101583822589461},
-        {-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729,
-         -0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05,
-         0.000316835483508523, 0.000151442890664899},
-        {-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231,
-         0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05,
-         -9.83662017231755e-05, 1.87078667116619e-05},
-        {27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016,
-         -0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772,
-         -0.000285438191474895, -5.71327627917386e-05},
-        {-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328,
-         -0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05,
-         -9.77555098179959e-05, 2.09624089449761e-05},
-        {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461,
-         -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688,
-         0.000141998709314758, 0.000101051304611037}};
+        {12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917,
+         0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05,
+         0.000821479971871302, -4.5330528329465e-05},
+        {1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823,
+         0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05,
+         -4.04514487800062e-05, -7.6296149087237e-05},
+        {5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941,
+         -0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412,
+         0.0011723869613426, 0.000122882034141316},
+        {2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172,
+         -0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05,
+         -5.87601932564404e-05, -3.92033314627704e-05},
+        {2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898,
+         0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264,
+         0.000259925974231328, 0.000222437797823852},
+        {1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167,
+         0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05,
+         -4.33518182614169e-05, 4.66993119419691e-05},
+        {-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108,
+         0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429,
+         0.000401546410974577, -0.000147563886502726},
+        {1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231,
+         0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05,
+         -5.3309466243918e-05, 6.20289310356821e-06},
+        {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785,
+         0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741,
+         -0.000186887396585446, 0.00119710704771344}};
     float sus1coeffAlpha[9][10] = {
         {-27.6783250420482, -0.964805032861791, -0.503974297997131, -0.0446471081874084,
          -0.048219538329297, 0.000958491361905381, -0.000290972187162876, -0.000657145721554176,
@@ -414,61 +429,61 @@ class AcsParameters /*: public HasParametersIF*/ {
          -0.0542697403292778, 0.0285360568428252, 0.000845084580479371, 0.00114184315411245,
          -0.000169538153750085, -0.000336529204350355}};
     float sus6coeffAlpha[9][10] = {
-        {13.0465222152293, 0.0639132159808454, 2.98083557560227, -0.0773202212713293,
-         0.0949075412003712, 0.0503055998355815, -0.00104133434256204, 0.000633099036136146,
-         0.00091428505258307, 0.000259857066722932},
-        {1.66740227859888, 1.55804368674744, 0.209274741749388, 0.0123798418560859,
-         0.00724950517167516, -0.000577445375457582, 8.94374551545955e-05, 6.94513586221567e-05,
-         -1.06065583714065e-05, -1.43899892666699e-05},
-        {8.71610925597519, 1.42112818752419, -0.549859300501301, 0.0374581774684577,
-         0.0617635595955198, 0.0447491072679598, 0.00069998577106559, 0.00101018723225412,
-         -4.88501228194031e-06, -0.000434861113274231},
-        {-2.3555601314395, 1.29430213886389, 0.179499593411187, 0.00440896450927253,
-         0.00352052300927628, 0.00434187143967281, -9.66615195654703e-05, 3.64923075694275e-05,
-         6.09619017310129e-05, 4.23908862836885e-05},
-        {-0.858019663974047, 1.10138705956076, 0.278789852526915, -0.000199798507752607,
-         0.00112092406838628, -0.00177346866231588, 0.000217816070307086, -0.000240713988238257,
-         0.000150795563555828, -0.000279246491927943},
-        {7.93661480471297, 1.33902098855997, -0.64010306493848, -0.00307944184518557,
-         -0.00511421127083497, 0.0204008636376403, -9.50042323904954e-05, 6.01530207062221e-05,
-         9.13233708460098e-05, -0.000206717750924323},
-        {16.2658124154565, 0.191301571705827, 1.02390350838635, 0.0258487436355216,
-         -0.0219752092833362, 0.0236916776412211, -0.000350496453661261, -0.000123849795280597,
-         -0.000532190902882765, 9.36018171121253e-05},
-        {-1.53023612303052, 1.29132951637076, 0.181159073530008, -0.0023490608317645,
-         -0.00370741703297037, -0.000229071300377431, -1.6634455407558e-05, 1.11387154630828e-05,
-         1.02609175615251e-05, -9.64717658954667e-06},
-        {-32.9918791079688, 0.093536793089853, 4.76858627395571, 0.0595845684553358,
-         -0.054845749101257, -0.133247382500001, -0.000688999201915199, 7.67286265747961e-05,
-         0.000868163357631254, 0.00120099606910313}};
+        {10.4400948050067, 1.38202655603079, 0.975299591736672, 0.0172133914423707,
+         -0.0163482459492803, 0.035730152619911, 0.00021725657060767, -0.000181685375645396,
+         -0.000124096561459262, 0.00040790566176981},
+        {6.38281281805793, 1.81388255990089, 0.28679524291736, 0.0218036823758417,
+         0.010516766426651, 0.000446101708841615, 0.00020187044149361, 0.000114957457831415,
+         1.63114413539632e-05, -2.0187452317724e-05},
+        {-29.3049094555, -0.506844002611835, 1.64911970541112, -0.0336282997119334,
+         0.053185806861685, -0.028164943139695, -0.00021098074590512, 0.000643681643489995,
+         -0.000249094601806692, 0.000231466668650876},
+        {-4.76233790255328, 1.1780710601961, -0.194257188545164, 0.00471817228628384,
+         -0.00183773644319332, -0.00570261621182479, -7.99203367291902e-05, 7.75752247926601e-05,
+         -9.78534772816957e-06, -4.72083745991256e-05},
+        {0.692159025649028, 1.11895461388667, 0.341706834956496, 0.000237989648019541,
+         -0.000188322779563912, 0.000227310789253953, 0.000133001646828401, -0.000305810826248463,
+         0.00010150571088124, -0.000367705461590854},
+        {3.38094203317731, 1.24778838596815, 0.067807236112956, -0.00379395536123526,
+         -0.00339180589343601, -0.00188754615986649, -7.52406312245606e-05, 4.58398750278147e-05,
+         6.97244631313601e-05, 2.50519145070895e-05},
+        {-7.10546287716029, 0.459472977452686, -1.12251049944014, 0.0175406972371191,
+         -0.0310525406867782, -0.0531315970690727, -0.000121107664597462, 0.000544665437051928,
+         -1.78466217018177e-05, -0.00058976234038192},
+        {1.60633684055984, 1.1975095485662, 0.180159204664965, -0.00259157601062089,
+         -0.0038106317634397, 0.000956686555225968, 4.28416721502134e-06, 5.84532336259517e-06,
+         -2.73407888222758e-05, 5.45131881032866e-06},
+        {43.3732235586222, 0.528096786861784, -3.41255850703983, -0.0161629934278675,
+         0.0790998053536612, 0.0743822668655928, 0.000237176965460634, -0.000426691336904078,
+         -0.000889196131314391, -0.000509766491897672}};
     float sus6coeffBeta[9][10] = {
-        {12.7380220453847, -0.6087309901836, 2.60957722462363, -0.0415319939920917,
-         0.0444944768824276, 0.0223231464060241, -0.000421503508733887, -9.39560038638717e-05,
-         0.000821479971871302, -4.5330528329465e-05},
-        {1.96846333975847, -0.33921438143463, 1.23957110477613, -0.00948832495296823,
-         0.00107211134687287, -0.00410820045700199, -9.33679611473279e-05, 3.72984782145427e-05,
-         -4.04514487800062e-05, -7.6296149087237e-05},
-        {5.7454444934481, -1.58476383793609, -0.418479494289251, -0.0985177320630941,
-         -0.0862179276808015, 0.0126762052037897, -0.00118207758271301, -0.000190361442918412,
-         0.0011723869613426, 0.000122882034141316},
-        {2.11042287406433, -0.225942746245056, 1.18084080712528, -0.00103013931607172,
-         -0.00675606790663387, -0.00106646109062746, 1.7708839355979e-05, -3.13642668374253e-05,
-         -5.87601932564404e-05, -3.92033314627704e-05},
-        {2.96049248725882, -0.286261455028255, 1.09122556181319, -0.000672369023155898,
-         0.000574446975796023, 0.000120303729680796, 0.000292285799270644, 0.000170497873487264,
-         0.000259925974231328, 0.000222437797823852},
-        {1.65218061201483, -0.19535446105784, 1.39609640918411, 0.000961524354787167,
-         0.00592400381724333, -0.0078500192096718, -7.02791628080906e-07, -2.07197580883822e-05,
-         -4.33518182614169e-05, 4.66993119419691e-05},
-        {-19.56673237415, 1.06558565338761, 0.151160448373445, -0.0252628659378108,
-         0.0281230551050938, -0.0217328869907185, 0.000241309440918385, -0.000116449585258429,
-         0.000401546410974577, -0.000147563886502726},
-        {1.56167171538684, -0.155299366654736, 1.20084049723279, 0.00457348893890231,
-         0.00118888040006052, 0.0029920178735941, -5.583448120596e-05, -2.34496315691865e-05,
-         -5.3309466243918e-05, 6.20289310356821e-06},
-        {1.95050549495182, -2.74909818412705, 3.80268788018641, 0.0629242254381785,
-         0.0581479035315726, -0.111361283351269, -0.00047845777495158, -0.00075354297736741,
-         -0.000186887396585446, 0.00119710704771344}};
+        {1.03872648284911, -0.213507239271552, 1.43193059498181, -0.000972717820830235,
+         -0.00661046096415371, 0.00974284211491888, 2.96098456891215e-05, -8.2933115634257e-05,
+         -5.52178824394723e-06, 5.73935295303589e-05},
+        {3.42242235823356, 0.0848392511283237, 1.24574390342586, 0.00356248195980133,
+         0.00100415659893053, -0.00460120247716139, 3.84891005422427e-05, 2.70236417852327e-05,
+         -7.58501977656551e-05, -8.79809730730992e-05},
+        {14.0092526123741, 1.03126714946215, 1.0611008563785, 0.04076462444523, 0.0114106419194518,
+         0.00746959159048058, 0.000388033225774727, -0.000124645014888926, -0.000296639947532341,
+         -0.00020861690864945},
+        {1.3562422681189, -0.241585615891602, 1.49170424068611, 0.000179184170448335,
+         -0.00712399257616284, 0.0121433526723498, 3.29770580642447e-05, 8.78960210966787e-06,
+         -6.00508568552101e-05, 0.000101583822589461},
+        {-0.718855428908583, -0.344067476078684, 1.12397093701762, 0.000236505431484729,
+         -0.000406441415248947, 0.00032834991502413, 0.000359422093285086, 8.18895560425272e-05,
+         0.000316835483508523, 0.000151442890664899},
+        {-0.268764016434841, -0.275272048639511, 1.26239753050527, -0.000511224336925231,
+         0.0095628568270856, -0.00397960092451418, 1.39587366293607e-05, 1.31409051361129e-05,
+         -9.83662017231755e-05, 1.87078667116619e-05},
+        {27.168106989145, -2.43346872338192, 1.91135512970771, 0.0553180826818016,
+         -0.0481878292619383, 0.0052773235604729, -0.000428011927975304, 0.000528018208222772,
+         -0.000285438191474895, -5.71327627917386e-05},
+        {-0.169494136517622, -0.350851545482921, 1.19922076033643, 0.0101120903675328,
+         -0.00151674465424115, 0.00548694086125656, -0.000108240000970513, 1.57202185024105e-05,
+         -9.77555098179959e-05, 2.09624089449761e-05},
+        {-32.3807957489507, 1.8271436443167, 2.51530814328123, -0.0532334586403461,
+         -0.0355980127727253, -0.0213373892796204, 0.00045506092539885, 0.000545065581027688,
+         0.000141998709314758, 0.000101051304611037}};
     float sus7coeffAlpha[9][10] = {
         {-92.1126183408754, -3.77261746189525, -4.50604668349213, -0.0909560776043523,
          -0.15646903318971, -0.0766293642415356, -0.00059452135473577, -0.00144790037129283,
@@ -749,9 +764,6 @@ class AcsParameters /*: public HasParametersIF*/ {
         {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136,
          0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
          -0.000889232196185857, -0.00168429567131815}};
-
-    float filterAlpha;
-    float sunThresh;
   } susHandlingParameters;
 
   struct GyrHandlingParameters {
@@ -759,110 +771,119 @@ class AcsParameters /*: public HasParametersIF*/ {
     double gyr1orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
     double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
     double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
-    // var = sqrt(sigma), 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 gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004};
+    double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359};
+    double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
+    double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
+
+    /* var = sqrt(sigma), 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 */
     float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
                               pow(3.0e-3 * sqrt(2), 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
                               pow(4.3e-3 * sqrt(2), 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)};
-    enum PreferAdis { NO = 0, YES = 1 };
-    uint8_t preferAdis = PreferAdis::YES;
+    uint8_t preferAdis = true;
   } gyrHandlingParameters;
 
   struct RwHandlingParameters {
-    double rw0orientationMatrix[3][3];
-    double rw1orientationMatrix[3][3];
-    double rw2orientationMatrix[3][3];
-    double rw3orientationMatrix[3][3];
     double inertiaWheel = 0.000028198;
-    double maxTrq = 0.0032;  // 3.2 [mNm]
+    double maxTrq = 0.0032;             // 3.2 [mNm]
+    double stictionSpeed = 100;         // 80;          // RPM
+    double stictionReleaseSpeed = 120;  // RPM
+    double stictionTorque = 0.0006;
   } 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 pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597},
-                                  {0.2136, -0.3317, 1.0123},
-                                  {-0.8672, -0.1406, 0.1778},
-                                  {0.6426, 0.4794, 1.3603}};
-    double without0[4][3];
-    double without1[4][3];
-    double without2[4][3];
-    double without3[4][3];
-    double nullspace[4] = {-0.7358, 0.5469, -0.3637, -0.1649};
+    double pseudoInverse[4][3] = {
+        {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
+    double without0[4][3] = {
+        {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
+    double without1[4][3] = {
+        {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
+    double without2[4][3] = {
+        {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
+    double without3[4][3] = {
+        {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
+    double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
   } rwMatrices;
 
   struct SafeModeControllerParameters {
     double k_rate_mekf = 0.00059437;
     double k_align_mekf = 0.000056875;
 
-    double k_rate_no_mekf;
-    double k_align_no_mekf;
-    double sunMagAngleMin;
+    double k_rate_no_mekf = 0.00059437;
+    double k_align_no_mekf = 0.000056875;
 
-    double sunTargetDir[3] = {1, 0, 0};  // Body frame
-    double satRateRef[3];                // Body frame
+    double sunMagAngleMin = 5 * M_PI / 180;
 
+    double sunTargetDirLeop[3] = {0, .5, .5};
+    double sunTargetDir[3] = {0, 0, 1};
+
+    double satRateRef[3] = {0, 0, 0};
   } safeModeControllerParameters;
 
-  struct DetumbleCtrlParameters {
-    double gainD = pow(10.0, -3.3);
-
-  } detumbleCtrlParameters;
-
-  // ToDo: mutiple structs for different pointing mode controllers?
-  struct PointingModeControllerParameters {
-    double updtFlag;
-    double A_rw[3][12];
-
-    double refDirection[3] = {1, 0, 0};
-    double refRotRate[3] = {0, 0, 0};
-    double quatRef[4] = {0, 0, 0, 1};
-    bool avoidBlindStr = true;
-    double blindAvoidStart = 1.5;
-    double blindAvoidStop = 2.5;
-    double blindRotRate = 1 * M_PI / 180;
-
+  struct PointingLawParameters {
     double zeta = 0.3;
-    double zetaLow;
     double om = 0.3;
-    double omLow;
     double omMax = 1 * M_PI / 180;
     double qiMin = 0.1;
     double gainNullspace = 0.01;
 
     double desatMomentumRef[3] = {0, 0, 0};
     double deSatGainFactor = 1000;
-    bool desatOn = true;
+    uint8_t desatOn = true;
+    uint8_t enableAntiStiction = true;
+  } pointingLawParameters;
 
-    double omegaEarth = 0.000072921158553;
+  struct TargetModeControllerParameters : PointingLawParameters {
+    double refDirection[3] = {-1, 0, 0};  // Antenna
+    double refRotRate[3] = {0, 0, 0};     // Not used atm, do we want an option to
+    // give this as an input- currently en calculation is done
+    double quatRef[4] = {0, 0, 0, 1};
+    int8_t timeElapsedMax = 10;  // rot rate calculations
 
-  } inertialModeControllerParameters, nadirModeControllerParameters, targetModeControllerParameters;
+    // Default is Stuttgart GS
+    double latitudeTgt = 48.7495 * M_PI / 180.;   // [rad] Latitude
+    double longitudeTgt = 9.10384 * M_PI / 180.;  // [rad] Longitude
+    double altitudeTgt = 500;                     // [m]
+
+    // For one-axis control:
+    uint8_t avoidBlindStr = true;
+    double blindAvoidStart = 1.5;
+    double blindAvoidStop = 2.5;
+    double blindRotRate = 1 * M_PI / 180;
+  } targetModeControllerParameters;
+
+  struct NadirModeControllerParameters : PointingLawParameters {
+    double refDirection[3] = {-1, 0, 0};  // Antenna
+    double quatRef[4] = {0, 0, 0, 1};
+    int8_t timeElapsedMax = 10;  // rot rate calculations
+  } nadirModeControllerParameters;
+
+  struct InertialModeControllerParameters : PointingLawParameters {
+    double tgtQuat[4] = {0, 0, 0, 1};
+    double refRotRate[3] = {0, 0, 0};
+    double quatRef[4] = {0, 0, 0, 1};
+  } inertialModeControllerParameters;
 
   struct StrParameters {
     double exclusionAngle = 20 * M_PI / 180;
-    //		double strOrientationMatrix[3][3];
-    double boresightAxis[3] = {0.7593, 0.0000, -0.6508};  // in body/geometry frame
+    double boresightAxis[3] = {0.7593, 0.0000, -0.6508};  // geometry frame
   } strParameters;
 
   struct GpsParameters {
+    double timeDiffVelocityMax = 30;  //[s]
   } gpsParameters;
 
-  struct GroundStationParameters {
-    double latitudeGs = 48.7495 * M_PI / 180.;   // [rad] Latitude
-    double longitudeGs = 9.10384 * M_PI / 180.;  // [rad] Longitude
-    double altitudeGs = 500;                     // [m] Altitude
-    double earthRadiusEquat = 6378137;           // [m]
-    double earthRadiusPolar = 6356752.314;       // [m]
-  } groundStationParameters;                     // Stuttgart
-
   struct SunModelParameters {
-    enum UseSunModel { NO = 0, YES = 3 };
-    uint8_t useSunModel;
     float domega = 36000.771;
-    float omega_0 = 282.94 * M_PI / 180.;  // RAAN plus argument of perigee
-    float m_0 = 357.5256;                  // coefficients for mean anomaly
+    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.;
@@ -872,19 +893,13 @@ class AcsParameters /*: public HasParametersIF*/ {
   } sunModelParameters;
 
   struct KalmanFilterParameters {
-    uint8_t activateKalmanFilter;
-    uint8_t requestResetFlag;
-    double maxToleratedTimeBetweenKalmanFilterExecutionSteps;
-    double processNoiseOmega[3];
-    double processNoiseQuaternion[4];
-
     double sensorNoiseSTR = 0.1 * M_PI / 180;
     double sensorNoiseSS = 8 * M_PI / 180;
     double sensorNoiseMAG = 4 * M_PI / 180;
-    double sensorNoiseRMU[3];
+    double sensorNoiseGYR = 0.1 * M_PI / 180;
 
-    double sensorNoiseArwRmu;  // Angular Random Walk
-    double sensorNoiseBsRMU;   // 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 MagnetorquesParameter {
@@ -899,8 +914,8 @@ class AcsParameters /*: public HasParametersIF*/ {
 
   struct DetumbleParameter {
     uint8_t detumblecounter = 75;  // 30 s
-    double omegaDetumbleStart = 2;
-    double omegaDetumbleEnd = 0.4;
+    double omegaDetumbleStart = 2 * M_PI / 180;
+    double omegaDetumbleEnd = 0.4 * M_PI / 180;
     double gainD = pow(10.0, -3.3);
   } detumbleParameter;
 };
diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp
index 771f9d6f..4e0052e0 100644
--- a/mission/controller/acs/ActuatorCmd.cpp
+++ b/mission/controller/acs/ActuatorCmd.cpp
@@ -21,26 +21,27 @@ ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsPa
 
 ActuatorCmd::~ActuatorCmd() {}
 
-void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
-                                const int32_t *speedRw2, const int32_t *speedRw3,
-                                const double *rwTrqIn, const double *rwTrqNS, double *rwCmdSpeed) {
-  using namespace Math;
+void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
   // Scaling the commanded torque to a maximum value
-  double torque[4] = {0, 0, 0, 0};
   double maxTrq = acsParameters.rwHandlingParameters.maxTrq;
-  VectorOperations<double>::add(rwTrqIn, rwTrqNS, torque, 4);
 
   double maxValue = 0;
   for (int i = 0; i < 4; i++) {  // size of torque, always 4 ?
-    if (abs(torque[i]) > maxValue) {
-      maxValue = abs(torque[i]);
+    if (abs(rwTrq[i]) > maxValue) {
+      maxValue = abs(rwTrq[i]);
     }
   }
 
   if (maxValue > maxTrq) {
     double scalingFactor = maxTrq / maxValue;
-    VectorOperations<double>::mulScalar(torque, scalingFactor, torque, 4);
+    VectorOperations<double>::mulScalar(rwTrq, scalingFactor, rwTrqScaled, 4);
   }
+}
+
+void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
+                                const int32_t *speedRw2, const int32_t *speedRw3,
+                                const double *rwTorque, double *rwCmdSpeed) {
+  using namespace Math;
 
   // Calculating the commanded speed in RPM for every reaction wheel
   double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
@@ -50,7 +51,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1
   double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
   //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
   double factor = commandTime / inertiaWheel * radToRpm;
-  VectorOperations<double>::mulScalar(torque, factor, deltaSpeed, 4);
+  VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
   VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
 }
 
diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h
index 181bf175..04e2b61f 100644
--- a/mission/controller/acs/ActuatorCmd.h
+++ b/mission/controller/acs/ActuatorCmd.h
@@ -1,10 +1,3 @@
-/*
- * ActuatorCmd.h
- *
- *  Created on: 4 Aug 2022
- *      Author: Robin Marquardt
- */
-
 #ifndef ACTUATORCMD_H_
 #define ACTUATORCMD_H_
 
@@ -18,6 +11,14 @@ class ActuatorCmd {
   ActuatorCmd(AcsParameters *acsParameters_);  // Input mode ?
   virtual ~ActuatorCmd();
 
+  /*
+   * @brief:	scalingTorqueRws()	scales the torque via maximum part in case this part is
+   * higher then the maximum torque
+   * @param:	rwTrq	 			given torque for reaction wheels
+   * 			rwTrqScaled			possible scaled torque
+   */
+  void scalingTorqueRws(const double *rwTrq, double *rwTrqScaled);
+
   /*
    * @brief:	cmdSpeedToRws()		will set the maximum possible torque for the reaction
    * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
@@ -28,8 +29,7 @@ class ActuatorCmd {
    * reaction wheel
    */
   void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
-                     const int32_t *speedRw3, const double *rwTrqIn, const double *rwTrqNS,
-                     double *rwCmdSpeed);
+                     const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed);
 
   /*
    * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index 35a7295a..383a265f 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -13,6 +13,8 @@
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <math.h>
 
+#include <filesystem>
+
 #include "string.h"
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
@@ -22,57 +24,53 @@ Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParamete
 Guidance::~Guidance() {}
 
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
-  for (int i = 0; i < 3; i++) {
-    sunTargetSafe[i] = acsParameters.safeModeControllerParameters.sunTargetDir[i];
-    satRateSafe[i] = acsParameters.safeModeControllerParameters.satRateRef[i];
+  if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
+      not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) {  // ToDo: if file does not exist anymore
+    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
+                3 * sizeof(double));
+  } else {
+    std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
+                3 * sizeof(double));
   }
-
-  //	memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);
+  std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
+              3 * sizeof(double));
 }
 
-void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                             acsctrl::SusDataProcessed *susDataProcessed, timeval now,
-                             double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                                       acsctrl::SusDataProcessed *susDataProcessed,
+                                       acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
+                                       double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion to groundstation
+  //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
   //-------------------------------------------------------------------------------------
-  //	Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth
+  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
   // fixed/centered frame)
-  double groundStationCart[3] = {0, 0, 0};
+  double targetCart[3] = {0, 0, 0};
 
-  MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
-                                                  acsParameters.groundStationParameters.longitudeGs,
-                                                  acsParameters.groundStationParameters.altitudeGs,
-                                                  groundStationCart);
+  MathOperations<double>::cartesianFromLatLongAlt(
+      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
 
   //	Position of the satellite in the earth/fixed frame via GPS
   double posSatE[3] = {0, 0, 0};
-  MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value,
-                                                  sensorValues->gpsSet.longitude.value,
+  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
+  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
+  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
                                                   sensorValues->gpsSet.altitude.value, posSatE);
 
   //	Target direction in the ECEF frame
   double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
+  VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
 
   //	Transformation between ECEF and IJK frame
   double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::dcmEJ(now, *dcmEJ);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
-  //	Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
   double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+
   double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
-  double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth;
-
-  //	TEST SECTION !
-  // double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  // MatrixOperations<double>::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix,
-  // dcmTEST, dcmTEST, 3, 3, 3);
-
-  MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
-  MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
   MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
 
   //	Transformation between ECEF and Body frame
@@ -111,9 +109,7 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
   //	Calculation of reference rotation rate
   //-------------------------------------------------------------------------------------
   double velSatE[3] = {0, 0, 0};
-  velSatE[0] = 0.0;  // sensorValues->gps0Velocity[0];
-  velSatE[1] = 0.0;  // sensorValues->gps0Velocity[1];
-  velSatE[2] = 0.0;  // sensorValues->gps0Velocity[2];
+  std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
   double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
   //	Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
   MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
@@ -134,10 +130,10 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
   //	Calculation of reference rotation rate in case of star tracker blinding
   //-------------------------------------------------------------------------------------
   if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
-    double sunDirJ[3] = {0, 0, 0};
     double sunDirB[3] = {0, 0, 0};
 
     if (susDataProcessed->sunIjkModel.isValid()) {
+      double sunDirJ[3] = {0, 0, 0};
       std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
       MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
     } else {
@@ -183,14 +179,414 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData
   }
 }
 
-void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
-                          double quatErrorComplete[4], double quatError[3], double deltaRate[3]) {
-  double quatRef[4] = {0, 0, 0, 0};
-  quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
-  quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
-  quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
-  quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];
+void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+                               double *refSatRate) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of reference rotation rate
+  //-------------------------------------------------------------------------------------
+  double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
+                       (timeSavedQuaternion.tv_sec +
+                        timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
+  if (timeElapsed < timeElapsedMax) {
+    double qDiff[4] = {0, 0, 0, 0};
+    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
+    VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
 
+    double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
+           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(quatInertialTarget, qDiff, sum1);
+    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
+    VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[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];
+}
+
+void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
+                                      acsctrl::GpsDataProcessed *gpsDataProcessed,
+                                      acsctrl::MekfData *mekfData, timeval now,
+                                      double targetQuat[4], double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for target pointing
+  //-------------------------------------------------------------------------------------
+  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
+  // fixed/centered frame)
+  double targetCart[3] = {0, 0, 0};
+
+  MathOperations<double>::cartesianFromLatLongAlt(
+      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
+  //	Position of the satellite in the earth/fixed frame via GPS
+  double posSatE[3] = {0, 0, 0};
+  std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
+
+  //	Transformation between ECEF and IJK frame
+  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+
+  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+
+  //	Target Direction and position vector in the inertial frame
+  double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
+
+  // negative x-Axis aligned with target (Camera/E-band transmitter position)
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
+  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+
+  // Transform velocity into inertial frame
+  double velocityE[3];
+  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
+  double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
+  VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
+
+  // orbital normal vector
+  double orbitalNormalJ[3] = {0, 0, 0};
+  VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
+  VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
+
+  // y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
+  double yAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+
+  // z-Axis completes RHS
+  double zAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(xAxis, yAxis, zAxis);
+
+  // Complete transformation matrix
+  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                         {xAxis[1], yAxis[1], zAxis[1]},
+                         {xAxis[2], yAxis[2], zAxis[2]}};
+  double quatInertialTarget[4] = {0, 0, 0, 0};
+  QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
+
+  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
+
+  // Transform in system relative to satellite frame
+  double quatBJ[4] = {0, 0, 0, 0};
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
+  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
+}
+
+void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                               acsctrl::SusDataProcessed *susDataProcessed,
+                               acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
+                               double targetQuat[4], double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for ground station pointing
+  //-------------------------------------------------------------------------------------
+  //	Transform longitude, latitude and altitude to cartesian coordiantes (earth
+  // fixed/centered frame)
+  double groundStationCart[3] = {0, 0, 0};
+
+  MathOperations<double>::cartesianFromLatLongAlt(
+      acsParameters.targetModeControllerParameters.latitudeTgt,
+      acsParameters.targetModeControllerParameters.longitudeTgt,
+      acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
+  //	Position of the satellite in the earth/fixed frame via GPS
+  double posSatE[3] = {0, 0, 0};
+  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
+  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
+  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
+                                                  sensorValues->gpsSet.altitude.value, posSatE);
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
+
+  //	Transformation between ECEF and IJK frame
+  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+
+  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+
+  //	Target Direction and position vector in the inertial frame
+  double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
+
+  // negative x-Axis aligned with target (Camera/E-band transmitter position)
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
+  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+
+  // get Sun Vector Model in ECI
+  double sunJ[3];
+  std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
+  VectorOperations<double>::normalize(sunJ, sunJ, 3);
+
+  // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
+  // z = sPerpenticular =  s - sParallel = s - (x*s)/norm(x)^2 * x
+  double xDotS = VectorOperations<double>::dot(xAxis, sunJ);
+  xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
+  double sunParallel[3], zAxis[3];
+  VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
+  VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
+  VectorOperations<double>::normalize(zAxis, zAxis, 3);
+
+  // calculate y-axis
+  double yAxis[3];
+  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+
+  // Complete transformation matrix
+  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                         {xAxis[1], yAxis[1], zAxis[1]},
+                         {xAxis[2], yAxis[2], zAxis[2]}};
+  double quatInertialTarget[4] = {0, 0, 0, 0};
+  QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
+
+  int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
+
+  // Transform in system relative to satellite frame
+  double quatBJ[4] = {0, 0, 0, 0};
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
+  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
+}
+
+void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                          acsctrl::SusDataProcessed *susDataProcessed,
+                          acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
+                          double targetQuat[4], double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion to sun
+  //-------------------------------------------------------------------------------------
+  double quatBJ[4] = {0, 0, 0, 0};
+  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
+  QuaternionOperations::toDcm(quatBJ, dcmBJ);
+
+  double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
+  if (susDataProcessed->sunIjkModel.isValid()) {
+    std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
+    MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
+  } else if (susDataProcessed->susVecTot.isValid()) {
+    std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
+  } else {
+    return;
+  }
+
+  //	Transformation between ECEF and IJK frame
+  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+
+  // positive z-Axis of EIVE in direction of sun
+  double zAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(sunDirB, zAxis, 3);
+
+  // Assign helper vector (north pole inertial)
+  double helperVec[3] = {0, 0, 1};
+
+  //
+  double yAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxis, helperVec, yAxis);
+  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+
+  //
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(yAxis, zAxis, xAxis);
+  VectorOperations<double>::normalize(xAxis, xAxis, 3);
+
+  // Transformation matrix to Sun, no further transforamtions, reference is already
+  // the EIVE body frame
+  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                         {xAxis[1], yAxis[1], zAxis[1]},
+                         {xAxis[2], yAxis[2], zAxis[2]}};
+  double quatSun[4] = {0, 0, 0, 0};
+  QuaternionOperations::fromDcm(dcmTgt, quatSun);
+
+  targetQuat[0] = quatSun[0];
+  targetQuat[1] = quatSun[1];
+  targetQuat[2] = quatSun[2];
+  targetQuat[3] = quatSun[3];
+
+  //----------------------------------------------------------------------------
+  //	Calculation of reference rotation rate
+  //----------------------------------------------------------------------------
+  refSatRate[0] = 0;
+  refSatRate[1] = 0;
+  refSatRate[2] = 0;
+}
+
+void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                                      timeval now, double targetQuat[4],
+                                      double refSatRate[3]) {  // old version of Nadir Pointing
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for Nadir pointing
+  //-------------------------------------------------------------------------------------
+  //	Position of the satellite in the earth/fixed frame via GPS
+  double posSatE[3] = {0, 0, 0};
+  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
+  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
+  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
+                                                  sensorValues->gpsSet.altitude.value, posSatE);
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
+
+  //	Transformation between ECEF and IJK frame
+  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+
+  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+
+  //	Transformation between ECEF and Body frame
+  double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double quatBJ[4] = {0, 0, 0, 0};
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
+  QuaternionOperations::toDcm(quatBJ, dcmBJ);
+  MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
+
+  //	Target Direction in the body frame
+  double targetDirB[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
+
+  //	rotation quaternion from two vectors
+  double refDir[3] = {0, 0, 0};
+  refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
+  refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
+  refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
+  double noramlizedTargetDirB[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
+  VectorOperations<double>::normalize(refDir, refDir, 3);
+  double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
+  double normRefDir = VectorOperations<double>::norm(refDir, 3);
+  double crossDir[3] = {0, 0, 0};
+  double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
+  VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
+  targetQuat[0] = crossDir[0];
+  targetQuat[1] = crossDir[1];
+  targetQuat[2] = crossDir[2];
+  targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
+  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
+
+  //-------------------------------------------------------------------------------------
+  //	Calculation of reference rotation rate
+  //-------------------------------------------------------------------------------------
+  refSatRate[0] = 0;
+  refSatRate[1] = 0;
+  refSatRate[2] = 0;
+}
+
+void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
+                                     acsctrl::GpsDataProcessed *gpsDataProcessed,
+                                     acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
+                                     double refSatRate[3]) {
+  //-------------------------------------------------------------------------------------
+  //	Calculation of target quaternion for Nadir pointing
+  //-------------------------------------------------------------------------------------
+  //	Position of the satellite in the earth/fixed frame via GPS
+  double posSatE[3] = {0, 0, 0};
+  double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
+  double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
+  MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
+                                                  sensorValues->gpsSet.altitude.value, posSatE);
+  double targetDirE[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
+
+  //	Transformation between ECEF and IJK frame
+  double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
+
+  double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
+
+  //	Target Direction in the body frame
+  double targetDirJ[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
+
+  // negative x-Axis aligned with target (Camera position)
+  double xAxis[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
+  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+
+  // z-Axis parallel to long side of picture resolution
+  double zAxis[3] = {0, 0, 0}, velocityE[3];
+  std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
+  double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
+  MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
+  VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
+  VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
+  VectorOperations<double>::normalize(zAxis, zAxis, 3);
+
+  // y-Axis completes RHS
+  double yAxis[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
+
+  // Complete transformation matrix
+  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
+                         {xAxis[1], yAxis[1], zAxis[1]},
+                         {xAxis[2], yAxis[2], zAxis[2]}};
+  double quatInertialTarget[4] = {0, 0, 0, 0};
+  QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
+
+  int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
+  refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
+
+  // Transform in system relative to satellite frame
+  double quatBJ[4] = {0, 0, 0, 0};
+  std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
+  QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
+}
+
+void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
+  std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
+              4 * sizeof(double));
+  std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
+              3 * sizeof(double));
+}
+
+void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
+                          double refSatRate[3], double quatErrorComplete[4], double quatError[3],
+                          double deltaRate[3]) {
   double satRate[3] = {0, 0, 0};
   std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
   VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
@@ -210,8 +606,8 @@ void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, dou
   quatError[1] = quatErrorComplete[1];
   quatError[2] = quatErrorComplete[2];
 
-  // target flag in matlab, importance, does look like it only gives
-  //	feedback if pointing control is under 150 arcsec ??
+  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
+  // under 150 arcsec ??
 }
 
 void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) {
@@ -300,7 +696,7 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *
     // 	    @note: This one takes the normal pseudoInverse of all four raction wheels valid.
     //		Does not make sense, but is implemented that way in MATLAB ?!
     //		Thought: It does not really play a role, because in case there are more then one
-    //		reaction wheel the pointing control is destined to fail.
+    //		reaction wheel invalid the pointing control is destined to fail.
     rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
     rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
     rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h
index 4c699561..0401d07c 100644
--- a/mission/controller/acs/Guidance.h
+++ b/mission/controller/acs/Guidance.h
@@ -21,16 +21,49 @@ class Guidance {
 
   void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]);
 
-  // Function to get the target quaternion and refence rotation rate from gps position and position
-  // of the ground station
-  void targetQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
-                     acsctrl::SusDataProcessed *susDataProcessed, timeval now, double targetQuat[4],
-                     double refSatRate[3]);
+  // Function to get the target quaternion and refence rotation rate from gps position and
+  // position of the ground station
+  void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
+                              acsctrl::GpsDataProcessed *gpsDataProcessed,
+                              acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
+                              double refSatRate[3]);
+  void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                       acsctrl::SusDataProcessed *susDataProcessed,
+                       acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
+                       double targetQuat[4], double refSatRate[3]);
+  void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                               acsctrl::SusDataProcessed *susDataProcessed,
+                               acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
+                               double targetQuat[4], double refSatRate[3]);
+
+  // Function to get the target quaternion and refence rotation rate for sun pointing after ground
+  // station
+  void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                  acsctrl::SusDataProcessed *susDataProcessed,
+                  acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4],
+                  double refSatRate[3]);
+
+  // Function to get the target quaternion and refence rotation rate from gps position for Nadir
+  // pointing
+  void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
+                             acsctrl::GpsDataProcessed *gpsDataProcessed,
+                             acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
+                             double refSatRate[3]);
+  void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
+                              timeval now, double targetQuat[4], double refSatRate[3]);
+
+  // Function to get the target quaternion and refence rotation rate from parameters for inertial
+  // pointing
+  void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);
 
   // @note: compares target Quaternion and reference quaternion, also actual satellite rate and
   // desired
-  void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double refSatRate[3],
-                  double quatErrorComplete[4], double quatError[3], double deltaRate[3]);
+  void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
+                  double refSatRate[3], double quatErrorComplete[4], double quatError[3],
+                  double deltaRate[3]);
+
+  void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
+                       double *refSatRate);
 
   //	@note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
   // reation wheel 	maybe can be done in "commanding.h"
@@ -39,6 +72,12 @@ class Guidance {
  private:
   AcsParameters acsParameters;
   bool strBlindAvoidFlag = false;
+  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/deployment";
+  static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/deployment";
 };
 
 #endif /* ACS_GUIDANCE_H_ */
diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp
index fcd95b68..63992010 100644
--- a/mission/controller/acs/Igrf13Model.cpp
+++ b/mission/controller/acs/Igrf13Model.cpp
@@ -1,22 +1,19 @@
-/*
- * Igrf13Model.cpp
- *
- *  Created on: 10 Mar 2022
- *      Author: Robin Marquardt
- */
-
 #include "Igrf13Model.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 <math.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() {}
 
@@ -25,7 +22,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
                                double* magFieldModelInertial) {
   double phi = longitude, theta = gcLatitude;  // geocentric
   /* Here is the co-latitude needed*/
-  theta -= 90 * Math::PI / 180;
+  theta -= 90 * PI / 180;
   theta *= (-1);
 
   double rE = 6371200.0;  // radius earth [m]
@@ -43,7 +40,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
         /* Calculation of Legendre Polynoms (normalised) */
         if (n == m) {
           P2 = sin(theta) * P11;
-          dP2 = sin(theta) * dP11 - cos(theta) * P11;
+          dP2 = sin(theta) * dP11 + cos(theta) * P11;
           P11 = P2;
           P10 = P11;
           P20 = 0;
@@ -70,11 +67,11 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
         magFieldModel[0] +=
             pow(rE / (altitude + rE), (n + 2)) * (n + 1) *
             ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * P2);
-        /* gradient of scalar potential towards phi */
+        /* gradient of scalar potential towards theta */
         magFieldModel[1] +=
             pow(rE / (altitude + rE), (n + 2)) *
             ((updatedG[m][n - 1] * cos(m * phi) + updatedH[m][n - 1] * sin(m * phi)) * dP2);
-        /* gradient of scalar potential towards theta */
+        /* gradient of scalar potential towards phi */
         magFieldModel[2] +=
             pow(rE / (altitude + rE), (n + 2)) *
             ((-updatedG[m][n - 1] * sin(m * phi) + updatedH[m][n - 1] * cos(m * phi)) * P2 * m);
@@ -85,31 +82,30 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
   magFieldModel[1] *= -1;
   magFieldModel[2] *= (-1 / sin(theta));
 
-  /* Next step: transform into inertial KOS (IJK)*/
-  // Julean Centuries
-  double JD2000Floor = 0;
   double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
-  JD2000Floor = floor(JD2000);
-  double JC2000 = JD2000Floor / 36525;
-
-  double gst = 100.4606184 + 36000.77005361 * JC2000 + 0.00038793 * pow(JC2000, 2) -
-               0.000000026 * pow(JC2000, 3);  // greenwich sidereal time
-  gst *= PI / 180;                            // convert to radians
-  double sec =
-      (JD2000 - JD2000Floor) * 86400;  // Seconds on this day (Universal time) // FROM GPS ?
-  double omega0 = 0.00007292115;       // mean angular velocity earth [rad/s]
-  gst += omega0 * sec;
+  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 *= PI / 180.;
   double lst = gst + longitude;  // local sidereal time [rad]
 
-  magFieldModelInertial[0] = magFieldModel[0] * cos(theta) +
-                             magFieldModel[1] * sin(theta) * cos(lst) - magFieldModel[1] * sin(lst);
-  magFieldModelInertial[1] = magFieldModel[0] * cos(theta) +
-                             magFieldModel[1] * sin(theta) * sin(lst) + magFieldModel[1] * cos(lst);
-  magFieldModelInertial[2] = magFieldModel[0] * sin(theta) + magFieldModel[1] * cos(lst);
+  magFieldModelInertial[0] =
+      (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * cos(lst) -
+      magFieldModel[2] * sin(lst);
+  magFieldModelInertial[1] =
+      (magFieldModel[0] * cos(gcLatitude) + magFieldModel[1] * sin(gcLatitude)) * sin(lst) +
+      magFieldModel[2] * cos(lst);
+  magFieldModelInertial[2] =
+      magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude);
 
   double normVecMagFieldInert[3] = {0, 0, 0};
   VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3);
+
+  magFieldModel[0] = 0;
+  magFieldModel[1] = 0;
+  magFieldModel[2] = 0;
 }
 
 void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
@@ -123,3 +119,34 @@ void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
     }
   }
 }
+
+void Igrf13Model::schmidtNormalization() {
+  double kronDelta = 0;
+  schmidtFactors[0][0] = 1;
+  for (int n = 1; n <= igrfOrder; n++) {
+    if (n == 1) {
+      schmidtFactors[0][n - 1] = 1;
+    } else {
+      schmidtFactors[0][n - 1] = schmidtFactors[0][n - 2] * (2 * n - 1) / n;
+    }
+    for (int m = 1; m <= igrfOrder; m++) {
+      if (m == 1) {
+        kronDelta = 1;
+      } else {
+        kronDelta = 0;
+      }
+      schmidtFactors[m][n - 1] =
+          schmidtFactors[m - 1][n - 1] * sqrt((n - m + 1) * (kronDelta + 1) / (n + m));
+    }
+  }
+
+  for (int i = 0; i <= igrfOrder; i++) {
+    for (int j = 0; j <= (igrfOrder - 1); j++) {
+      coeffG[i][j] = schmidtFactors[i][j] * coeffG[i][j];
+      coeffH[i][j] = schmidtFactors[i][j] * coeffH[i][j];
+
+      svG[i][j] = schmidtFactors[i][j] * svG[i][j];
+      svH[i][j] = schmidtFactors[i][j] * svH[i][j];
+    }
+  }
+}
diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h
index aa7e8b73..ea2c1044 100644
--- a/mission/controller/acs/Igrf13Model.h
+++ b/mission/controller/acs/Igrf13Model.h
@@ -43,14 +43,15 @@ class Igrf13Model /*:public HasParametersIF*/ {
    * - timeOfMagMeasurement: time of actual measurement [s]
    *
    * Outputs:
-   * - magFieldModelInertial: Magnetic Field Vector in IJK KOS [nT]*/
+   * - magFieldModelInertial: Magnetic Field Vector in IJK RF [nT]*/
 
   // Coefficient wary over year, could be updated sometimes.
   void updateCoeffGH(timeval timeOfMagMeasurement);  // Secular variation (SV)
   double magFieldModel[3];
+  void schmidtNormalization();
 
  private:
-  const double coeffG[14][13] = {
+  double coeffG[14][13] = {
       {-29404.8, -2499.6, 1363.2, 903.0, -234.3, 66.0, 80.6, 23.7, 5.0, -1.9, 3.0, -2.0, 0.1},
       {-1450.9, 2982.0, -2381.2, 809.5, 363.2, 65.5, -76.7, 9.7, 8.4, -6.2, -1.4, -0.1, -0.9},
       {0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2, -17.6, 2.9, -0.1, -2.5, 0.5, 0.5},
@@ -66,7 +67,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
       {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -0.5},
       {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.4}};  // [m][n] in nT
 
-  const double coeffH[14][13] = {
+  double coeffH[14][13] = {
       {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},
       {4652.5, -2991.6, -82.1, 281.9, 47.7, -19.1, -51.5, 8.4, -23.4, 3.4, 0.0, -1.2, -0.9},
       {0.0, -734.6, 241.9, -158.4, 208.3, 25.1, -16.9, -15.3, 11.0, -0.2, 2.5, 0.5, 0.6},
@@ -82,7 +83,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
       {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.4},
       {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.6}};  // [m][n] in nT
 
-  const double svG[14][13] = {
+  double svG[14][13] = {
       {5.7, -11.0, 2.2, -1.2, -0.3, -0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
       {7.4, -7.0, -5.9, -1.6, 0.5, -0.3, -0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
       {0.0, -2.1, 3.1, -5.9, -0.6, 0.4, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
@@ -98,7 +99,7 @@ class Igrf13Model /*:public HasParametersIF*/ {
       {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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};  // [m][n] in nT
 
-  const double svH[14][13] = {
+  double svH[14][13] = {
       {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},
       {-25.9, -30.2, 6.0, -0.1, 0.0, 0.0, 0.6, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
       {0.0, -22.4, -1.1, 6.5, 2.5, -1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
@@ -114,6 +115,16 @@ class Igrf13Model /*:public HasParametersIF*/ {
       {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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};  // [m][n] in nT
 
+  double schmidtFactors[14][13] = {
+      {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, 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, 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, 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, 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, 0, 0}};
+  ;
+
   double updatedG[14][13];
   double updatedH[14][13];
   static const int igrfOrder = 13;  // degree of truncation
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index 199440d5..f54893a2 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -1,10 +1,3 @@
-/*
- * MultiplicativeKalmanFilter.cpp
- *
- *  Created on: 4 Feb 2022
- *      Author: rooob
- */
-
 #include "MultiplicativeKalmanFilter.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>
@@ -14,6 +7,8 @@
 #include <stdio.h>
 #include <string.h>
 
+#include <cmath>
+
 #include "util/CholeskyDecomposition.h"
 #include "util/MathOperations.h"
 
@@ -29,7 +24,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
 
 void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_) {
   inertiaEIVE = &(acsParameters_->inertiaEIVE);
-  kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters); /*Sensor noises also here*/
+  kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
 }
 
 void MultiplicativeKalmanFilter::reset() {}
@@ -41,14 +36,11 @@ void MultiplicativeKalmanFilter::init(
   // check for valid mag/sun
   if (validMagField_ && validSS && validSSModel && validMagModel) {
     validInit = true;
-    // AcsParameters mekfEstParams;
-    // loadAcsParameters(&mekfEstParams);
     //  QUEST ALGO -----------------------------------------------------------------------
     double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
     sigmaSun = kalmanFilterParameters->sensorNoiseSS;
     sigmaMag = kalmanFilterParameters->sensorNoiseMAG;
-
-    sigmaGyro = 0.1 * 3.141 / 180;  // acs parameters
+    sigmaGyro = kalmanFilterParameters->sensorNoiseGYR;
 
     double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
            normSunJ[3] = {0, 0, 0};
@@ -136,7 +128,6 @@ void MultiplicativeKalmanFilter::init(
            matrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
            matrixSunMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
            matrixMagSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-    /* vector*transpose(vector)*/
     MatrixOperations<double>::multiply(sunEstB, sunEstB, *matrixSun, 3, 1, 3);
     MatrixOperations<double>::multiply(magEstB, magEstB, *matrixMag, 3, 1, 3);
     MatrixOperations<double>::multiply(sunEstB, magEstB, *matrixSunMag, 3, 1, 3);
@@ -199,8 +190,6 @@ void MultiplicativeKalmanFilter::init(
     initialCovarianceMatrix[5][3] = initGyroCov[2][0];
     initialCovarianceMatrix[5][4] = initGyroCov[2][1];
     initialCovarianceMatrix[5][5] = initGyroCov[2][2];
-    // initialCovarianceMatrix[][] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},
-    //{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
   } else {
     // no initialisation possible, no valid measurements
     validInit = false;
@@ -208,14 +197,15 @@ void MultiplicativeKalmanFilter::init(
 }
 
 // --------------- MEKF DISCRETE TIME STEP -------------------------------
-ReturnValue_t MultiplicativeKalmanFilter::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) {
+ReturnValue_t MultiplicativeKalmanFilter::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, double sampleTime,
+                                                  acsctrl::MekfData *mekfData) {
   // Check for GYR Measurements
-  // AcsParameters mekfEstParams;
-  // loadAcsParameters(&mekfEstParams);
   int MDF = 0;  // Matrix Dimension Factor
   if (!validGYRs_) {
     {
@@ -960,10 +950,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   biasGYR[2] = updatedGyroBias[2];
 
   /* ----------- PROPAGATION ----------*/
-  // double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
-  // double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
-  double sigmaU = 3 * 3.141 / 180 / 3600;
-  double sigmaV = 3 * 0.0043 * 3.141 / sqrt(10) / 180;
+  double sigmaU = kalmanFilterParameters->sensorNoiseBsGYR;
+  double sigmaV = kalmanFilterParameters->sensorNoiseArwGYR;
 
   double discTimeMatrix[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}};
@@ -977,170 +965,135 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   // Discrete Process Noise Covariance Q
   double discProcessNoiseCov[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 covQ1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
-         covQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
-         covQ3[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
-         transCovQ2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  if (normRotEst * sampleTime < 3.141 / 10) {
-    double fact1 = sampleTime * pow(sigmaV, 2) + pow(sampleTime, 3) * pow(sigmaU, 2 / 3);
-    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
-    double fact2 = -(0.5 * pow(sampleTime, 2) * pow(sigmaU, 2));
-    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ2, 3, 3);
-    MatrixOperations<double>::transpose(*covQ2, *transCovQ2, 3);
-    double fact3 = sampleTime * pow(sigmaU, 2);
-    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ3, 3, 3);
+  double covQ11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
+         covQ12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
+         covQ22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
+         covQ12trans[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  if (normRotEst * sampleTime < M_PI / 10) {
+    double fact11 = pow(sigmaV, 2) * sampleTime + 1. / 3. * pow(sigmaU, 2) * pow(sampleTime, 3);
+    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11, *covQ11, 3, 3);
 
-    discProcessNoiseCov[0][0] = covQ1[0][0];
-    discProcessNoiseCov[0][1] = covQ1[0][1];
-    discProcessNoiseCov[0][2] = covQ1[0][2];
-    discProcessNoiseCov[0][3] = covQ2[0][0];
-    discProcessNoiseCov[0][4] = covQ2[0][1];
-    discProcessNoiseCov[0][5] = covQ2[0][2];
-    discProcessNoiseCov[1][0] = covQ1[1][0];
-    discProcessNoiseCov[1][1] = covQ1[1][1];
-    discProcessNoiseCov[1][2] = covQ1[1][2];
-    discProcessNoiseCov[1][3] = covQ2[1][0];
-    discProcessNoiseCov[1][4] = covQ2[1][1];
-    discProcessNoiseCov[1][5] = covQ2[1][2];
-    discProcessNoiseCov[2][0] = covQ1[2][0];
-    discProcessNoiseCov[2][1] = covQ1[2][1];
-    discProcessNoiseCov[2][2] = covQ1[2][2];
-    discProcessNoiseCov[2][3] = covQ2[2][0];
-    discProcessNoiseCov[2][4] = covQ2[2][1];
-    discProcessNoiseCov[2][5] = covQ2[2][2];
-    discProcessNoiseCov[3][0] = transCovQ2[0][0];
-    discProcessNoiseCov[3][1] = transCovQ2[0][1];
-    discProcessNoiseCov[3][2] = transCovQ2[0][2];
-    discProcessNoiseCov[3][3] = covQ3[0][0];
-    discProcessNoiseCov[3][4] = covQ3[0][1];
-    discProcessNoiseCov[3][5] = covQ3[0][2];
-    discProcessNoiseCov[4][0] = transCovQ2[1][0];
-    discProcessNoiseCov[4][1] = transCovQ2[1][1];
-    discProcessNoiseCov[4][2] = transCovQ2[1][2];
-    discProcessNoiseCov[4][3] = covQ3[1][0];
-    discProcessNoiseCov[4][4] = covQ3[1][1];
-    discProcessNoiseCov[4][5] = covQ3[1][2];
-    discProcessNoiseCov[5][0] = transCovQ2[2][0];
-    discProcessNoiseCov[5][1] = transCovQ2[2][1];
-    discProcessNoiseCov[5][2] = transCovQ2[2][2];
-    discProcessNoiseCov[5][3] = covQ3[2][0];
-    discProcessNoiseCov[5][4] = covQ3[2][1];
-    discProcessNoiseCov[5][5] = covQ3[2][2];
+    double fact12 = -(1. / 2. * pow(sigmaU, 2) * pow(sampleTime, 2));
+    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12, *covQ12, 3, 3);
+    std::memcpy(*covQ12trans, *covQ12, 3 * 3 * sizeof(double));
+
+    double fact22 = pow(sigmaU, 2) * sampleTime;
+    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
   } else {
-    // double fact1 = sampleTime*pow(sigmaV,2);
-    double covQ11[3][3], covQ12[3][3], covQ13[3][3];
-    // MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact1, *covQ1, 3, 3);
-    double fact1 = (2 * normRotEst + sampleTime - 2 * sin(normRotEst * sampleTime) -
-                    pow(normRotEst, 3) / 3 * pow(sampleTime, 3)) /
-                   pow(normRotEst, 5);
-    MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11, 3, 3, 3);
-    MatrixOperations<double>::multiplyScalar(*covQ11, fact1, *covQ11, 3, 3);
-    double fact2 = pow(sampleTime, 3);
-    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact2, *covQ12, 3, 3);
-    MatrixOperations<double>::subtract(*covQ12, *covQ11, *covQ11, 3, 3);
-    double fact3 = sampleTime * pow(sigmaV, 2);
-    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact3, *covQ13, 3, 3);
-    MatrixOperations<double>::add(*covQ13, *covQ11, *covQ1, 3, 3);
+    double fact22 = pow(sigmaU, 2) * sampleTime;
+    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact22, *covQ22, 3, 3);
 
-    double covQ21[3][3], covQ22[3][3], covQ23[3][3];
-    double fact4 =
-        (0.5 * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
+    double covQ12_0[3][3], covQ12_1[3][3], covQ12_2[3][3], covQ12_01[3][3];
+    double fact12_0 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
+    MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *covQ12_0, 3, 3);
+    double fact12_1 = 1. / 2. * pow(sampleTime, 2);
+    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact12_1, *covQ12_1, 3, 3);
+    double fact12_2 =
+        (1. / 2. * pow(normRotEst, 2) * pow(sampleTime, 2) + cos(normRotEst * sampleTime) - 1) /
         pow(normRotEst, 4);
-    MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ21, 3, 3, 3);
-    MatrixOperations<double>::multiplyScalar(*covQ21, fact4, *covQ21, 3, 3);
-    double fact5 = 0.5 * pow(sampleTime, 2);
-    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact5, *covQ22, 3, 3);
-    MatrixOperations<double>::add(*covQ22, *covQ21, *covQ21, 3, 3);
-    double fact6 = normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3);
-    MatrixOperations<double>::multiplyScalar(*crossRotEst, fact6, *covQ23, 3, 3);
-    MatrixOperations<double>::subtract(*covQ23, *covQ21, *covQ21, 3, 3);
-    double fact7 = pow(sigmaU, 2);
-    MatrixOperations<double>::multiplyScalar(*covQ21, fact7, *covQ2, 3, 3);
+    MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ12_2, 3, 3, 3);
+    MatrixOperations<double>::multiplyScalar(*covQ12_2, fact12_2, *covQ12_2, 3, 3);
+    MatrixOperations<double>::subtract(*covQ12_0, *covQ12_1, *covQ12_01, 3, 3);
+    MatrixOperations<double>::subtract(*covQ12_01, *covQ12_2, *covQ12, 3, 3);
+    MatrixOperations<double>::multiplyScalar(*covQ12, pow(sigmaU, 2), *covQ12, 3, 3);
+    MatrixOperations<double>::transpose(*covQ12, *covQ12trans, 3);
 
-    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact7, *covQ3, 3, 3);
-
-    discProcessNoiseCov[0][0] = covQ1[0][0];
-    discProcessNoiseCov[0][1] = covQ1[0][1];
-    discProcessNoiseCov[0][2] = covQ1[0][2];
-    discProcessNoiseCov[0][3] = covQ2[0][0];
-    discProcessNoiseCov[0][4] = covQ2[0][1];
-    discProcessNoiseCov[0][5] = covQ2[0][2];
-    discProcessNoiseCov[1][0] = covQ1[1][0];
-    discProcessNoiseCov[1][1] = covQ1[1][1];
-    discProcessNoiseCov[1][2] = covQ1[1][2];
-    discProcessNoiseCov[1][3] = covQ2[1][0];
-    discProcessNoiseCov[1][4] = covQ2[1][1];
-    discProcessNoiseCov[1][5] = covQ2[1][2];
-    discProcessNoiseCov[2][0] = covQ1[2][0];
-    discProcessNoiseCov[2][1] = covQ1[2][1];
-    discProcessNoiseCov[2][2] = covQ1[2][2];
-    discProcessNoiseCov[2][3] = covQ2[2][0];
-    discProcessNoiseCov[2][4] = covQ2[2][1];
-    discProcessNoiseCov[2][5] = covQ2[2][2];
-    discProcessNoiseCov[3][0] = covQ2[0][0];
-    discProcessNoiseCov[3][1] = covQ2[0][1];
-    discProcessNoiseCov[3][2] = covQ2[0][2];
-    discProcessNoiseCov[3][3] = covQ3[0][0];
-    discProcessNoiseCov[3][4] = covQ3[0][1];
-    discProcessNoiseCov[3][5] = covQ3[0][2];
-    discProcessNoiseCov[4][0] = covQ2[1][0];
-    discProcessNoiseCov[4][1] = covQ2[1][1];
-    discProcessNoiseCov[4][2] = covQ2[1][2];
-    discProcessNoiseCov[4][3] = covQ3[1][0];
-    discProcessNoiseCov[4][4] = covQ3[1][1];
-    discProcessNoiseCov[4][5] = covQ3[1][2];
-    discProcessNoiseCov[5][0] = covQ2[2][0];
-    discProcessNoiseCov[5][1] = covQ2[2][1];
-    discProcessNoiseCov[5][2] = covQ2[2][2];
-    discProcessNoiseCov[5][3] = covQ3[2][0];
-    discProcessNoiseCov[5][4] = covQ3[2][1];
-    discProcessNoiseCov[5][5] = covQ3[2][2];
+    double covQ11_0[3][3], covQ11_1[3][3], covQ11_2[3][3], covQ11_12[3][3];
+    double fact11_0 = pow(sigmaV, 2) * sampleTime;
+    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_0, *covQ11_0, 3, 3);
+    double fact11_1 = 1. / 3. * pow(sampleTime, 3);
+    MatrixOperations<double>::multiplyScalar(*identityMatrix3, fact11_1, *covQ11_1, 3, 3);
+    double fact11_2 = (2 * normRotEst * sampleTime - 2 * sin(normRotEst * sampleTime) -
+                       1. / 3. * pow(normRotEst, 3) * pow(sampleTime, 3)) /
+                      pow(normRotEst, 5);
+    MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *covQ11_2, 3, 3, 3);
+    MatrixOperations<double>::multiplyScalar(*covQ11_2, fact11_2, *covQ11_2, 3, 3);
+    MatrixOperations<double>::subtract(*covQ11_1, *covQ11_2, *covQ11_12, 3, 3);
+    MatrixOperations<double>::multiplyScalar(*covQ11_12, pow(sigmaU, 2), *covQ11_12, 3, 3);
+    MatrixOperations<double>::add(*covQ11_0, *covQ11_12, *covQ11, 3, 3);
   }
+  discProcessNoiseCov[0][0] = covQ11[0][0];
+  discProcessNoiseCov[0][1] = covQ11[0][1];
+  discProcessNoiseCov[0][2] = covQ11[0][2];
+  discProcessNoiseCov[0][3] = covQ12[0][0];
+  discProcessNoiseCov[0][4] = covQ12[0][1];
+  discProcessNoiseCov[0][5] = covQ12[0][2];
+  discProcessNoiseCov[1][0] = covQ11[1][0];
+  discProcessNoiseCov[1][1] = covQ11[1][1];
+  discProcessNoiseCov[1][2] = covQ11[1][2];
+  discProcessNoiseCov[1][3] = covQ12[1][0];
+  discProcessNoiseCov[1][4] = covQ12[1][1];
+  discProcessNoiseCov[1][5] = covQ12[1][2];
+  discProcessNoiseCov[2][0] = covQ11[2][0];
+  discProcessNoiseCov[2][1] = covQ11[2][1];
+  discProcessNoiseCov[2][2] = covQ11[2][2];
+  discProcessNoiseCov[2][3] = covQ12[2][0];
+  discProcessNoiseCov[2][4] = covQ12[2][1];
+  discProcessNoiseCov[2][5] = covQ12[2][2];
+  discProcessNoiseCov[3][0] = covQ12trans[0][0];
+  discProcessNoiseCov[3][1] = covQ12trans[0][1];
+  discProcessNoiseCov[3][2] = covQ12trans[0][2];
+  discProcessNoiseCov[3][3] = covQ22[0][0];
+  discProcessNoiseCov[3][4] = covQ22[0][1];
+  discProcessNoiseCov[3][5] = covQ22[0][2];
+  discProcessNoiseCov[4][0] = covQ12trans[1][0];
+  discProcessNoiseCov[4][1] = covQ12trans[1][1];
+  discProcessNoiseCov[4][2] = covQ12trans[1][2];
+  discProcessNoiseCov[4][3] = covQ22[1][0];
+  discProcessNoiseCov[4][4] = covQ22[1][1];
+  discProcessNoiseCov[4][5] = covQ22[1][2];
+  discProcessNoiseCov[5][0] = covQ12trans[2][0];
+  discProcessNoiseCov[5][1] = covQ12trans[2][1];
+  discProcessNoiseCov[5][2] = covQ12trans[2][2];
+  discProcessNoiseCov[5][3] = covQ22[2][0];
+  discProcessNoiseCov[5][4] = covQ22[2][1];
+  discProcessNoiseCov[5][5] = covQ22[2][2];
 
   // State Transition Matrix phi
-  double phi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
-         phi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
+  double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
+         phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
          phi[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                       {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
-  double phi11[3][3], phi12[3][3];
-  double fact1 = sin(normRotEst * sampleTime);
-  MatrixOperations<double>::multiplyScalar(*crossRotEst, fact1, *phi11, 3, 3);
-  double fact2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
-  MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12, 3, 3, 3);
-  MatrixOperations<double>::multiplyScalar(*phi12, fact2, *phi12, 3, 3);
-  MatrixOperations<double>::subtract(*identityMatrix3, *phi11, *phi11, 3, 3);
-  MatrixOperations<double>::add(*phi11, *phi12, *phi1, 3, 3);
+  double phi11_1[3][3], phi11_2[3][3], phi11_01[3][3];
+  double fact11_1 = sin(normRotEst * sampleTime) / normRotEst;
+  MatrixOperations<double>::multiplyScalar(*crossRotEst, fact11_1, *phi11_1, 3, 3);
+  double fact11_2 = (1 - cos(normRotEst * sampleTime)) / pow(normRotEst, 2);
+  MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi11_2, 3, 3, 3);
+  MatrixOperations<double>::multiplyScalar(*phi11_2, fact11_2, *phi11_2, 3, 3);
+  MatrixOperations<double>::subtract(*identityMatrix3, *phi11_1, *phi11_01, 3, 3);
+  MatrixOperations<double>::add(*phi11_01, *phi11_2, *phi11, 3, 3);
 
-  double phi21[3][3], phi22[3][3];
-  MatrixOperations<double>::multiplyScalar(*crossRotEst, fact2, *phi21, 3, 3);
-  MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi22, 3, 3);
-  MatrixOperations<double>::subtract(*phi21, *phi22, *phi21, 3, 3);
-  double fact3 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
-  MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi22, 3, 3, 3);
-  MatrixOperations<double>::multiplyScalar(*phi22, fact3, *phi22, 3, 3);
-  MatrixOperations<double>::subtract(*phi21, *phi22, *phi2, 3, 3);
+  double phi12_0[3][3], phi12_1[3][3], phi12_2[3][3], phi12_01[3][3];
+  double fact12_0 = fact11_2;
+  MatrixOperations<double>::multiplyScalar(*crossRotEst, fact12_0, *phi12_0, 3, 3);
+  MatrixOperations<double>::multiplyScalar(*identityMatrix3, sampleTime, *phi12_1, 3, 3);
+  double fact12_2 = (normRotEst * sampleTime - sin(normRotEst * sampleTime) / pow(normRotEst, 3));
+  MatrixOperations<double>::multiply(*crossRotEst, *crossRotEst, *phi12_2, 3, 3, 3);
+  MatrixOperations<double>::multiplyScalar(*phi12_2, fact12_2, *phi12_2, 3, 3);
+  MatrixOperations<double>::subtract(*phi12_0, *phi12_1, *phi12_01, 3, 3);
+  MatrixOperations<double>::subtract(*phi12_01, *phi12_2, *phi12, 3, 3);
 
-  phi[0][0] = phi1[0][0];
-  phi[0][1] = phi1[0][1];
-  phi[0][2] = phi1[0][2];
-  phi[0][3] = phi2[0][0];
-  phi[0][4] = phi2[0][1];
-  phi[0][5] = phi2[0][2];
-  phi[1][0] = phi1[1][0];
-  phi[1][1] = phi1[1][1];
-  phi[1][2] = phi1[1][2];
-  phi[1][3] = phi2[1][0];
-  phi[1][4] = phi2[1][1];
-  phi[1][5] = phi2[1][2];
-  phi[2][0] = phi1[2][0];
-  phi[2][1] = phi1[2][1];
-  phi[2][2] = phi1[2][2];
-  phi[2][3] = phi2[2][0];
-  phi[2][4] = phi2[2][1];
-  phi[2][5] = phi2[2][2];
+  phi[0][0] = phi11[0][0];
+  phi[0][1] = phi11[0][1];
+  phi[0][2] = phi11[0][2];
+  phi[0][3] = phi12[0][0];
+  phi[0][4] = phi12[0][1];
+  phi[0][5] = phi12[0][2];
+  phi[1][0] = phi11[1][0];
+  phi[1][1] = phi11[1][1];
+  phi[1][2] = phi11[1][2];
+  phi[1][3] = phi12[1][0];
+  phi[1][4] = phi12[1][1];
+  phi[1][5] = phi12[1][2];
+  phi[2][0] = phi11[2][0];
+  phi[2][1] = phi11[2][1];
+  phi[2][2] = phi11[2][2];
+  phi[2][3] = phi12[2][0];
+  phi[2][4] = phi12[2][1];
+  phi[2][5] = phi12[2][2];
 
   // Propagated Quaternion
-  double rotSin[3] = {0, 0, 0}, omega1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  double rotSin[3] = {0, 0, 0}, rotCosMat[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   double rotCos = cos(0.5 * normRotEst * sampleTime);
   double sinFac = sin(0.5 * normRotEst * sampleTime) / normRotEst;
   VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
@@ -1148,25 +1101,26 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MathOperations<double>::skewMatrix(rotSin, *skewSin);
 
-  MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *omega1, 3, 3);
-  MatrixOperations<double>::subtract(*omega1, *skewSin, *omega1, 3, 3);
-  double omega[4][4] = {{omega1[0][0], omega1[0][1], omega1[0][2], rotSin[0]},
-                        {omega1[1][0], omega1[1][1], omega1[1][2], rotSin[1]},
-                        {omega1[2][0], omega1[2][1], omega1[2][2], rotSin[2]},
+  MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3);
+  double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MatrixOperations<double>::subtract(*rotCosMat, *skewSin, *subMatUL, 3, 3);
+  double omega[4][4] = {{subMatUL[0][0], subMatUL[0][1], subMatUL[0][2], rotSin[0]},
+                        {subMatUL[1][0], subMatUL[1][1], subMatUL[1][2], rotSin[1]},
+                        {subMatUL[2][0], subMatUL[2][1], subMatUL[2][2], rotSin[2]},
                         {-rotSin[0], -rotSin[1], -rotSin[2], rotCos}};
   MatrixOperations<double>::multiply(*omega, quatBJ, propagatedQuaternion, 4, 4, 1);
 
   // Update Covariance Matrix
-  double cov1[6][6], cov2[6][6], transDiscTimeMatrix[6][6], transPhi[6][6];
+  double cov0[6][6], cov1[6][6], transPhi[6][6], transDiscTimeMatrix[6][6];
+  MatrixOperations<double>::transpose(*phi, *transPhi, 6);
+  MatrixOperations<double>::multiply(*covMatPlus, *transPhi, *cov0, 6, 6, 6);
+  MatrixOperations<double>::multiply(*phi, *cov0, *cov0, 6, 6, 6);
+
   MatrixOperations<double>::transpose(*discTimeMatrix, *transDiscTimeMatrix, 6);
   MatrixOperations<double>::multiply(*discProcessNoiseCov, *transDiscTimeMatrix, *cov1, 6, 6, 6);
   MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
 
-  MatrixOperations<double>::transpose(*phi, *transPhi, 6);
-  MatrixOperations<double>::multiply(*covMatPlus, *transPhi, *cov2, 6, 6, 6);
-  MatrixOperations<double>::multiply(*phi, *cov2, *cov2, 6, 6, 6);
-
-  MatrixOperations<double>::add(*cov2, *cov1, *initialCovarianceMatrix, 6, 6);
+  MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
   validMekf = true;
 
   // Discrete Time Step
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h
index 5eb47418..a2f81272 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -20,7 +20,7 @@
 
 #include "../controllerdefinitions/AcsCtrlDefinitions.h"
 #include "AcsParameters.h"
-#include "config/classIds.h"
+#include "eive/resultClassIds.h"
 
 class MultiplicativeKalmanFilter {
  public:
@@ -61,17 +61,13 @@ class MultiplicativeKalmanFilter {
                         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);
+                        const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
 
-  // Declaration of Events (like init) and memberships
-  // static const uint8_t INTERFACE_ID = CLASS_ID::MEKF; //CLASS IDS ND
-  // (/config/returnvalues/classIDs.h) static const Event RESET  =
-  // MAKE_EVENT(1,severity::INFO);//typedef uint32_t Event (Event.h), should be
   // resetting Mekf
-  static const uint8_t INTERFACE_ID = CLASS_ID::KALMAN;
-  static const ReturnValue_t KALMAN_NO_GYR_MEAS = MAKE_RETURN_CODE(0x01);
-  static const ReturnValue_t KALMAN_NO_MODEL = MAKE_RETURN_CODE(0x02);
-  static const ReturnValue_t KALMAN_INVERSION_FAILED = MAKE_RETURN_CODE(0x03);
+  static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
+  static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1);
+  static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2);
+  static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3);
 
  private:
   /*Parameters*/
@@ -79,7 +75,6 @@ class MultiplicativeKalmanFilter {
   AcsParameters::KalmanFilterParameters *kalmanFilterParameters;
   double quaternion_STR_SB[4];
   bool validInit;
-  double sampleTime = 0.1;
 
   /*States*/
   double initialQuaternion[4];          /*after reset?QUEST*/
diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp
index 921ae604..a9de5817 100644
--- a/mission/controller/acs/Navigation.cpp
+++ b/mission/controller/acs/Navigation.cpp
@@ -39,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues,
         mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
         susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
         susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
-        mgmDataProcessed->magIgrfModel.isValid(),
+        mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime,
         mekfData);  // VALIDS FOR QUAT AND RATE ??
   } else {
     multiplicativeKalmanFilter.init(
diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index ac2e5752..24547247 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -1,3 +1,10 @@
+/*
+ * SensorProcessing.cpp
+ *
+ *  Created on: 7 Mar 2022
+ *      Author: Robin Marquardt
+ */
+
 #include "SensorProcessing.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>
@@ -14,8 +21,7 @@
 
 using namespace Math;
 
-SensorProcessing::SensorProcessing(AcsParameters *acsParameters_)
-    : savedMgmVecTot{0, 0, 0}, validMagField(false), validGcLatitude(false) {}
+SensorProcessing::SensorProcessing(AcsParameters *acsParameters_) {}
 
 SensorProcessing::~SensorProcessing() {}
 
@@ -27,19 +33,35 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
                                   acsctrl::GpsDataProcessed *gpsDataProcessed,
                                   const double gpsAltitude, bool gpsValid,
                                   acsctrl::MgmDataProcessed *mgmDataProcessed) {
+  // ---------------- IGRF- 13 Implementation here ------------------------------------------------
+  double magIgrfModel[3] = {0.0, 0.0, 0.0};
+  if (gpsValid) {
+    // Should be existing class object which will be called and modified here.
+    Igrf13Model igrf13;
+    // So the line above should not be done here. Update: Can be done here as long updated coffs
+    //		stored in acsParameters ?
+    igrf13.schmidtNormalization();
+    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,
+                        gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
+  }
   if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
     {
       PoolReadGuard pg(mgmDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(mgmDataProcessed->magIgrfModel.value, zeroVector, 3 * sizeof(double));
+        float zeroVec[3] = {0.0, 0.0, 0.0};
+        std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float));
         mgmDataProcessed->setValidity(false, true);
+        std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
+        mgmDataProcessed->magIgrfModel.setValid(gpsValid);
       }
     }
     return;
@@ -47,71 +69,72 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
         mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
         mgm4ValueNoBias[3] = {0, 0, 0};
-  float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
-        mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
   float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
         mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
+  float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
+        mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
   float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
 
   if (mgm0valid) {
-    VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
+                                      mgm0ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset,
+                                      mgm0ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
                                       mgm0ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
-                                      mgm0ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
+      sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
     }
   }
   if (mgm1valid) {
-    VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
+                                      mgm1ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset,
+                                      mgm1ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
                                       mgm1ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
-                                      mgm1ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
+      sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
     }
   }
   if (mgm2valid) {
-    VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
+                                      mgm2ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset,
+                                      mgm2ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
                                       mgm2ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
-                                      mgm2ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
+      sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
     }
   }
   if (mgm3valid) {
-    VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
+                                      mgm3ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset,
+                                      mgm3ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
                                       mgm3ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
-                                      mgm3ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
+      sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
     }
   }
   if (mgm4valid) {
-    float mgm4ValueNT[3];
-    VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueNT, 3);  // uT to nT
-    VectorOperations<float>::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset,
+    float mgm4ValueUT[3];
+    VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3);  // nT to uT
+    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
+                                      mgm4ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset,
                                       mgm4ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
                                       mgm4ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
-                                      mgm4ValueBody, 3, 3, 1);
+
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
+      sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
     }
   }
@@ -128,35 +151,23 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
     for (uint8_t i = 0; i < 3; i++) {
       mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
       savedMgmVecTot[i] = mgmVecTot[i];
+      mgmVecTotDerivativeValid = true;
     }
   }
   timeOfSavedMagFieldEst = timeOfMgmMeasurement;
 
-  // ---------------- IGRF- 13 Implementation here ------------------------------------------------
-  double magIgrfModel[3] = {0.0, 0.0, 0.0};
-  if (gpsValid) {
-    // Should be existing class object which will be called and modified here.
-    Igrf13Model igrf13;
-    // So the line above should not be done here. Update: Can be done here as long updated coffs
-    //		stored in acsParameters ?
-    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,
-                        gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
-  }
   {
     PoolReadGuard pg(mgmDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm0vec.setValid(mgm0valid);
-      std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm1vec.setValid(mgm1valid);
-      std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm2vec.setValid(mgm2valid);
-      std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm3vec.setValid(mgm3valid);
-      std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm4vec.setValid(mgm4valid);
       std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
       mgmDataProcessed->mgmVecTot.setValid(true);
@@ -180,6 +191,25 @@ void SensorProcessing::processSus(
     timeval timeOfSusMeasurement, const AcsParameters::SusHandlingParameters *susParameters,
     const AcsParameters::SunModelParameters *sunModelParameters,
     acsctrl::SusDataProcessed *susDataProcessed) {
+  /* -------- Sun Model Direction (IJK frame) ------- */
+  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
+
+  // Julean Centuries
+  double sunIjkModel[3] = {0.0, 0.0, 0.0};
+  double JC2000 = JD2000 / 36525.;
+
+  double meanLongitude =
+      sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
+  double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
+
+  double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
+                             sunModelParameters->p2 * sin(2 * meanAnomaly);
+
+  double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
+
+  sunIjkModel[0] = cos(eclipticLongitude);
+  sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
+  sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
   if (sus0valid) {
     sus0valid = susConverter.checkSunSensorData(sus0Value);
   }
@@ -222,22 +252,24 @@ void SensorProcessing::processSus(
     {
       PoolReadGuard pg(susDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus2vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus3vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus4vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus5vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus6vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus7vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus8vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus9vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus10vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sus11vec.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->susVecTot.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVector, 3 * sizeof(float));
-        std::memcpy(susDataProcessed->sunIjkModel.value, zeroVector, 3 * sizeof(double));
+        float zeroVec[3] = {0.0, 0.0, 0.0};
+        std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float));
         susDataProcessed->setValidity(false, true);
+        std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
+        susDataProcessed->sunIjkModel.setValid(true);
       }
     }
     return;
@@ -256,16 +288,6 @@ void SensorProcessing::processSus(
                                              susParameters->sus0coeffBeta),
         sus0VecBody, 3, 3, 1);
   }
-  {
-    PoolReadGuard pg(susDataProcessed);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
-      susDataProcessed->sus0vec.setValid(sus0valid);
-      if (!sus0valid) {
-        std::memcpy(susDataProcessed->sus0vec.value, zeroVector, 3 * sizeof(float));
-      }
-    }
-  }
   if (sus1valid) {
     MatrixOperations<float>::multiply(
         susParameters->sus1orientationMatrix[0],
@@ -273,16 +295,6 @@ void SensorProcessing::processSus(
                                              susParameters->sus1coeffBeta),
         sus1VecBody, 3, 3, 1);
   }
-  {
-    PoolReadGuard pg(susDataProcessed);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
-      susDataProcessed->sus1vec.setValid(sus1valid);
-      if (!sus1valid) {
-        std::memcpy(susDataProcessed->sus1vec.value, zeroVector, 3 * sizeof(float));
-      }
-    }
-  }
   if (sus2valid) {
     MatrixOperations<float>::multiply(
         susParameters->sus2orientationMatrix[0],
@@ -387,30 +399,10 @@ void SensorProcessing::processSus(
     for (uint8_t i = 0; i < 3; i++) {
       susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
       savedSusVecTot[i] = susVecTot[i];
+      susVecTotDerivativeValid = true;
     }
   }
   timeOfSavedSusDirEst = timeOfSusMeasurement;
-
-  /* -------- Sun Model Direction (IJK frame) ------- */
-  // if (useSunModel) eventuell
-  double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfSusMeasurement);
-
-  // Julean Centuries
-  double sunIjkModel[3] = {0.0, 0.0, 0.0};
-  double JC2000 = JD2000 / 36525;
-
-  double meanLongitude =
-      (sunModelParameters->omega_0 + (sunModelParameters->domega) * JC2000) * PI / 180;
-  double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
-
-  double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
-                             sunModelParameters->p2 * sin(2 * meanAnomaly);
-
-  double epsilon = sunModelParameters->e - (sunModelParameters->e1) * JC2000;
-
-  sunIjkModel[0] = cos(eclipticLongitude);
-  sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
-  sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
   {
     PoolReadGuard pg(susDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -467,6 +459,7 @@ void SensorProcessing::processGyr(
     {
       PoolReadGuard pg(gyrDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
+        double zeroVector[3] = {0.0, 0.0, 0.0};
         std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
         std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
         std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
@@ -486,6 +479,8 @@ void SensorProcessing::processGyr(
     const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
                                        gyr0ValueBody, 3, 3, 1);
+    VectorOperations<double>::subtract(gyr0ValueBody, gyrParameters->gyr0bias, gyr0ValueBody, 3);
+    VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
       sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@@ -495,6 +490,8 @@ void SensorProcessing::processGyr(
     const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
                                        gyr1ValueBody, 3, 3, 1);
+    VectorOperations<double>::subtract(gyr1ValueBody, gyrParameters->gyr1bias, gyr1ValueBody, 3);
+    VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
       sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@@ -504,6 +501,8 @@ void SensorProcessing::processGyr(
     const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
                                        gyr2ValueBody, 3, 3, 1);
+    VectorOperations<double>::subtract(gyr2ValueBody, gyrParameters->gyr2bias, gyr2ValueBody, 3);
+    VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
       sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@@ -513,6 +512,8 @@ void SensorProcessing::processGyr(
     const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
     MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
                                        gyr3ValueBody, 3, 3, 1);
+    VectorOperations<double>::subtract(gyr3ValueBody, gyrParameters->gyr3bias, gyr3ValueBody, 3);
+    VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
     for (uint8_t i = 0; i < 3; i++) {
       sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
       sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@@ -523,7 +524,7 @@ void SensorProcessing::processGyr(
   // take ADIS measurements, if both avail
   // if just one ADIS measurement avail, perform sensor fusion
   double gyrVecTot[3] = {0.0, 0.0, 0.0};
-  if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == gyrParameters->PreferAdis::YES) {
+  if ((gyr0valid && gyr2valid) && gyrParameters->preferAdis == true) {
     double gyr02ValuesSum[3];
     VectorOperations<double>::add(gyr0ValueBody, gyr2ValueBody, gyr02ValuesSum, 3);
     VectorOperations<double>::mulScalar(gyr02ValuesSum, .5, gyrVecTot, 3);
@@ -550,30 +551,45 @@ void SensorProcessing::processGyr(
   }
 }
 
-void SensorProcessing::processGps(const double gps0latitude, const double gps0longitude,
+void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
+                                  const double gpsAltitude, const double gpsUnixSeconds,
                                   const bool validGps,
+                                  const AcsParameters::GpsParameters *gpsParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed) {
   // name to convert not process
-  double gdLongitude, gcLatitude;
+  double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
   if (validGps) {
     // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
-    gdLongitude = gps0longitude * PI / 180;
-    double latitudeRad = gps0latitude * PI / 180;
+    gdLongitude = gpsLongitude * PI / 180.;
+    double latitudeRad = gpsLatitude * PI / 180.;
     double eccentricityWgs84 = 0.0818195;
     double factor = 1 - pow(eccentricityWgs84, 2);
     gcLatitude = atan(factor * tan(latitudeRad));
-    // validGcLatitude = true;
+
+    // Calculation of the satellite velocity in earth fixed frame
+    double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
+    MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
+    if (validSavedPosSatE &&
+        (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
+      VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 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;
   }
   {
     PoolReadGuard pg(gpsDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
       gpsDataProcessed->gdLongitude.value = gdLongitude;
       gpsDataProcessed->gcLatitude.value = gcLatitude;
-      gpsDataProcessed->setValidity(validGps, validGps);
-      if (!validGps) {
-        gpsDataProcessed->gdLongitude.value = 0.0;
-        gpsDataProcessed->gcLatitude.value = 0.0;
-      }
+      std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
+      std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
+      gpsDataProcessed->setValidity(validGps, true);
     }
   }
 }
@@ -585,10 +601,12 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
                                acsctrl::GpsDataProcessed *gpsDataProcessed,
                                const AcsParameters *acsParameters) {
   sensorValues->update();
-  processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
-             (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
-              sensorValues->gpsSet.altitude.isValid()),
-             gpsDataProcessed);
+  processGps(
+      sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
+      sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
+      (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
+       sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.unixSeconds.isValid()),
+      &acsParameters->gpsParameters, gpsDataProcessed);
 
   processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
              sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h
index 61bd7a96..6fa1ab8c 100644
--- a/mission/controller/acs/SensorProcessing.h
+++ b/mission/controller/acs/SensorProcessing.h
@@ -1,5 +1,5 @@
 /*******************************
- *	EIVE Flight Software Framework (FSFW)
+ *	EIVE Flight Software
  * (c) 2022 IRS, Uni Stuttgart
  *******************************/
 #ifndef SENSORPROCESSING_H_
@@ -13,7 +13,7 @@
 #include "AcsParameters.h"
 #include "SensorValues.h"
 #include "SusConverter.h"
-#include "config/classIds.h"
+#include "eive/resultClassIds.h"
 
 class SensorProcessing {
  public:
@@ -65,17 +65,20 @@ class SensorProcessing {
 
   void processStr();
 
-  void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
+  void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
+                  const double gpsUnixSeconds, const bool validGps,
+                  const AcsParameters::GpsParameters *gpsParameters,
                   acsctrl::GpsDataProcessed *gpsDataProcessed);
 
-  double savedMgmVecTot[3];
+  double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
   timeval timeOfSavedMagFieldEst;
-  double savedSusVecTot[3];
+  double savedSusVecTot[3] = {0.0, 0.0, 0.0};
   timeval timeOfSavedSusDirEst;
-  bool validMagField;
-  bool validGcLatitude;
+  bool validMagField = false;
 
-  const float zeroVector[3] = {0.0, 0.0, 0.0};
+  double savedPosSatE[3] = {0.0, 0.0, 0.0};
+  double timeOfSavedPosSatE = 0.0;
+  bool validSavedPosSatE = false;
 
   SusConverter susConverter;
   AcsParameters acsParameters;
diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h
index ef897207..ec1795fc 100644
--- a/mission/controller/acs/SensorValues.h
+++ b/mission/controller/acs/SensorValues.h
@@ -1,12 +1,12 @@
 #ifndef SENSORVALUES_H_
 #define SENSORVALUES_H_
 
+#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
 #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
 #include "mission/devices/devicedefinitions/GPSDefinitions.h"
 #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
-#include "mission/devices/devicedefinitions/GyroL3GD20Definitions.h"
 #include "mission/devices/devicedefinitions/RwDefinitions.h"
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
 #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
diff --git a/mission/controller/acs/config/classIds.h b/mission/controller/acs/config/classIds.h
deleted file mode 100644
index ccf6b616..00000000
--- a/mission/controller/acs/config/classIds.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef ACS_CONFIG_CLASSIDS_H_
-#define ACS_CONFIG_CLASSIDS_H_
-
-#include <common/config/eive/resultClassIds.h>
-#include <fsfw/returnvalues/FwClassIds.h>
-
-namespace CLASS_ID {
-enum eiveclassIds : uint8_t {
-  EIVE_CLASS_ID_START = COMMON_CLASS_ID_END,
-  KALMAN,
-  SAFE,
-  PTG,
-  DETUMBLE,
-  EIVE_CLASS_ID_END  // [EXPORT] : [END]
-};
-}
-
-#endif /* ACS_CONFIG_CLASSIDS_H_ */
diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp
index 4c8217dc..705bf599 100644
--- a/mission/controller/acs/control/Detumble.cpp
+++ b/mission/controller/acs/control/Detumble.cpp
@@ -22,7 +22,7 @@ Detumble::Detumble(AcsParameters *acsParameters_) { loadAcsParameters(acsParamet
 Detumble::~Detumble() {}
 
 void Detumble::loadAcsParameters(AcsParameters *acsParameters_) {
-  detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
+  detumbleParameter = &(acsParameters_->detumbleParameter);
   magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
 }
 
@@ -31,7 +31,7 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid,
   if (!magRateValid || !magFieldValid) {
     return DETUMBLE_NO_SENSORDATA;
   }
-  double gain = detumbleCtrlParameters->gainD;
+  double gain = detumbleParameter->gainD;
   double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
   VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
   return returnvalue::OK;
@@ -50,3 +50,15 @@ ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateVal
 
   return returnvalue::OK;
 }
+
+ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
+                                    const double *magField, const bool *magFieldValid,
+                                    double *magMom) {
+  if (!satRateValid || !magFieldValid) {
+    return DETUMBLE_NO_SENSORDATA;
+  }
+  double gain = detumbleParameter->gainD;
+  double factor = -gain / pow(VectorOperations<double>::norm(magField, 3), 2);
+  VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
+  return returnvalue::OK;
+}
diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h
index bea9b1bf..65e5ec28 100644
--- a/mission/controller/acs/control/Detumble.h
+++ b/mission/controller/acs/control/Detumble.h
@@ -1,10 +1,3 @@
-/*
- * Detumble.h
- *
- *  Created on: 17 Aug 2022
- *      Author: Robin Marquardt
- */
-
 #ifndef ACS_CONTROL_DETUMBLE_H_
 #define ACS_CONTROL_DETUMBLE_H_
 
@@ -15,17 +8,17 @@
 
 #include "../AcsParameters.h"
 #include "../SensorValues.h"
-#include "../config/classIds.h"
+#include "eive/resultClassIds.h"
 
 class Detumble {
  public:
   Detumble(AcsParameters *acsParameters_);
   virtual ~Detumble();
 
-  static const uint8_t INTERFACE_ID = CLASS_ID::DETUMBLE;
+  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE;
   static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01);
 
-  /* @brief: Load AcsParameters für this class
+  /* @brief: Load AcsParameters for this class
    * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
    */
   void loadAcsParameters(AcsParameters *acsParameters_);
@@ -35,8 +28,13 @@ class Detumble {
 
   ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom);
 
+  ReturnValue_t bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom);
+
+  ReturnValue_t bDotLawGyro(const double *satRate, const bool *satRateValid, const double *magField,
+                            const bool *magFieldValid, double *magMom);
+
  private:
-  AcsParameters::DetumbleCtrlParameters *detumbleCtrlParameters;
+  AcsParameters::DetumbleParameter *detumbleParameter;
   AcsParameters::MagnetorquesParameter *magnetorquesParameter;
 };
 
diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp
index 07b595ec..352c0319 100644
--- a/mission/controller/acs/control/PtgCtrl.cpp
+++ b/mission/controller/acs/control/PtgCtrl.cpp
@@ -21,21 +21,21 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { loadAcsParameters(acsParameter
 PtgCtrl::~PtgCtrl() {}
 
 void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
-  pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
   inertiaEIVE = &(acsParameters_->inertiaEIVE);
   rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
   rwMatrices = &(acsParameters_->rwMatrices);
 }
 
-void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
-                               const double *rwPseudoInv, double *torqueRws) {
+void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
+                     const double *qError, const double *deltaRate, const double *rwPseudoInv,
+                     double *torqueRws) {
   //------------------------------------------------------------------------------------------------
   // Compute gain matrix K and P matrix
   //------------------------------------------------------------------------------------------------
-  double om = pointingModeControllerParameters->om;
-  double zeta = pointingModeControllerParameters->zeta;
-  double qErrorMin = pointingModeControllerParameters->qiMin;
-  double omMax = pointingModeControllerParameters->omMax;
+  double om = pointingLawParameters->om;
+  double zeta = pointingLawParameters->zeta;
+  double qErrorMin = pointingLawParameters->qiMin;
+  double omMax = pointingLawParameters->omMax;
 
   double cInt = 2 * om * zeta;
   double kInt = 2 * pow(om, 2);
@@ -104,12 +104,14 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
   double torque[3] = {0, 0, 0};
   VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
   MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
+  VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
 }
 
-void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
+void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
+                              double *magFieldEst, bool magFieldEstValid, double *satRate,
                               int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
                               int32_t *speedRw3, double *mgtDpDes) {
-  if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
+  if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
     mgtDpDes[0] = 0;
     mgtDpDes[1] = 0;
     mgtDpDes[2] = 0;
@@ -127,17 +129,18 @@ void PtgCtrl::ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double
   VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
   //	calculating momentum error
   double deltaMomentum[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(
-      momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
+  VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
+                                     deltaMomentum, 3);
   //	resulting magnetic dipole command
   double crossMomentumMagField[3] = {0, 0, 0};
   VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
   double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
-  factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
+  factor = (pointingLawParameters->deSatGainFactor) / normMag;
   VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
 }
 
-void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
+void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+                           const int32_t *speedRw0, const int32_t *speedRw1,
                            const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
   double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
   double wheelMomentum[4] = {0, 0, 0, 0};
@@ -149,7 +152,7 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
   VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
   VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
                                       wheelMomentum, 4);
-  double gainNs = pointingModeControllerParameters->gainNullspace;
+  double gainNs = pointingLawParameters->gainNullspace;
   double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
                                                 *nullSpaceMatrix, 4);
@@ -157,3 +160,32 @@ void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
   VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
   VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
 }
+
+void PtgCtrl::rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw,
+                             double *torqueCommand) {
+  for (uint8_t i = 0; i < 4; i++) {
+    if (rwAvailable[i]) {
+      if (torqueMemory[i] != 0) {
+        if ((omegaRw[i] * torqueMemory[i]) > rwHandlingParameters->stictionReleaseSpeed) {
+          torqueMemory[i] = 0;
+        } else {
+          torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
+        }
+      } else {
+        if ((omegaRw[i] < rwHandlingParameters->stictionSpeed) &&
+            (omegaRw[i] > -rwHandlingParameters->stictionSpeed)) {
+          if (omegaRw[i] < omegaMemory[i]) {
+            torqueMemory[i] = -1;
+          } else {
+            torqueMemory[i] = 1;
+          }
+
+          torqueCommand[i] = torqueMemory[i] * rwHandlingParameters->stictionTorque;
+        }
+      }
+    } else {
+      torqueMemory[i] = 0;
+    }
+    omegaMemory[i] = omegaRw[i];
+  }
+}
diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h
index 64b4110c..5e9a704c 100644
--- a/mission/controller/acs/control/PtgCtrl.h
+++ b/mission/controller/acs/control/PtgCtrl.h
@@ -20,7 +20,7 @@
 
 #include "../AcsParameters.h"
 #include "../SensorValues.h"
-#include "../config/classIds.h"
+#include "eive/resultClassIds.h"
 
 class PtgCtrl {
  public:
@@ -30,10 +30,10 @@ class PtgCtrl {
   PtgCtrl(AcsParameters *acsParameters_);
   virtual ~PtgCtrl();
 
-  static const uint8_t INTERFACE_ID = CLASS_ID::PTG;
+  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG;
   static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
 
-  /* @brief: Load AcsParameters für this class
+  /* @brief: Load AcsParameters for this class
    * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
    */
   void loadAcsParameters(AcsParameters *acsParameters_);
@@ -41,21 +41,32 @@ class PtgCtrl {
   /* @brief: Calculates the needed torque for the pointing control mechanism
    * @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
    */
-  void ptgGroundstation(const double mode, const double *qError, const double *deltaRate,
-                        const double *rwPseudoInv, double *torqueRws);
+  void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
+              const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
 
-  void ptgDesaturation(double *magFieldEst, bool magFieldEstValid, double *satRate,
+  void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
+                       double *magFieldEst, bool magFieldEstValid, double *satRate,
                        int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
                        double *mgtDpDes);
 
-  void ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
+  void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+                    const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
                     const int32_t *speedRw3, double *rwTrqNs);
 
+  /* @brief: Commands the stiction torque in case wheel speed is to low
+   * @param: rwAvailable 	Boolean Flag for all reaction wheels
+   * 		 omegaRw		current wheel speed of reaction wheels
+   * 		 torqueCommand  modified torque after antistiction
+   */
+  void rwAntistiction(const bool *rwAvailable, const int32_t *omegaRw, double *torqueCommand);
+
  private:
-  AcsParameters::PointingModeControllerParameters *pointingModeControllerParameters;
   AcsParameters::RwHandlingParameters *rwHandlingParameters;
   AcsParameters::InertiaEIVE *inertiaEIVE;
   AcsParameters::RwMatrices *rwMatrices;
+
+  double torqueMemory[4] = {0, 0, 0, 0};
+  double omegaMemory[4] = {0, 0, 0, 0};
 };
 
 #endif /* ACS_CONTROL_PTGCTRL_H_ */
diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index 54ae27ee..aa04cbb6 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -87,7 +87,7 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
   return returnvalue::OK;
 }
 
-// Will be the version in worst case scenario in event of no working MEKF (nor RMUs)
+// Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
 void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
                           bool sunRateBValid, double *magFieldB, bool magFieldBValid,
                           double *magRateB, bool magRateBValid, double *sunDirRef,
@@ -127,20 +127,17 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
   VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
 
   /* Only valid if angle between sun direction and magnetic field direction
-     is sufficiently large */
-
-  double sinAngle = 0;
-  sinAngle = sin(acos(cos(cosAngleSunMag)));
-
-  if (!(sinAngle > sin(safeModeControllerParameters->sunMagAngleMin * M_PI / 180))) {
+   * is sufficiently large */
+  double angleSunMag = acos(cosAngleSunMag);
+  if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) {
     return;
   }
 
-  //    Rate for Torque Calculation
+  // Rate for Torque Calculation
   double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
   VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
 
-  //    Torque Align calculation
+  // Torque Align calculation
   double kRateNoMekf = 0, kAlignNoMekf = 0;
   kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf;
   kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf;
diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h
index 72e45f08..1784f9ca 100644
--- a/mission/controller/acs/control/SafeCtrl.h
+++ b/mission/controller/acs/control/SafeCtrl.h
@@ -1,10 +1,3 @@
-/*
- * safeCtrl.h
- *
- *  Created on: 19 Apr 2022
- *      Author: rooob
- */
-
 #ifndef SAFECTRL_H_
 #define SAFECTRL_H_
 
@@ -14,14 +7,14 @@
 
 #include "../AcsParameters.h"
 #include "../SensorValues.h"
-#include "../config/classIds.h"
+#include "eive/resultClassIds.h"
 
 class SafeCtrl {
  public:
   SafeCtrl(AcsParameters *acsParameters_);
   virtual ~SafeCtrl();
 
-  static const uint8_t INTERFACE_ID = CLASS_ID::SAFE;
+  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
   static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
 
   void loadAcsParameters(AcsParameters *acsParameters_);
diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h
index 6484a72d..b8d1fa4d 100644
--- a/mission/controller/acs/util/MathOperations.h
+++ b/mission/controller/acs/util/MathOperations.h
@@ -96,8 +96,16 @@ class MathOperations {
 
   static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
                                       T2 *cartesianOutput) {
-    double radiusPolar = 6378137;
-    double radiusEqua = 6356752.314;
+    /* @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));
@@ -106,13 +114,13 @@ class MathOperations {
     cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
     cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
   }
-
-  /* @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]
-   * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
-   * 			Landis Markley and John L. Crassidis*/
-  static void dcmEJ(timeval time, T1 *outputDcmEJ) {
+  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)
@@ -143,6 +151,16 @@ class MathOperations {
     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
@@ -215,7 +233,7 @@ class MathOperations {
     precession[2][2] = cos(theta2);
 
     //-------------------------------------------------------------------------------------
-    //		Calculation of Transformation from earth Nutation size
+    //		Calculation of Transformation from earth Nutation N
     //-------------------------------------------------------------------------------------
     double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
     //      lunar asc node
@@ -230,7 +248,6 @@ class MathOperations {
 
     //        % true obliquity of the ecliptic eps p.71 (simplified)
     double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
-    ;
 
     nutation[0][0] = cos(dp);
     nutation[1][0] = cos(e + de) * sin(dp);
@@ -260,10 +277,9 @@ class MathOperations {
 
     MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
   }
-
   static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
     int i, j;
-    double determinant;
+    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]}};
@@ -272,8 +288,8 @@ class MathOperations {
       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<<"\size\ndeterminant: "<<determinant;
-    //		cout<<"\size\nInverse of matrix is: \size";
+    //		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]) -
diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
index d9b1deea..e3908bd0 100644
--- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
+++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
@@ -86,6 +86,8 @@ enum PoolIds : lp_id_t {
   // GPS Processed
   GC_LATITUDE,
   GD_LONGITUDE,
+  GPS_POSITION,
+  GPS_VELOCITY,
   // MEKF
   SAT_ROT_RATE_MEKF,
   QUAT_MEKF,
@@ -99,13 +101,13 @@ enum PoolIds : lp_id_t {
   MTQ_TARGET_DIPOLE,
 };
 
-static constexpr uint8_t MGM_SET_RAW_ENTRIES = 10;
+static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
 static constexpr uint8_t MGM_SET_PROCESSED_ENTRIES = 8;
 static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
 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 = 2;
+static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 4;
 static constexpr uint8_t MEKF_SET_ENTRIES = 2;
 static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
@@ -224,6 +226,8 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
 
   lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
   lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
+  lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
+  lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);
 
  private:
 };
diff --git a/mission/core/GenericFactory.cpp b/mission/core/GenericFactory.cpp
index ae77bf85..6faa0601 100644
--- a/mission/core/GenericFactory.cpp
+++ b/mission/core/GenericFactory.cpp
@@ -123,18 +123,19 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
       new CcsdsDistributor(config::EIVE_PUS_APID, objects::CCSDS_PACKET_DISTRIBUTOR);
   new PusDistributor(config::EIVE_PUS_APID, objects::PUS_PACKET_DISTRIBUTOR, ccsdsDistrib);
 
-  *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, config::EIVE_CFDP_APID, *tmStore, 50, 15,
-                                 *ipcStore);
-  *pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, *timeStamper, *tmStore, sdcMan,
-                               config::MAX_PUS_FUNNEL_QUEUE_DEPTH, 15, *ipcStore);
+  PusTmFunnel::FunnelCfg cfdpFunnelCfg(objects::CFDP_TM_FUNNEL, *tmStore, *ipcStore, 50, 15);
+  *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, config::EIVE_CFDP_APID);
+  PusTmFunnel::FunnelCfg pusFunnelCfg(objects::PUS_TM_FUNNEL, *tmStore, *ipcStore,
+                                      config::MAX_PUS_FUNNEL_QUEUE_DEPTH, 15);
+  *pusFunnel = new PusTmFunnel(pusFunnelCfg, *timeStamper, sdcMan);
 #if OBSW_ADD_TCPIP_SERVERS == 1
 #if OBSW_ADD_TMTC_UDP_SERVER == 1
-  (*cfdpFunnel)->addDestination(*udpBridge, 0);
-  (*pusFunnel)->addDestination(*udpBridge, 0);
+  (*cfdpFunnel)->addDestination("UDP Server", *udpBridge, 0);
+  (*pusFunnel)->addDestination("UDP Server", *udpBridge, 0);
 #endif
 #if OBSW_ADD_TMTC_TCP_SERVER == 1
-  (*cfdpFunnel)->addDestination(*tcpBridge, 0);
-  (*pusFunnel)->addDestination(*tcpBridge, 0);
+  (*cfdpFunnel)->addDestination("TCP Server", *tcpBridge, 0);
+  (*pusFunnel)->addDestination("TCP Server", *tcpBridge, 0);
 #endif
 #endif
   // Every TM packet goes through this funnel
diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h
index 0b88cfa5..8acb77e9 100644
--- a/mission/devices/devicedefinitions/GPSDefinitions.h
+++ b/mission/devices/devicedefinitions/GPSDefinitions.h
@@ -13,6 +13,9 @@ static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER;
 //! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix
 //! 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix
 static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO);
+//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. P1: Maximum allowed time
+//! to get a fix after the GPS was switched on.
+static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW);
 
 static constexpr DeviceCommandId_t GPS_REPLY = 0;
 static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;
diff --git a/mission/system/objects/AcsSubsystem.cpp b/mission/system/objects/AcsSubsystem.cpp
index f5b8a0ce..dcf4e67b 100644
--- a/mission/system/objects/AcsSubsystem.cpp
+++ b/mission/system/objects/AcsSubsystem.cpp
@@ -58,7 +58,7 @@ void AcsSubsystem::handleEventMessages() {
       case EventMessage::EVENT_MESSAGE:
         if (event.getEvent() == acs::SAFE_RATE_VIOLATION) {
           CommandMessage msg;
-          ModeMessage::setCmdModeModeMessage(msg, acs::CtrlSubmode::DETUMBLE, 0);
+          ModeMessage::setCmdModeMessage(msg, acs::AcsMode::DETUMBLE, 0);
           ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
           if (result != returnvalue::OK) {
             sif::error << "AcsSubsystem: sending DETUMBLE mode cmd to self has failed" << std::endl;
@@ -66,7 +66,7 @@ void AcsSubsystem::handleEventMessages() {
         }
         if (event.getEvent() == acs::SAFE_RATE_RECOVERY) {
           CommandMessage msg;
-          ModeMessage::setCmdModeModeMessage(msg, acs::CtrlSubmode::SAFE, 0);
+          ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
           ReturnValue_t result = commandQueue->sendMessage(commandQueue->getId(), &msg);
           if (result != returnvalue::OK) {
             sif::error << "AcsSubsystem: sending IDLE mode cmd to self has failed" << std::endl;
diff --git a/mission/system/tree/acsModeTree.cpp b/mission/system/tree/acsModeTree.cpp
index 849959f9..0474d0a1 100644
--- a/mission/system/tree/acsModeTree.cpp
+++ b/mission/system/tree/acsModeTree.cpp
@@ -30,69 +30,69 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& entryHelper);
 static const auto OFF = HasModesIF::MODE_OFF;
 static const auto NML = DeviceHandlerIF::MODE_NORMAL;
 
-auto ACS_SEQUENCE_OFF = std::make_pair(acs::CtrlSubmode::OFF, FixedArrayList<ModeListEntry, 3>());
+auto ACS_SEQUENCE_OFF = std::make_pair(acs::AcsMode::OFF, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_OFF_TGT =
-    std::make_pair((acs::CtrlSubmode::OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((acs::AcsMode::OFF << 24) | 1, FixedArrayList<ModeListEntry, 1>());
 auto ACS_TABLE_OFF_TRANS_0 =
-    std::make_pair((acs::CtrlSubmode::OFF << 24) | 2, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((acs::AcsMode::OFF << 24) | 2, FixedArrayList<ModeListEntry, 1>());
 auto ACS_TABLE_OFF_TRANS_1 =
-    std::make_pair((acs::CtrlSubmode::OFF << 24) | 3, FixedArrayList<ModeListEntry, 6>());
+    std::make_pair((acs::AcsMode::OFF << 24) | 3, FixedArrayList<ModeListEntry, 6>());
 
 auto ACS_SEQUENCE_DETUMBLE =
-    std::make_pair(acs::CtrlSubmode::DETUMBLE, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(acs::AcsMode::DETUMBLE, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_DETUMBLE_TGT =
-    std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
+    std::make_pair((acs::AcsMode::DETUMBLE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_DETUMBLE_TRANS_0 =
-    std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
+    std::make_pair((acs::AcsMode::DETUMBLE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 auto ACS_TABLE_DETUMBLE_TRANS_1 =
-    std::make_pair((acs::CtrlSubmode::DETUMBLE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((acs::AcsMode::DETUMBLE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
 
-auto ACS_SEQUENCE_SAFE = std::make_pair(acs::CtrlSubmode::SAFE, FixedArrayList<ModeListEntry, 3>());
+auto ACS_SEQUENCE_SAFE = std::make_pair(acs::AcsMode::SAFE, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_SAFE_TGT =
-    std::make_pair((acs::CtrlSubmode::SAFE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
+    std::make_pair((acs::AcsMode::SAFE << 24) | 1, FixedArrayList<ModeListEntry, 4>());
 auto ACS_TABLE_SAFE_TRANS_0 =
-    std::make_pair((acs::CtrlSubmode::SAFE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
+    std::make_pair((acs::AcsMode::SAFE << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 auto ACS_TABLE_SAFE_TRANS_1 =
-    std::make_pair((acs::CtrlSubmode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((acs::AcsMode::SAFE << 24) | 3, FixedArrayList<ModeListEntry, 1>());
 
-auto ACS_SEQUENCE_IDLE = std::make_pair(acs::CtrlSubmode::IDLE, FixedArrayList<ModeListEntry, 3>());
+auto ACS_SEQUENCE_IDLE = std::make_pair(acs::AcsMode::PTG_IDLE, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_IDLE_TGT =
-    std::make_pair((acs::CtrlSubmode::IDLE << 24) | 1, FixedArrayList<ModeListEntry, 6>());
+    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 1, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_IDLE_TRANS_0 =
-    std::make_pair((acs::CtrlSubmode::IDLE << 24) | 2, FixedArrayList<ModeListEntry, 6>());
+    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 2, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_IDLE_TRANS_1 =
-    std::make_pair((acs::CtrlSubmode::IDLE << 24) | 3, FixedArrayList<ModeListEntry, 2>());
+    std::make_pair((acs::AcsMode::PTG_IDLE << 24) | 3, FixedArrayList<ModeListEntry, 2>());
 
 auto ACS_TABLE_PTG_TRANS_0 =
-    std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 2, FixedArrayList<ModeListEntry, 5>());
+    std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 2, FixedArrayList<ModeListEntry, 5>());
 
 auto ACS_SEQUENCE_PTG_TARGET =
-    std::make_pair(acs::CtrlSubmode::PTG_TARGET, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(acs::AcsMode::PTG_TARGET, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_PTG_TARGET_TGT =
-    std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 1, FixedArrayList<ModeListEntry, 6>());
+    std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 1, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_PTG_TARGET_TRANS_1 =
-    std::make_pair((acs::CtrlSubmode::PTG_TARGET << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((acs::AcsMode::PTG_TARGET << 24) | 3, FixedArrayList<ModeListEntry, 1>());
 
 auto ACS_SEQUENCE_PTG_TARGET_GS =
-    std::make_pair(acs::CtrlSubmode::PTG_TARGET_GS, FixedArrayList<ModeListEntry, 3>());
+    std::make_pair(acs::AcsMode::PTG_TARGET_GS, FixedArrayList<ModeListEntry, 3>());
 auto ACS_TABLE_PTG_TARGET_GS_TGT =
-    std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 1, FixedArrayList<ModeListEntry, 6>());
+    std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 1, FixedArrayList<ModeListEntry, 6>());
 auto ACS_TABLE_PTG_TARGET_GS_TRANS_1 =
-    std::make_pair((acs::CtrlSubmode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair((acs::AcsMode::PTG_TARGET_GS << 24) | 3, FixedArrayList<ModeListEntry, 1>());
 
 auto ACS_SEQUENCE_PTG_TARGET_NADIR =
-    std::make_pair(acs::CtrlSubmode::PTG_TARGET_NADIR, FixedArrayList<ModeListEntry, 3>());
-auto ACS_TABLE_PTG_TARGET_NADIR_TGT = std::make_pair((acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 1,
-                                                     FixedArrayList<ModeListEntry, 6>());
-auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 = std::make_pair(
-    (acs::CtrlSubmode::PTG_TARGET_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair(acs::AcsMode::PTG_NADIR, FixedArrayList<ModeListEntry, 3>());
+auto ACS_TABLE_PTG_TARGET_NADIR_TGT =
+    std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 1, FixedArrayList<ModeListEntry, 6>());
+auto ACS_TABLE_PTG_TARGET_NADIR_TRANS_1 =
+    std::make_pair((acs::AcsMode::PTG_NADIR << 24) | 3, FixedArrayList<ModeListEntry, 1>());
 
 auto ACS_SEQUENCE_PTG_TARGET_INERTIAL =
-    std::make_pair(acs::CtrlSubmode::PTG_TARGET_INERTIAL, FixedArrayList<ModeListEntry, 3>());
-auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT = std::make_pair(
-    (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>());
-auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 = std::make_pair(
-    (acs::CtrlSubmode::PTG_TARGET_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>());
+    std::make_pair(acs::AcsMode::PTG_INERTIAL, FixedArrayList<ModeListEntry, 3>());
+auto ACS_TABLE_PTG_TARGET_INERTIAL_TGT =
+    std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 1, FixedArrayList<ModeListEntry, 6>());
+auto ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1 =
+    std::make_pair((acs::AcsMode::PTG_INERTIAL << 24) | 3, FixedArrayList<ModeListEntry, 1>());
 
 void satsystem::acs::init() {
   ModeListEntry entry;
@@ -122,7 +122,7 @@ void satsystem::acs::init() {
   buildTargetPtGsSequence(ACS_SUBSYSTEM, entry);
   buildTargetPtNadirSequence(ACS_SUBSYSTEM, entry);
   buildTargetPtInertialSequence(ACS_SUBSYSTEM, entry);
-  ACS_SUBSYSTEM.setInitialMode(::acs::CtrlSubmode::SAFE);
+  ACS_SUBSYSTEM.setInitialMode(::acs::AcsMode::SAFE);
 }
 
 namespace {
@@ -190,7 +190,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
     check(sequence.insert(eh), ctxc);
   };
   // Build SAFE target
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::SAFE, ACS_TABLE_SAFE_TGT.second);
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TGT.second);
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_SAFE_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second);
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second);
@@ -206,7 +206,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build SAFE transition 1
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::SAFE, ACS_TABLE_SAFE_TRANS_1.second);
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::SAFE, ACS_TABLE_SAFE_TRANS_1.second);
   check(ss.addTable(&ACS_TABLE_SAFE_TRANS_1.second, ACS_TABLE_SAFE_TRANS_1.first, false, true),
         ctxc);
 
@@ -239,7 +239,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {
     check(sequence.insert(eh), ctxc);
   };
   // Build DETUMBLE target
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second);
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::DETUMBLE, ACS_TABLE_DETUMBLE_TGT.second);
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_DETUMBLE_TGT.second);
@@ -257,7 +257,7 @@ void buildDetumbleSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build DETUMBLE transition 1
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second);
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::DETUMBLE, ACS_TABLE_DETUMBLE_TRANS_1.second);
   check(ss.addTable(&ACS_TABLE_DETUMBLE_TRANS_1.second, ACS_TABLE_DETUMBLE_TRANS_1.first, false,
                     true),
         ctxc);
@@ -291,7 +291,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
     check(sequence.insert(eh), ctxc);
   };
   // Build IDLE target
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TGT.second);
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TGT.second);
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_IDLE_TGT.second);
   iht(objects::RW_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second);
@@ -307,7 +307,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
   ss.addTable(&ACS_TABLE_IDLE_TRANS_0.second, ACS_TABLE_IDLE_TRANS_0.first, false, true);
 
   // Build IDLE transition 1
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::IDLE, ACS_TABLE_IDLE_TRANS_1.second);
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_IDLE, ACS_TABLE_IDLE_TRANS_1.second);
   ss.addTable(&ACS_TABLE_IDLE_TRANS_1.second, ACS_TABLE_IDLE_TRANS_1.first, false, true);
 
   // Build IDLE sequence
@@ -339,7 +339,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
   };
 
   // Build TARGET PT table
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second);
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
@@ -350,8 +350,7 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
 
   // Transition 0 already built
   // Build TARGET PT transition 1
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET,
-      ACS_TABLE_PTG_TARGET_TRANS_1.second);
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET, ACS_TABLE_PTG_TARGET_TRANS_1.second);
   check(ss.addTable(&ACS_TABLE_PTG_TARGET_TRANS_1.second, ACS_TABLE_PTG_TARGET_TRANS_1.first, false,
                     true),
         ctxc);
@@ -386,7 +385,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
   };
 
   // Build TARGET PT table
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET,
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET,
       ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
@@ -399,7 +398,7 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
 
   // Transition 0 already built
   // Build TARGET PT transition 1
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_NADIR,
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_NADIR,
       ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second);
   check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.first,
                                &ACS_TABLE_PTG_TARGET_NADIR_TRANS_1.second)),
@@ -436,7 +435,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
   };
 
   // Build TARGET PT table
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_GS,
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS,
       ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
@@ -449,7 +448,7 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
 
   // Transition 0 already built
   // Build TARGET PT transition 1
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_GS,
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_TARGET_GS,
       ACS_TABLE_PTG_TARGET_GS_TRANS_1.second);
   check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_GS_TRANS_1.first,
                                &ACS_TABLE_PTG_TARGET_GS_TRANS_1.second)),
@@ -485,7 +484,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
   };
 
   // Build TARGET PT table
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL,
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL,
       ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   iht(objects::IMTQ_HANDLER, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
@@ -498,7 +497,7 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
 
   // Transition 0 already built
   // Build TARGET PT transition 1
-  iht(objects::ACS_CONTROLLER, NML, acs::CtrlSubmode::PTG_TARGET_INERTIAL,
+  iht(objects::ACS_CONTROLLER, NML, acs::AcsMode::PTG_INERTIAL,
       ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second);
   check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.first,
                                &ACS_TABLE_PTG_TARGET_INERTIAL_TRANS_1.second)),
diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp
index f2d31a92..4c9dd57f 100644
--- a/mission/tmtc/CfdpTmFunnel.cpp
+++ b/mission/tmtc/CfdpTmFunnel.cpp
@@ -4,11 +4,8 @@
 #include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
 
-CfdpTmFunnel::CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid,
-                           StorageManagerIF& tmStore, uint32_t tmMsgDepth, uint32_t tcMsgDepth,
-                           StorageManagerIF& ipcStore)
-    : TmFunnelBase(objectId, tmStore, tmMsgDepth, tcMsgDepth, ipcStore),
-      cfdpInCcsdsApid(cfdpInCcsdsApid) {}
+CfdpTmFunnel::CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, uint16_t cfdpInCcsdsApid)
+    : TmFunnelBase(cfg), cfdpInCcsdsApid(cfdpInCcsdsApid) {}
 
 const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; }
 
@@ -70,7 +67,7 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
   msg.setStorageId(newStoreId);
   store_address_t origStoreId = newStoreId;
   for (unsigned int idx = 0; idx < destinations.size(); idx++) {
-    const auto& destVcidPair = destinations[idx];
+    const auto& dest = destinations[idx];
     if (destinations.size() > 1) {
       if (idx < destinations.size() - 1) {
         // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
@@ -89,10 +86,11 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
         msg.setStorageId(origStoreId);
       }
     }
-    result = tmQueue->sendMessage(destVcidPair.first, &msg);
+    result = tmQueue->sendMessage(dest.queueId, &msg);
     if (result != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
+      sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler " << dest.name
+                 << " failed" << std::endl;
 #endif
       tmStore.deleteData(msg.getStorageId());
     }
diff --git a/mission/tmtc/CfdpTmFunnel.h b/mission/tmtc/CfdpTmFunnel.h
index 55917727..717573a0 100644
--- a/mission/tmtc/CfdpTmFunnel.h
+++ b/mission/tmtc/CfdpTmFunnel.h
@@ -12,8 +12,7 @@
 
 class CfdpTmFunnel : public TmFunnelBase {
  public:
-  CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, StorageManagerIF& tmStore,
-               uint32_t tmMsgDepth, uint32_t tcMsgDepth, StorageManagerIF& ipcStore);
+  CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, uint16_t cfdpInCcsdsApid);
   [[nodiscard]] const char* getName() const override;
   ReturnValue_t performOperation(uint8_t opCode);
   ReturnValue_t initialize() override;
diff --git a/mission/tmtc/PersistentTmStore.cpp b/mission/tmtc/PersistentTmStore.cpp
index 8b0c4ee4..92cff08d 100644
--- a/mission/tmtc/PersistentTmStore.cpp
+++ b/mission/tmtc/PersistentTmStore.cpp
@@ -57,7 +57,7 @@ ReturnValue_t TmStore::passPacket(PusTmReader& reader) {
   return returnvalue::OK;
 }
 
-void TmStore::dumpFrom(uint32_t fromUnixSeconds, TmFunnelBase &tmFunnel) {
+void TmStore::dumpFrom(uint32_t fromUnixSeconds, TmFunnelBase& tmFunnel) {
   return dumpFromUpTo(fromUnixSeconds, currentTv.tv_sec, tmFunnel);
 }
 
diff --git a/mission/tmtc/PusTmFunnel.cpp b/mission/tmtc/PusTmFunnel.cpp
index 7da02bca..ec6aa0f7 100644
--- a/mission/tmtc/PusTmFunnel.cpp
+++ b/mission/tmtc/PusTmFunnel.cpp
@@ -8,10 +8,9 @@
 #include "fsfw/tmstorage/TmStoreMessage.h"
 #include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h"
 
-PusTmFunnel::PusTmFunnel(object_id_t objectId, TimeReaderIF &timeReader, StorageManagerIF &tmStore,
-                         SdCardMountedIF &sdcMan, uint32_t tmMsgDepth, uint32_t tcMsgDepth,
-                         StorageManagerIF &ipcStore)
-    : TmFunnelBase(objectId, tmStore, tmMsgDepth, tcMsgDepth, ipcStore),
+PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, TimeReaderIF &timeReader,
+                         SdCardMountedIF &sdcMan)
+    : TmFunnelBase(cfg),
       timeReader(timeReader),
       miscStore(objects::MISC_TM_STORE, "tm", "misc", RolloverInterval::HOURLY, 2, currentTv,
                 tmStore, sdcMan),
@@ -107,17 +106,21 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
   TmTcMessage currentMessage;
   status = tmQueue->receiveMessage(&currentMessage);
   while (status == returnvalue::OK) {
-    status = handlePacket(currentMessage);
+    status = handleTmPacket(currentMessage);
     if (status != returnvalue::OK) {
       sif::warning << "TmFunnel packet handling failed" << std::endl;
       break;
     }
     status = tmQueue->receiveMessage(&currentMessage);
   }
+
+  if (status == MessageQueueIF::EMPTY) {
+    return returnvalue::OK;
+  }
   return status;
 }
 
-ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) {
+ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) {
   uint8_t *packetData = nullptr;
   size_t size = 0;
   store_address_t origStoreId = message.getStorageId();
diff --git a/mission/tmtc/PusTmFunnel.h b/mission/tmtc/PusTmFunnel.h
index e47f5e96..487931a3 100644
--- a/mission/tmtc/PusTmFunnel.h
+++ b/mission/tmtc/PusTmFunnel.h
@@ -22,9 +22,7 @@
  */
 class PusTmFunnel : public TmFunnelBase {
  public:
-  explicit PusTmFunnel(object_id_t objectId, TimeReaderIF &timeReader, StorageManagerIF &tmStore,
-                       SdCardMountedIF &sdcMan, uint32_t tmMsgDepth, uint32_t tcMsgDepth,
-                       StorageManagerIF &ipcStore);
+  PusTmFunnel(TmFunnelBase::FunnelCfg cfg, TimeReaderIF &timeReader, SdCardMountedIF &sdcMan);
   [[nodiscard]] const char *getName() const override;
   ~PusTmFunnel() override;
 
@@ -45,7 +43,7 @@ class PusTmFunnel : public TmFunnelBase {
   TmStore hkStore;
   SdCardMountedIF &sdcMan;
 
-  ReturnValue_t handlePacket(TmTcMessage &message);
+  ReturnValue_t handleTmPacket(TmTcMessage &message);
   void initStoresIfPossible(bool sdCardUsable);
   ReturnValue_t initialize() override;
 };
diff --git a/mission/tmtc/TmFunnelBase.cpp b/mission/tmtc/TmFunnelBase.cpp
index 0f600efe..2489186d 100644
--- a/mission/tmtc/TmFunnelBase.cpp
+++ b/mission/tmtc/TmFunnelBase.cpp
@@ -4,24 +4,22 @@
 
 #include "fsfw/ipc/QueueFactory.h"
 
-TmFunnelBase::TmFunnelBase(object_id_t objectId, StorageManagerIF &tmStore, uint32_t tmMsgDepth,
-                           uint32_t tcMsgDepth, StorageManagerIF &ipcStore)
-    : SystemObject(objectId), tmStore(tmStore), ipcStore(ipcStore) {
-  tmQueue = QueueFactory::instance()->createMessageQueue(tmMsgDepth);
-  tcQueue = QueueFactory::instance()->createMessageQueue(tcMsgDepth);
+TmFunnelBase::TmFunnelBase(FunnelCfg cfg)
+    : SystemObject(cfg.objectId), tmStore(cfg.tmStore), ipcStore(cfg.ipcStore) {
+  tmQueue = QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth);
+  tcQueue = QueueFactory::instance()->createMessageQueue(cfg.tcMsgDepth);
 }
 
-MessageQueueId_t TmFunnelBase::getCommandQueue() const { return tcQueue->getId(); }
-
 TmFunnelBase::~TmFunnelBase() { QueueFactory::instance()->deleteMessageQueue(tmQueue); }
 
 MessageQueueId_t TmFunnelBase::getReportReceptionQueue(uint8_t virtualChannel) const {
   return tmQueue->getId();
 }
 
-void TmFunnelBase::addDestination(const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) {
+void TmFunnelBase::addDestination(const char *name, const AcceptsTelemetryIF &downlinkDestination,
+                                  uint8_t vcid) {
   auto queueId = downlinkDestination.getReportReceptionQueue(vcid);
-  destinations.emplace_back(queueId, vcid);
+  destinations.emplace_back(name, queueId, vcid);
 }
 
 ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId,
@@ -29,7 +27,7 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
                                                      const uint8_t *packetData, size_t size) {
   ReturnValue_t result;
   for (unsigned int idx = 0; idx < destinations.size(); idx++) {
-    const auto &destVcidPair = destinations[idx];
+    const auto &dest = destinations[idx];
     if (destinations.size() > 1) {
       if (idx < destinations.size() - 1) {
         // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
@@ -48,7 +46,7 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
         message.setStorageId(origStoreId);
       }
     }
-    result = tmQueue->sendMessage(destVcidPair.first, &message);
+    result = tmQueue->sendMessage(dest.queueId, &message);
     if (result != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
@@ -58,3 +56,5 @@ ReturnValue_t TmFunnelBase::sendPacketToDestinations(store_address_t origStoreId
   }
   return result;
 }
+
+MessageQueueId_t TmFunnelBase::getCommandQueue() const { return tcQueue->getId(); }
diff --git a/mission/tmtc/TmFunnelBase.h b/mission/tmtc/TmFunnelBase.h
index bfff3440..97b627b4 100644
--- a/mission/tmtc/TmFunnelBase.h
+++ b/mission/tmtc/TmFunnelBase.h
@@ -13,13 +13,25 @@ class TmFunnelBase : public TmStoreFrontendSimpleIF,
                      public AcceptsTelemetryIF,
                      public SystemObject {
  public:
-  TmFunnelBase(object_id_t objectId, StorageManagerIF& tmStore, uint32_t tmMsgDepth,
-               uint32_t tcMsgDepth, StorageManagerIF& ipcStore);
-  void addDestination(const AcceptsTelemetryIF& downlinkDestination, uint8_t vcid = 0);
+  struct FunnelCfg {
+    FunnelCfg(object_id_t objId, StorageManagerIF& tmStore, StorageManagerIF& ipcStore,
+              uint32_t tmMsgDepth, uint32_t tcMsgDepth)
+        : objectId(objId),
+          tmStore(tmStore),
+          ipcStore(ipcStore),
+          tmMsgDepth(tmMsgDepth),
+          tcMsgDepth(tcMsgDepth) {}
+    object_id_t objectId;
+    StorageManagerIF& tmStore;
+    StorageManagerIF& ipcStore;
+    uint32_t tmMsgDepth;
+    uint32_t tcMsgDepth;
+  };
+  TmFunnelBase(FunnelCfg cfg);
+  void addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
+                      uint8_t vcid = 0);
   ReturnValue_t sendPacketToDestinations(store_address_t origStoreId, TmTcMessage& message,
                                          const uint8_t* packetData, size_t size);
-
-  [[nodiscard]] MessageQueueId_t getCommandQueue() const override;
   [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override;
 
   virtual ~TmFunnelBase();
@@ -27,9 +39,22 @@ class TmFunnelBase : public TmStoreFrontendSimpleIF,
  protected:
   StorageManagerIF& tmStore;
   StorageManagerIF& ipcStore;
-  std::vector<std::pair<MessageQueueId_t, uint8_t>> destinations;
-  MessageQueueIF* tcQueue = nullptr;
+
+  struct Destination {
+    Destination(const char* name, MessageQueueId_t queueId, uint8_t virtualChannel)
+        : name(name), queueId(queueId), virtualChannel(virtualChannel) {}
+
+    const char* name;
+    MessageQueueId_t queueId;
+    uint8_t virtualChannel = 0;
+  };
+
+  std::vector<Destination> destinations;
+
   MessageQueueIF* tmQueue = nullptr;
+  MessageQueueIF* tcQueue = nullptr;
+
+  MessageQueueId_t getCommandQueue() const override;
 };
 
 #endif /* MISSION_TMTC_TMFUNNELBASE_H_ */
diff --git a/release_checklist.md b/release_checklist.md
new file mode 100644
index 00000000..b4f08165
--- /dev/null
+++ b/release_checklist.md
@@ -0,0 +1,12 @@
+OBSW Release Checklist
+=========
+
+# Pre-Release
+
+1. Update version in `CMakeLists.txt`
+2. Verify that the Q7S, Q7S EM and Host build are working
+3. Wait for CI/CD results
+
+# Post-Release
+
+1. Create a new release with tag on `EGit`
diff --git a/tmtc b/tmtc
index 2bd6caa3..d0c8e20d 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 2bd6caa3c21255f2ab5a2773eb83d2fca78c2234
+Subproject commit d0c8e20d4f9a6f5aee3ccfa05c4f7ab7151400b5