2023-06-22 16:45:15 +02:00
62 changed files with 943 additions and 671 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -18,20 +18,67 @@ will consitute of a breaking change warranting a new major release:

 # [v4.0.0] to be released

- eive-tmtc: v4.0.0 (to be released)
-TODO: New firmware package version.
+- `eive-tmtc` version v4.0.0
+- `q7s-package` version v3.0.0

 ## Fixed

- CFDP low level protocol bugfix. Requires fsfw update and tmtc update.
 - Important bugfixes for PTME. See `q7s-package` CHANGELOG.
+- Fixed H parameter in SUS converter from 1 mm to 2.5 mm.

 ## Changed

 - Removed PTME busy/ready signals. Those were not used anyway because register reads are used now.
 - APB bus access busy checking is not done anymore as this is performed by the bus itself now.

-# [v3.0.0] to be released
+## Added
+
+- Added PL I2C reset pin. It is not used for now but could be used for FDIR procedures to restore
+  the PL I2C.
+
+# [v3.3.0] 2023-06-21
+
+Like v3.2.0 but without the custom FM changes related to VC0.
+
+# [v3.2.0] 2023-06-21
+
+## Fixed
+
+- Fix sun vector calculation
+- SUS total vector was not reset to being a zero vector during eclipse due to a wrong memcpy
+  length.
+
+## Changed
+
+- Reverted all changes related to VC0 handling to avoid FM bug possibly related to FPGA bug.
+
+# [v3.1.1] 2023-06-14
+
+## Fixed
+
+- TMP1075 bugfix where negative temperatures could not be measured because of a two's-complement
+  conversion bug.
+
+# [v3.1.0] 2023-06-14
+
+- `eive-tmtc` version v4.1.0
+
+## Fixed
+
+- TCS heater switch enumeration naming was old/wrong and was not updated in sync with the object ID
+  update. This lead to the TCS controller commanding the wrong heaters.
+
+## Changed
+
+- Increase number of allowed parallel HK commands to 16
+
+## Added
+
+- Added `CONFIG_SET`, `MAN_HEATER_ON` and `MAN_HEATER_OFF` support for the BPX battery handler
+
+# [v3.0.0] 2023-06-11
+
+- `eive-tmtc` version v4.0.0

 ## Changed

@ -66,6 +113,13 @@ TODO: New firmware package version.
  only be used to cancel a transfer.
 - Handling of multiple RWs in the ACS Controller is improved and can be changed by parameter
  commands.
+- The Directory Listing direct dumper now has a state machine to stagger the directory listing dump.
+  This is required because very large dumps will overload the queue capacities in the framework.
+- The PUS Service 8 now has larger queue sizes to handle more action replies. The PUS Service 1
+  also has a larger queue size to handle a lot of step replies now.
+- Moved PDU `vcc` and `vbat` variable from auxiliary dataset to core dataset.
+- Tweak TCP/IP configuration: Only the TCP server will be included on the EM. For the FM, no
+  TCP/IP servers will be included by default.

 ## Added

@ -75,9 +129,13 @@ TODO: New firmware package version.
 - ACU dummy HK sets
 - IMTQ HK sets
 - IMTQ dummy now handles power switch
+- Added some new ACS parameters
+- Enabled decimation filter for the ADIS GYRs
+- Enabled second low-pass filter for L3GD20H GYRs

 ## Fixed

+- CFDP low level protocol bugfix. Requires `fsfw` update and `tmtc` update.
 - Compile fix if SCEX is compiled for the EM.
 - Set up Rad Sensor chip select even for EM to avoid SPI bus issues.
 - Correct ADIS Gyroscope type configuration for the EM, where the 16507 type is used instead of the
@ -106,8 +164,13 @@ TODO: New firmware package version.
 - Prevent spam of TCS controller heater unavailability event if all heaters are in external control.
 - TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS
  controller. There is not crash risk but the heater states were invalid.
+- STR datasets were not set to invalid on shutdown.
 - Fixed usage of quaternion valid flag, which does not actually represent the validity of the
  quaternion.
+- Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as
+  intended.
+- The variance for the ADIS GYRs now represents the used `-3` version and not the `-1` version
+- CFDP funnel did not route packets to live channel VC0

 # [v2.0.5] 2023-05-11

--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -79,6 +79,13 @@ else()
  set(INIT_VAL 1)
  set(OBSW_STAR_TRACKER_GROUND_CONFIG 0)
 endif()
+
+set(OBSW_ADD_TMTC_TCP_SERVER
+    ${OBSW_Q7S_EM}
+    CACHE STRING "Add TCP TMTC Server")
+set(OBSW_ADD_TMTC_UDP_SERVER
+    0
+    CACHE STRING "Add UDP TMTC Server")
 set(OBSW_ADD_MGT
    ${INIT_VAL}
    CACHE STRING "Add MGT module")
--- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp
+++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 295 translations.
+ * @brief    Auto-generated event translation file. Contains 296 translations.
 * @details
- * Generated on: 2023-05-17 17:15:34
+ * Generated on: 2023-06-21 19:01:02
 */
 #include "translateEvents.h"

@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS";
 const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY";
 const char *I2C_REBOOT_STRING = "I2C_REBOOT";
 const char *PDEC_REBOOT_STRING = "PDEC_REBOOT";
+const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO";
 const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE";
 const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE";
 const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING";
@ -847,6 +848,8 @@ const char *translateEvents(Event event) {
      return I2C_REBOOT_STRING;
    case (14012):
      return PDEC_REBOOT_STRING;
+    case (14013):
+      return FIRMWARE_INFO_STRING;
    case (14100):
      return NO_VALID_SENSOR_TEMPERATURE_STRING;
    case (14101):
--- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
+++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
@ -2,7 +2,7 @@
 * @brief  Auto-generated object translation file.
 * @details
 * Contains 171 translations.
- * Generated on: 2023-05-17 17:15:34
+ * Generated on: 2023-06-21 19:01:02
 */
 #include "translateObjects.h"

--- a/bsp_q7s/OBSWConfig.h.in
+++ b/bsp_q7s/OBSWConfig.h.in
@ -67,8 +67,8 @@
 // because UDP packets are not allowed in the VPN
 // This will cause the OBSW to initialize the TMTC bridge responsible for exchanging data with the
 // CCSDS IP Cores. 
-#define OBSW_ADD_TMTC_TCP_SERVER                    1
-#define OBSW_ADD_TMTC_UDP_SERVER                    1
+#define OBSW_ADD_TMTC_TCP_SERVER                    @OBSW_ADD_TMTC_TCP_SERVER@
+#define OBSW_ADD_TMTC_UDP_SERVER                    @OBSW_ADD_TMTC_UDP_SERVER@

 // Can be used to switch device to NORMAL mode immediately
 #define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP    0
--- a/bsp_q7s/boardconfig/busConf.h
+++ b/bsp_q7s/boardconfig/busConf.h
@ -23,6 +23,7 @@ static constexpr char UART_SCEX_DEV[] = "/dev/scex";
 static constexpr char UIO_PDEC_REGISTERS[] = "/dev/uio_pdec_regs";
 static constexpr char UIO_PTME[] = "/dev/uio_ptme";
 static constexpr char UIO_PDEC_CONFIG_MEMORY[] = "/dev/uio_pdec_cfg_mem";
+static constexpr char UIO_SYS_ROM[] = "/dev/uio_sys_rom";
 static constexpr char UIO_PDEC_RAM[] = "/dev/uio_pdec_ram";
 static constexpr char UIO_PDEC_IRQ[] = "/dev/uio_pdec_irq";
 static constexpr int MAP_ID_PTME_CONFIG = 3;
@ -57,6 +58,7 @@ static constexpr char GYRO_0_ENABLE[] = "enable_gyro_0";
 static constexpr char GYRO_2_ENABLE[] = "enable_gyro_2";
 static constexpr char GNSS_SELECT[] = "gnss_mux_select";
 static constexpr char GNSS_MUX_SELECT[] = "gnss_mux_select";
+static constexpr char PL_I2C_ARESETN[] = "pl_i2c_aresetn";

 static constexpr char HEATER_0[] = "heater0";
 static constexpr char HEATER_1[] = "heater1";
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@ -4,6 +4,7 @@
 #include <fsfw/filesystem/HasFileSystemIF.h>
 #include <fsfw/ipc/QueueFactory.h>
 #include <fsfw/tasks/TaskFactory.h>
+#include <fsfw_hal/linux/uio/UioMapper.h>

 #include "commonConfig.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
@ -22,6 +23,7 @@
 #include <algorithm>
 #include <filesystem>

+#include "bsp_q7s/boardconfig/busConf.h"
 #include "bsp_q7s/fs/SdCardManager.h"
 #include "bsp_q7s/memory/scratchApi.h"
 #include "bsp_q7s/xadc/Xadc.h"
@ -112,6 +114,9 @@ void CoreController::performControlOperation() {
  sdStateMachine();
  performMountedSdCardOperations();
  readHkData();
+  if (dumpContext.active) {
+    dirListingDumpHandler();
+  }

  if (shellCmdIsExecuting) {
    bool replyReceived = false;
@ -182,6 +187,14 @@ ReturnValue_t CoreController::initialize() {
  if (result != returnvalue::OK) {
    sif::warning << "Subscribing for GPS GPS_FIX_CHANGE event failed" << std::endl;
  }
+
+  UioMapper sysRomMapper(q7s::UIO_SYS_ROM);
+  result = sysRomMapper.getMappedAdress(&mappedSysRomAddr, UioMapper::Permissions::READ_ONLY);
+  if (result != returnvalue::OK) {
+    // TODO: This might be a reason to switch to another image..
+    sif::error << "Getting mapped SYS ROM UIO address failed" << std::endl;
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
  return returnvalue::OK;
 }

@ -220,6 +233,11 @@ ReturnValue_t CoreController::executeAction(ActionId_t actionId, MessageQueueId_
      }

      triggerEvent(VERSION_INFO, p1, p2);
+      if (mappedSysRomAddr != nullptr) {
+        uint32_t p1Firmware = *(reinterpret_cast<uint32_t *>(mappedSysRomAddr));
+        uint32_t p2Firmware = *(reinterpret_cast<uint32_t *>(mappedSysRomAddr) + 1);
+        triggerEvent(FIRMWARE_INFO, p1Firmware, p2Firmware);
+      }
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case (ANNOUNCE_BOOT_COUNTS): {
@ -1041,7 +1059,6 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI
      return returnvalue::FAILED;
    }
  }
-  std::array<uint8_t, 1024> dirListingBuf{};
  dirListingBuf[8] = parser.compressionOptionSet();
  // First four bytes reserved for segment index. One byte for compression option information
  std::strcpy(reinterpret_cast<char *>(dirListingBuf.data() + 2 * sizeof(uint32_t) + 1), repoName);
@ -1050,38 +1067,47 @@ ReturnValue_t CoreController::actionListDirectoryDumpDirectly(ActionId_t actionI
    return returnvalue::FAILED;
  }
  std::error_code e;
-  size_t totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e);
-  uint32_t segmentIdx = 0;
-  size_t dumpedBytes = 0;
+  dumpContext.totalFileSize = std::filesystem::file_size(LIST_DIR_DUMP_WORK_FILE, e);
+  dumpContext.segmentIdx = 0;
+  dumpContext.dumpedBytes = 0;
  size_t nextDumpLen = 0;
  size_t dummy = 0;
-  size_t maxDumpLen = dirListingBuf.size() - 2 * sizeof(uint32_t) - 1 - repoNameLen - 1;
-  size_t listingDataOffset = 2 * sizeof(uint32_t) + 1 + repoNameLen + 1;
-  uint32_t chunks = totalFileSize / maxDumpLen;
-  if (totalFileSize % maxDumpLen != 0) {
+  dumpContext.maxDumpLen = dirListingBuf.size() - 2 * sizeof(uint32_t) - 1 - repoNameLen - 1;
+  dumpContext.listingDataOffset = 2 * sizeof(uint32_t) + 1 + repoNameLen + 1;
+  uint32_t chunks = dumpContext.totalFileSize / dumpContext.maxDumpLen;
+  if (dumpContext.totalFileSize % dumpContext.maxDumpLen != 0) {
    chunks++;
  }
  SerializeAdapter::serialize(&chunks, dirListingBuf.data() + sizeof(uint32_t), &dummy,
                              dirListingBuf.size() - sizeof(uint32_t),
                              SerializeIF::Endianness::NETWORK);
-  while (dumpedBytes < totalFileSize) {
-    ifile.seekg(dumpedBytes, std::ios::beg);
-    nextDumpLen = maxDumpLen;
-    if (totalFileSize - dumpedBytes < maxDumpLen) {
-      nextDumpLen = totalFileSize - dumpedBytes;
+  while (dumpContext.dumpedBytes < dumpContext.totalFileSize) {
+    ifile.seekg(dumpContext.dumpedBytes, std::ios::beg);
+    nextDumpLen = dumpContext.maxDumpLen;
+    if (dumpContext.totalFileSize - dumpContext.dumpedBytes < dumpContext.maxDumpLen) {
+      nextDumpLen = dumpContext.totalFileSize - dumpContext.dumpedBytes;
    }
-    SerializeAdapter::serialize(&segmentIdx, dirListingBuf.data(), &dummy, dirListingBuf.size(),
-                                SerializeIF::Endianness::NETWORK);
-    ifile.read(reinterpret_cast<char *>(dirListingBuf.data() + listingDataOffset), nextDumpLen);
+    SerializeAdapter::serialize(&dumpContext.segmentIdx, dirListingBuf.data(), &dummy,
+                                dirListingBuf.size(), SerializeIF::Endianness::NETWORK);
+    ifile.read(reinterpret_cast<char *>(dirListingBuf.data() + dumpContext.listingDataOffset),
+               nextDumpLen);
    result = actionHelper.reportData(commandedBy, actionId, dirListingBuf.data(),
-                                     listingDataOffset + nextDumpLen);
+                                     dumpContext.listingDataOffset + nextDumpLen);
    if (result != returnvalue::OK) {
      // Remove work file when we are done
      std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e);
      return result;
    }
-    segmentIdx++;
-    dumpedBytes += nextDumpLen;
+    dumpContext.segmentIdx++;
+    dumpContext.dumpedBytes += nextDumpLen;
+    // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles.
+    if (dumpContext.segmentIdx == 10) {
+      dumpContext.active = true;
+      dumpContext.firstDump = true;
+      dumpContext.commander = commandedBy;
+      dumpContext.actionId = actionId;
+      return returnvalue::OK;
+    }
  }
  // Remove work file when we are done
  std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e);
@ -2349,6 +2375,54 @@ MessageQueueId_t CoreController::getCommandQueue() const {
  return ExtendedControllerBase::getCommandQueue();
 }

+void CoreController::dirListingDumpHandler() {
+  if (dumpContext.firstDump) {
+    dumpContext.firstDump = false;
+    return;
+  }
+  size_t nextDumpLen = 0;
+  size_t dummy = 0;
+  ReturnValue_t result;
+  std::error_code e;
+  std::ifstream ifile(LIST_DIR_DUMP_WORK_FILE, std::ios::binary);
+  if (ifile.bad()) {
+    return;
+  }
+  while (dumpContext.dumpedBytes < dumpContext.totalFileSize) {
+    ifile.seekg(dumpContext.dumpedBytes, std::ios::beg);
+    nextDumpLen = dumpContext.maxDumpLen;
+    if (dumpContext.totalFileSize - dumpContext.dumpedBytes < dumpContext.maxDumpLen) {
+      nextDumpLen = dumpContext.totalFileSize - dumpContext.dumpedBytes;
+    }
+    SerializeAdapter::serialize(&dumpContext.segmentIdx, dirListingBuf.data(), &dummy,
+                                dirListingBuf.size(), SerializeIF::Endianness::NETWORK);
+    ifile.read(reinterpret_cast<char *>(dirListingBuf.data() + dumpContext.listingDataOffset),
+               nextDumpLen);
+    result =
+        actionHelper.reportData(dumpContext.commander, dumpContext.actionId, dirListingBuf.data(),
+                                dumpContext.listingDataOffset + nextDumpLen);
+    if (result != returnvalue::OK) {
+      // Remove work file when we are done
+      std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e);
+      dumpContext.active = false;
+      actionHelper.finish(false, dumpContext.commander, dumpContext.actionId, result);
+      return;
+    }
+    dumpContext.segmentIdx++;
+    dumpContext.dumpedBytes += nextDumpLen;
+    // Dump takes multiple task cycles, so cache the dump state and continue dump the next cycles.
+    if (dumpContext.segmentIdx == 10) {
+      break;
+    }
+  }
+  if (dumpContext.dumpedBytes >= dumpContext.totalFileSize) {
+    actionHelper.finish(true, dumpContext.commander, dumpContext.actionId, result);
+    dumpContext.active = false;
+    // Remove work file when we are done
+    std::filesystem::remove(LIST_DIR_DUMP_WORK_FILE, e);
+  }
+}
+
 bool CoreController::isNumber(const std::string &s) {
  return !s.empty() && std::find_if(s.begin(), s.end(),
                                    [](unsigned char c) { return !std::isdigit(c); }) == s.end();
--- a/bsp_q7s/core/CoreController.h
+++ b/bsp_q7s/core/CoreController.h
@ -6,6 +6,7 @@
 #include <fsfw/globalfunctions/PeriodicOperationDivider.h>
 #include <fsfw/parameters/ParameterHelper.h>
 #include <fsfw/parameters/ReceivesParameterMessagesIF.h>
+#include <fsfw_hal/linux/uio/UioMapper.h>
 #include <libxiphos.h>
 #include <mission/acs/archive/GPSDefinitions.h>
 #include <mission/utility/trace.h>
@ -142,6 +143,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe

  static constexpr bool BLOCKING_SD_INIT = false;

+  uint32_t* mappedSysRomAddr = nullptr;
  SdCardManager* sdcMan = nullptr;
  MessageQueueIF* eventQueue = nullptr;

@ -177,6 +179,20 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
    DeviceCommandId_t actionId;
  } sdCommandingInfo;

+  struct DirListingDumpContext {
+    bool active;
+    bool firstDump;
+    size_t dumpedBytes;
+    size_t totalFileSize;
+    size_t listingDataOffset;
+    size_t maxDumpLen;
+    uint32_t segmentIdx;
+    MessageQueueId_t commander = MessageQueueIF::NO_QUEUE;
+    DeviceCommandId_t actionId;
+  };
+  std::array<uint8_t, 1024> dirListingBuf{};
+  DirListingDumpContext dumpContext{};
+
  RebootFile rebootFile = {};

  CommandExecutor cmdExecutor;
@ -274,6 +290,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
  void rewriteRebootFile(RebootFile file);
  void announceBootCounts();
  void readHkData();
+  void dirListingDumpHandler();
  bool isNumber(const std::string& s);
 };

--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@ -2,6 +2,7 @@

 #include <fsfw/devicehandlers/HealthDevice.h>
 #include <fsfw/subsystem/Subsystem.h>
+#include <fsfw/tasks/TaskFactory.h>
 #include <linux/acs/AcsBoardPolling.h>
 #include <linux/acs/GpsHyperionLinuxController.h>
 #include <linux/acs/ImtqPollingTask.h>
@ -1013,3 +1014,19 @@ void ObjectFactory::createRadSensorChipSelect(LinuxLibgpioIF* gpioIF) {
  gpioCookieRadSensor->addGpio(gpioIds::ENABLE_RADFET, gpio);
  gpioChecker(gpioIF->addGpios(gpioCookieRadSensor), "RAD sensor");
 }
+
+void ObjectFactory::createPlI2cResetGpio(LinuxLibgpioIF* gpioIF) {
+  using namespace gpio;
+  if (gpioIF == nullptr) {
+    return;
+  }
+  GpioCookie* gpioI2cResetnCookie = new GpioCookie;
+  GpiodRegularByLineName* gpioI2cResetn = new GpiodRegularByLineName(
+      q7s::gpioNames::PL_I2C_ARESETN, "PL_I2C_ARESETN", Direction::OUT, Levels::HIGH);
+  gpioI2cResetnCookie->addGpio(gpioIds::PL_I2C_ARESETN, gpioI2cResetn);
+  gpioChecker(gpioIF->addGpios(gpioI2cResetnCookie), "PL I2C ARESETN");
+  // Reset I2C explicitely again.
+  gpioIF->pullLow(gpioIds::PL_I2C_ARESETN);
+  TaskFactory::delayTask(1);
+  gpioIF->pullHigh(gpioIds::PL_I2C_ARESETN);
+}
--- a/bsp_q7s/core/ObjectFactory.h
+++ b/bsp_q7s/core/ObjectFactory.h
@ -78,6 +78,7 @@ ReturnValue_t createCcsdsComponents(CcsdsComponentArgs& args);
 void createMiscComponents();

 void createTestComponents(LinuxLibgpioIF* gpioComIF);
+void createPlI2cResetGpio(LinuxLibgpioIF* gpioComIF);

 void testAcsBrdAss(AcsBoardAssembly* assAss);

--- a/bsp_q7s/em/emObjectFactory.cpp
+++ b/bsp_q7s/em/emObjectFactory.cpp
@ -47,6 +47,7 @@ void ObjectFactory::produce(void* args) {
  /* Adding gpios for chip select decoding to the gpioComIf */
  q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF);
  gpioCallbacks::disableAllDecoder(gpioComIF);
+  createPlI2cResetGpio(gpioComIF);

  // Hardware is usually not connected to EM, so we need to create dummies which replace lower
  // level components.
@ -140,6 +141,7 @@ void ObjectFactory::produce(void* args) {
 #if OBSW_TM_TO_PTME == 1
  if (ccsdsArgs.liveDestination != nullptr) {
    pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0);
+    cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0);
  }
 #endif
 #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */
--- a/bsp_q7s/fmObjectFactory.cpp
+++ b/bsp_q7s/fmObjectFactory.cpp
@ -45,6 +45,7 @@ void ObjectFactory::produce(void* args) {
  /* Adding gpios for chip select decoding to the gpioComIf */
  q7s::gpioCallbacks::initSpiCsDecoder(gpioComIF);
  gpioCallbacks::disableAllDecoder(gpioComIF);
+  createPlI2cResetGpio(gpioComIF);

  new CoreController(objects::CORE_CONTROLLER, enableHkSets);
  createPcduComponents(gpioComIF, &pwrSwitcher, enableHkSets);
@ -97,6 +98,7 @@ void ObjectFactory::produce(void* args) {
 #if OBSW_TM_TO_PTME == 1
  if (ccsdsArgs.liveDestination != nullptr) {
    pusFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0);
+    cfdpFunnel->addLiveDestination("VC0 LIVE TM", *ccsdsArgs.liveDestination, 0);
  }
 #endif
 #endif /* OBSW_ADD_CCSDS_IP_CORES == 1 */
--- a/common/config/devices/gpioIds.h
+++ b/common/config/devices/gpioIds.h
@ -77,6 +77,8 @@ enum gpioId_t {
  CS_RAD_SENSOR,
  ENABLE_RADFET,

+  PL_I2C_ARESETN,
+
  PAPB_BUSY_N,
  PAPB_EMPTY,

--- a/common/config/eive/definitions.h
+++ b/common/config/eive/definitions.h
@ -58,7 +58,9 @@ static constexpr uint32_t CFDP_STORE_QUEUE_SIZE = 300;

 static constexpr uint32_t MAX_PUS_FUNNEL_QUEUE_DEPTH = 100;
 static constexpr uint32_t MAX_CFDP_FUNNEL_QUEUE_DEPTH = 80;
+static constexpr uint32_t VERIFICATION_SERVICE_QUEUE_DEPTH = 120;
 static constexpr uint32_t HK_SERVICE_QUEUE_DEPTH = 60;
+static constexpr uint32_t ACTION_SERVICE_QUEUE_DEPTH = 60;

 static constexpr uint32_t MAX_STORED_CMDS_UDP = 150;
 static constexpr uint32_t MAX_STORED_CMDS_TCP = 180;
--- a/dummies/helperFactory.h
+++ b/dummies/helperFactory.h
@ -19,7 +19,7 @@ struct DummyCfg {
  bool addTempSensorDummies = true;
  bool addRtdComIFDummy = true;
  bool addPlocDummies = true;
-  bool addCamSwitcherDummy = true;
+  bool addCamSwitcherDummy = false;
 };

 void createDummies(DummyCfg cfg, PowerSwitchIF& pwrSwitch, GpioIF* gpioIF, bool enableHkSets);
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 4391823f01d792bcc078d47b60f7df7624f8cbe4
+Subproject commit 0f76cdb3ba54f5e90a8eee4316c49cf0f581f996
--- a/generators/bsp_hosted_events.csv
+++ b/generators/bsp_hosted_events.csv
@ -271,6 +271,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h
 14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h
 14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h
+14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). 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.;mission/sysDefs.h
 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h
 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h
 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h
--- a/generators/bsp_q7s_events.csv
+++ b/generators/bsp_q7s_events.csv
@ -271,6 +271,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h
 14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h
 14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h
+14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). 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.;mission/sysDefs.h
 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h
 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h
 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 295 translations.
+ * @brief    Auto-generated event translation file. Contains 296 translations.
 * @details
- * Generated on: 2023-05-17 17:15:34
+ * Generated on: 2023-06-21 19:01:02
 */
 #include "translateEvents.h"

@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS";
 const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY";
 const char *I2C_REBOOT_STRING = "I2C_REBOOT";
 const char *PDEC_REBOOT_STRING = "PDEC_REBOOT";
+const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO";
 const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE";
 const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE";
 const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING";
@ -847,6 +848,8 @@ const char *translateEvents(Event event) {
      return I2C_REBOOT_STRING;
    case (14012):
      return PDEC_REBOOT_STRING;
+    case (14013):
+      return FIRMWARE_INFO_STRING;
    case (14100):
      return NO_VALID_SENSOR_TEMPERATURE_STRING;
    case (14101):
--- a/generators/objects/translateObjects.cpp
+++ b/generators/objects/translateObjects.cpp
@ -2,7 +2,7 @@
 * @brief  Auto-generated object translation file.
 * @details
 * Contains 175 translations.
- * Generated on: 2023-05-17 17:15:34
+ * Generated on: 2023-06-21 19:01:02
 */
 #include "translateObjects.h"

--- a/linux/acs/AcsBoardPolling.cpp
+++ b/linux/acs/AcsBoardPolling.cpp
@ -113,6 +113,7 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
    if (req->mode != adis.mode) {
      if (req->mode == acs::SimpleSensorMode::NORMAL) {
        adis.type = req->type;
+        adis.decRate = req->cfg.decRateReg;
        // The initial countdown is handled by the device handler now.
        // adis.countdown.setTimeout(adis1650x::START_UP_TIME);
        if (adis.type == adis1650x::Type::ADIS16507) {
@ -376,6 +377,80 @@ void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
  }
 }

+ReturnValue_t AcsBoardPolling::writeAdisReg(SpiCookie& cookie) {
+  ReturnValue_t result = returnvalue::OK;
+  int retval = 0;
+  // Prepare transfer
+  int fileDescriptor = 0;
+  std::string device = spiComIF.getSpiDev();
+  UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
+  if (fileHelper.getOpenResult() != returnvalue::OK) {
+    return spi::OPENING_FILE_FAILED;
+  }
+  spi::SpiModes spiMode = spi::SpiModes::MODE_0;
+  uint32_t spiSpeed = 0;
+  cookie.getSpiParameters(spiMode, spiSpeed, nullptr);
+  spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
+  cookie.assignWriteBuffer(cmdBuf.data());
+  cookie.setTransferSize(2);
+
+  gpioId_t gpioId = cookie.getChipSelectPin();
+  MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+  uint32_t timeoutMs = 0;
+  MutexIF* mutex = spiComIF.getCsMutex();
+  cookie.getMutexParams(timeoutType, timeoutMs);
+  if (mutex == nullptr) {
+    sif::warning << "GyroADIS16507Handler::spiSendCallback: "
+                    "Mutex or GPIO interface invalid"
+                 << std::endl;
+    return returnvalue::FAILED;
+  }
+
+  size_t idx = 0;
+  spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle();
+  uint64_t origTx = transferStruct->tx_buf;
+  uint64_t origRx = transferStruct->rx_buf;
+  for (idx = 0; idx < 4; idx += 2) {
+    result = mutex->lockMutex(timeoutType, timeoutMs);
+    if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl;
+#endif
+      return result;
+    }
+    // Pull SPI CS low. For now, no support for active high given
+    if (gpioId != gpio::NO_GPIO) {
+      gpioIF.pullLow(gpioId);
+    }
+
+    // Execute transfer
+    // Initiate a full duplex SPI transfer.
+    retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle());
+    if (retval < 0) {
+      utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
+      result = spi::FULL_DUPLEX_TRANSFER_FAILED;
+    }
+#if FSFW_HAL_SPI_WIRETAPPING == 1
+    comIf->performSpiWiretapping(cookie);
+#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
+
+    if (gpioId != gpio::NO_GPIO) {
+      gpioIF.pullHigh(gpioId);
+    }
+    mutex->unlockMutex();
+
+    transferStruct->tx_buf += 2;
+    transferStruct->rx_buf += 2;
+    if (idx < 4) {
+      usleep(adis1650x::STALL_TIME_MICROSECONDS);
+    }
+  }
+  transferStruct->tx_buf = origTx;
+  transferStruct->rx_buf = origRx;
+  cookie.setTransferSize(0);
+  return returnvalue::OK;
+}
+
 ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) {
  ReturnValue_t result = returnvalue::OK;
  int retval = 0;
@ -455,15 +530,20 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
  ReturnValue_t result;
  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
  bool mustPerformStartup = false;
+  uint16_t decRate = 0;
  {
    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    mode = gyro.mode;
+    decRate = gyro.decRate;
    mustPerformStartup = gyro.performStartup;
  }
  if (mode == acs::SimpleSensorMode::OFF) {
    return;
  }
  if (mustPerformStartup) {
+    adis1650x::prepareWriteRegCommand(adis1650x::DEC_RATE_REG, decRate, cmdBuf.data(),
+                                      cmdBuf.size());
+    writeAdisReg(*gyro.cookie);
    uint8_t regList[6];
    // Read configuration
    regList[0] = adis1650x::DIAG_STAT_REG;
@ -491,13 +571,19 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
      gyro.replyResult = returnvalue::FAILED;
      return;
    }
+    uint16_t decRateReadBack = (rawReply[10] << 8) | rawReply[11];
+    if (decRateReadBack != decRate) {
+      sif::warning << "AcsPollingTask: DEC rate configuration failed. Read " << decRateReadBack
+                   << ", expected " << decRate << std::endl;
+      gyro.replyResult = returnvalue::FAILED;
+    }
    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    gyro.ownReply.cfgWasSet = true;
    gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3];
    gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5];
    gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7];
    gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9];
-    gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11];
+    gyro.ownReply.cfg.decRateReg = decRateReadBack;
    gyro.ownReply.cfg.prodId = prodId;
    gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
    gyro.performStartup = false;
--- a/linux/acs/AcsBoardPolling.h
+++ b/linux/acs/AcsBoardPolling.h
@ -37,6 +37,7 @@ class AcsBoardPolling : public SystemObject,

  struct GyroAdis : public DevBase {
    adis1650x::Type type;
+    uint16_t decRate;
    Countdown countdown;
    acs::Adis1650XReply ownReply;
    acs::Adis1650XReply readerReply;
@ -84,6 +85,8 @@ class AcsBoardPolling : public SystemObject,
  void gyroAdisHandler(GyroAdis& gyro);
  void mgmLis3Handler(MgmLis3& mgm);
  void mgmRm3100Handler(MgmRm3100& mgm);
+  // This fumction configures the register as specified on p.21 of the datasheet.
+  ReturnValue_t writeAdisReg(SpiCookie& cookie);
  // Special readout: 16us stall time between small 2 byte transfers.
  ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen);
 };
--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 295 translations.
+ * @brief    Auto-generated event translation file. Contains 296 translations.
 * @details
- * Generated on: 2023-05-17 17:15:34
+ * Generated on: 2023-06-21 19:01:02
 */
 #include "translateEvents.h"

@ -277,6 +277,7 @@ const char *INDIVIDUAL_BOOT_COUNTS_STRING = "INDIVIDUAL_BOOT_COUNTS";
 const char *TRYING_I2C_RECOVERY_STRING = "TRYING_I2C_RECOVERY";
 const char *I2C_REBOOT_STRING = "I2C_REBOOT";
 const char *PDEC_REBOOT_STRING = "PDEC_REBOOT";
+const char *FIRMWARE_INFO_STRING = "FIRMWARE_INFO";
 const char *NO_VALID_SENSOR_TEMPERATURE_STRING = "NO_VALID_SENSOR_TEMPERATURE";
 const char *NO_HEALTHY_HEATER_AVAILABLE_STRING = "NO_HEALTHY_HEATER_AVAILABLE";
 const char *SYRLINKS_OVERHEATING_STRING = "SYRLINKS_OVERHEATING";
@ -847,6 +848,8 @@ const char *translateEvents(Event event) {
      return I2C_REBOOT_STRING;
    case (14012):
      return PDEC_REBOOT_STRING;
+    case (14013):
+      return FIRMWARE_INFO_STRING;
    case (14100):
      return NO_VALID_SENSOR_TEMPERATURE_STRING;
    case (14101):
--- a/linux/fsfwconfig/objects/translateObjects.cpp
+++ b/linux/fsfwconfig/objects/translateObjects.cpp
@ -2,7 +2,7 @@
 * @brief  Auto-generated object translation file.
 * @details
 * Contains 175 translations.
- * Generated on: 2023-05-17 17:15:34
+ * Generated on: 2023-06-21 19:01:02
 */
 #include "translateObjects.h"

--- a/mission/acs/GyrAdis1650XHandler.cpp
+++ b/mission/acs/GyrAdis1650XHandler.cpp
@ -15,7 +15,7 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic
 }

 void GyrAdis1650XHandler::doStartUp() {
-  if (internalState != InternalState::STARTUP) {
+  if (internalState == InternalState::NONE) {
    internalState = InternalState::STARTUP;
    commandExecuted = false;
  }
@ -24,13 +24,14 @@ void GyrAdis1650XHandler::doStartUp() {
    if (not commandExecuted) {
      warningSwitch = true;
      breakCountdown.setTimeout(adis1650x::START_UP_TIME);
+      updatePeriodicReply(true, adis1650x::REPLY);
      commandExecuted = true;
    }
-    if (breakCountdown.hasTimedOut()) {
-      updatePeriodicReply(true, adis1650x::REPLY);
-      setMode(MODE_ON);
-      internalState = InternalState::NONE;
-    }
+  }
+  if (internalState == InternalState::STARTUP_DONE) {
+    setMode(MODE_ON);
+    commandExecuted = false;
+    internalState = InternalState::NONE;
  }
 }

@ -61,6 +62,7 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_
        return NOTHING_TO_SEND;
      }
      *id = adis1650x::REQUEST;
+      adisRequest.cfg.decRateReg = adis1650x::DEC_RATE;
      return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
    }
    case (InternalState::SHUTDOWN): {
@ -91,6 +93,9 @@ ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t rem
      getMode() == _MODE_POWER_DOWN) {
    return IGNORE_FULL_PACKET;
  }
+  if (internalState == InternalState::STARTUP) {
+    internalState = InternalState::STARTUP_DONE;
+  }
  *foundLen = remainingSize;
  if (remainingSize != sizeof(acs::Adis1650XReply)) {
    return returnvalue::FAILED;
--- a/mission/acs/GyrAdis1650XHandler.h
+++ b/mission/acs/GyrAdis1650XHandler.h
@ -48,7 +48,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase {

  bool warningSwitch = true;

-  enum class InternalState { NONE, STARTUP, SHUTDOWN };
+  enum class InternalState { NONE, STARTUP, STARTUP_DONE, SHUTDOWN };

  InternalState internalState = InternalState::STARTUP;
  bool commandExecuted = false;
--- a/mission/acs/acsBoardPolling.h
+++ b/mission/acs/acsBoardPolling.h
@ -8,11 +8,6 @@

 namespace acs {

-struct Adis1650XRequest {
-  SimpleSensorMode mode;
-  adis1650x::Type type;
-};
-
 struct Adis1650XConfig {
  uint16_t diagStat;
  uint16_t filterSetting;
@ -22,6 +17,12 @@ struct Adis1650XConfig {
  uint16_t prodId;
 };

+struct Adis1650XRequest {
+  SimpleSensorMode mode;
+  adis1650x::Type type;
+  Adis1650XConfig cfg;
+};
+
 struct Adis1650XData {
  double sensitivity = 0.0;
  // Angular velocities in all axes (X, Y and Z)
--- a/mission/acs/gyroAdisHelpers.cpp
+++ b/mission/acs/gyroAdisHelpers.cpp
@ -52,3 +52,14 @@ double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) {
    }
  }
 }
+
+void adis1650x::prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf,
+                                       size_t maxLen) {
+  if (maxLen < 4) {
+    return;
+  }
+  outBuf[0] = regStart | adis1650x::WRITE_MASK;
+  outBuf[1] = val & 0xff;
+  outBuf[2] = (regStart + 1) | adis1650x::WRITE_MASK;
+  outBuf[3] = (val >> 8) & 0xff;
+}
--- a/mission/acs/gyroAdisHelpers.h
+++ b/mission/acs/gyroAdisHelpers.h
@ -16,6 +16,8 @@ enum class BurstModes {
 };

 size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen);
+void prepareWriteRegCommand(uint8_t regStart, uint16_t val, uint8_t* outBuf, size_t maxLen);
+
 BurstModes burstModeFromMscCtrl(uint16_t mscCtrl);
 double rangMdlToSensitivity(uint16_t rangMdl);

@ -92,6 +94,9 @@ static constexpr size_t SENSOR_READOUT_SIZE = 20 + 2;
 static constexpr uint32_t ADIS_DATASET_ID = READ_SENSOR_DATA;
 static constexpr uint32_t ADIS_CFG_DATASET_ID = READ_OUT_CONFIG;

+static constexpr uint16_t FILT_CTRL = 0x0000;
+static constexpr uint16_t DEC_RATE = 0x00C7;
+
 enum GlobCmds : uint8_t {
  FACTORY_CALIBRATION = 0b0000'0010,
  SENSOR_SELF_TEST = 0b0000'0100,
--- a/mission/acs/str/StarTrackerHandler.cpp
+++ b/mission/acs/str/StarTrackerHandler.cpp
@ -100,6 +100,19 @@ void StarTrackerHandler::doShutDown() {
  startupState = StartupState::IDLE;
  bootState = FwBootState::NONE;
  solutionSet.setReportingEnabled(false);
+  {
+    PoolReadGuard pg(&solutionSet);
+    solutionSet.caliQw.value = 0.0;
+    solutionSet.caliQx.value = 0.0;
+    solutionSet.caliQy.value = 0.0;
+    solutionSet.caliQz.value = 0.0;
+    solutionSet.isTrustWorthy.value = 0;
+    solutionSet.setValidity(false, true);
+  }
+  {
+    PoolReadGuard pg(&temperatureSet);
+    temperatureSet.setValidity(false, true);
+  }
  reinitNextSetParam = false;
  setMode(_MODE_POWER_DOWN);
 }
--- a/mission/cfdp/CfdpHandler.cpp
+++ b/mission/cfdp/CfdpHandler.cpp
@ -93,6 +93,8 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) {
      return INVALID_PDU_FORMAT;
    }
    if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) {
+      sif::error << "CfdpHandler: Invalid PDU directive field " << static_cast<int>(pduDataField[0])
+                 << std::endl;
      return INVALID_DIRECTIVE_FIELD;
    }
    auto directive = static_cast<FileDirective>(pduDataField[0]);
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@ -169,7 +169,7 @@ void AcsController::performSafe() {
  guidance.getTargetParamsSafe(sunTargetDir);

  double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
-  uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
+  acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
      mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
      gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
      acsParameters.safeModeControllerParameters.useMekf,
@ -205,11 +205,13 @@ void AcsController::performSafe() {
    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
      safeCtrlFailure(0, 1);
      break;
+    default:
+      sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
+      break;
  }

-  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
-                          *acsParameters.magnetorquerParameter.inverseAlignment,
-                          acsParameters.magnetorquerParameter.dipolMax);
+  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
+                           acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);

  // detumble check and switch
  if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
@ -231,8 +233,8 @@ void AcsController::performSafe() {
  }

  updateCtrlValData(errAng, safeCtrlStrat);
-  updateActuatorCmdData(cmdDipolMtqs);
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  updateActuatorCmdData(cmdDipoleMtqs);
+  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                   acsParameters.magnetorquerParameter.torqueDuration);
 }

@ -259,7 +261,7 @@ void AcsController::performDetumble() {
    triggerEvent(acs::MEKF_RECOVERY);
    mekfInvalidFlag = false;
  }
-  uint8_t safeCtrlStrat = detumble.detumbleStrategy(
+  acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
      mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
      mgmDataProcessed.mgmVecTotDerivative.isValid(),
      acsParameters.detumbleParameter.useFullDetumbleLaw);
@ -279,11 +281,13 @@ void AcsController::performDetumble() {
    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
      safeCtrlFailure(0, 1);
      break;
+    default:
+      sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
+      break;
  }

-  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
-                          *acsParameters.magnetorquerParameter.inverseAlignment,
-                          acsParameters.magnetorquerParameter.dipolMax);
+  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
+                           acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);

  if (mekfData.satRotRateMekf.isValid() &&
      VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@ -304,8 +308,8 @@ void AcsController::performDetumble() {
  }

  disableCtrlValData();
-  updateActuatorCmdData(cmdDipolMtqs);
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  updateActuatorCmdData(cmdDipoleMtqs);
+  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                   acsParameters.magnetorquerParameter.torqueDuration);
 }

@ -366,24 +370,26 @@ void AcsController::performPointingCtrl() {
  // Variables required for setting actuators
  double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
         mgtDpDes[3] = {0, 0, 0};
+
  switch (mode) {
    case acs::PTG_IDLE:
-      guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
+      guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
+                                targetSatRotRate);
      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
-          &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
+                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
      break;

@ -397,17 +403,17 @@ void AcsController::performPointingCtrl() {
                          errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
-          &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
+                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &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);
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
      break;

@ -418,17 +424,17 @@ void AcsController::performPointingCtrl() {
                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
-          &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
+                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
-          &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
      break;

@ -442,63 +448,61 @@ void AcsController::performPointingCtrl() {
                          errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
-          &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
+                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &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);
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
      break;

    case acs::PTG_INERTIAL:
      std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
-                  4 * sizeof(double));
+                  sizeof(targetQuat));
      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                          targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
                          acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
                          errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
-      ptgCtrl.ptgNullspace(
-          &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
-          &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
-          &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
+      ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
+                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
+                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &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);
+          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
      break;
    default:
-      sif::error << "AcsController: Invalid mode for performPointingCtrl";
+      sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
      break;
  }

  actuatorCmd.cmdSpeedToRws(
      sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws,
-      cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
-      acsParameters.rwHandlingParameters.maxRwSpeed,
-      acsParameters.rwHandlingParameters.inertiaWheel);
+      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
+      acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
+      acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
  if (enableAntiStiction) {
    ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
  }
-  actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
-                          *acsParameters.magnetorquerParameter.inverseAlignment,
-                          acsParameters.magnetorquerParameter.dipolMax);
+  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
+                           acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);

  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
-  updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
+  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                   acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
                   acsParameters.rwHandlingParameters.rampTime);
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
  bool mekfLost = false;

  int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
-  int16_t cmdDipolMtqs[3] = {0, 0, 0};
+  int16_t cmdDipoleMtqs[3] = {0, 0, 0};

 #if OBSW_THREAD_TRACING == 1
  uint32_t opCounter = 0;
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@ -1002,7 +1002,7 @@ void ThermalController::copyDevices() {

 void ThermalController::ctrlAcsBoard() {
  heater::Switch switchNr = heater::HEATER_2_ACS_BRD;
-  heater::Switch redSwitchNr = heater::HEATER_0_OBC_BRD;
+  heater::Switch redSwitchNr = heater::HEATER_3_OBC_BRD;

  // A side
  thermalComponent = ACS_BOARD;
@ -1067,7 +1067,7 @@ void ThermalController::ctrlMgt() {
  sensors[2].first = sensorTemperatures.plpcduHeatspreader.isValid();
  sensors[2].second = sensorTemperatures.plpcduHeatspreader.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, mgtLimits);
+  HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, mgtLimits);
  ctrlComponentTemperature(htrCtx);
  if (componentAboveUpperLimit and not mgtTooHotFlag) {
    triggerEvent(tcsCtrl::MGT_OVERHEATING, tempFloatToU32());
@ -1206,7 +1206,7 @@ void ThermalController::ctrlIfBoard() {
  sensors[2].first = deviceTemperatures.mgm2SideB.isValid();
  sensors[2].second = deviceTemperatures.mgm2SideB.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_3_PCDU_PDU, ifBoardLimits);
+  HeaterContext htrCtx(heater::HEATER_2_ACS_BRD, heater::HEATER_1_PCDU_PDU, ifBoardLimits);
  ctrlComponentTemperature(htrCtx);
  // TODO: special event overheating + could go back to safe mode
 }
@ -1220,7 +1220,7 @@ void ThermalController::ctrlTcsBoard() {
  sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid();
  sensors[2].second = sensorTemperatures.tmp1075Tcs1.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits);
+  HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, tcsBoardLimits);
  ctrlComponentTemperature(htrCtx);
  // TODO: special event overheating + could go back to safe mode
 }
@ -1234,7 +1234,7 @@ void ThermalController::ctrlObc() {
  sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid();
  sensors[2].second = sensorTemperatures.tmp1075Tcs0.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits);
+  HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcLimits);
  ctrlComponentTemperature(htrCtx);
  if (componentAboveUpperLimit and not obcTooHotFlag) {
    triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32());
@ -1253,7 +1253,7 @@ void ThermalController::ctrlObcIfBoard() {
  sensors[2].first = sensorTemperatures.tmp1075Tcs1.isValid();
  sensors[2].second = sensorTemperatures.tmp1075Tcs1.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_0_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits);
+  HeaterContext htrCtx(heater::HEATER_3_OBC_BRD, heater::HEATER_2_ACS_BRD, obcIfBoardLimits);
  ctrlComponentTemperature(htrCtx);
  if (componentAboveUpperLimit and not obcTooHotFlag) {
    triggerEvent(tcsCtrl::OBC_OVERHEATING, tempFloatToU32());
@ -1288,7 +1288,7 @@ void ThermalController::ctrlPcduP60Board() {
  sensors[1].first = deviceTemperatures.temp2P60dock.isValid();
  sensors[1].second = deviceTemperatures.temp2P60dock.value;
  numSensors = 2;
-  HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits);
+  HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduP60BoardLimits);
  ctrlComponentTemperature(htrCtx);
  if (componentAboveUpperLimit and not pcduSystemTooHotFlag) {
    triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32());
@ -1300,7 +1300,7 @@ void ThermalController::ctrlPcduP60Board() {

 void ThermalController::ctrlPcduAcu() {
  thermalComponent = PCDUACU;
-  heater::Switch switchNr = heater::HEATER_3_PCDU_PDU;
+  heater::Switch switchNr = heater::HEATER_1_PCDU_PDU;
  heater::Switch redSwitchNr = heater::HEATER_2_ACS_BRD;

  if (chooseHeater(switchNr, redSwitchNr)) {
@ -1340,7 +1340,7 @@ void ThermalController::ctrlPcduPdu() {
  sensors[2].first = sensorTemperatures.tmp1075Tcs0.isValid();
  sensors[2].second = sensorTemperatures.tmp1075Tcs0.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits);
+  HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, pcduPduLimits);
  ctrlComponentTemperature(htrCtx);
  if (componentAboveUpperLimit and not pcduSystemTooHotFlag) {
    triggerEvent(tcsCtrl::PCDU_SYSTEM_OVERHEATING, tempFloatToU32());
@ -1361,7 +1361,7 @@ void ThermalController::ctrlPlPcduBoard() {
  sensors[3].first = sensorTemperatures.plpcduHeatspreader.isValid();
  sensors[3].second = sensorTemperatures.plpcduHeatspreader.value;
  numSensors = 4;
-  HeaterContext htrCtx(heater::HEATER_3_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits);
+  HeaterContext htrCtx(heater::HEATER_1_PCDU_PDU, heater::HEATER_2_ACS_BRD, plPcduBoardLimits);
  ctrlComponentTemperature(htrCtx);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
 }
@ -1375,7 +1375,7 @@ void ThermalController::ctrlPlocMissionBoard() {
  sensors[2].first = sensorTemperatures.dacHeatspreader.isValid();
  sensors[2].second = sensorTemperatures.dacHeatspreader.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
+  HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD,
                       plocMissionBoardLimits);
  ctrlComponentTemperature(htrCtx);
  tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag);
@ -1390,7 +1390,7 @@ void ThermalController::ctrlPlocProcessingBoard() {
  sensors[2].first = sensorTemperatures.dacHeatspreader.isValid();
  sensors[2].second = sensorTemperatures.dacHeatspreader.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD,
+  HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD,
                       plocProcessingBoardLimits);
  ctrlComponentTemperature(htrCtx);
  tooHotHandler(objects::PLOC_SUPERVISOR_HANDLER, plocTooHotFlag);
@ -1405,7 +1405,7 @@ void ThermalController::ctrlDac() {
  sensors[2].first = sensorTemperatures.plocHeatspreader.isValid();
  sensors[2].second = sensorTemperatures.plocHeatspreader.value;
  numSensors = 3;
-  HeaterContext htrCtx(heater::HEATER_1_PLOC_PROC_BRD, heater::HEATER_0_OBC_BRD, dacLimits);
+  HeaterContext htrCtx(heater::HEATER_0_PLOC_PROC_BRD, heater::HEATER_3_OBC_BRD, dacLimits);
  ctrlComponentTemperature(htrCtx);
  tooHotHandler(objects::PLPCDU_HANDLER, eBandTooHotFlag);
 }
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@ -221,6 +221,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
        case 0x23:
          parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta);
          break;
+        case 0x24:
+          parameterWrapper->set(susHandlingParameters.susBrightnessThreshold);
+          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
@ -318,7 +321,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->setMatrix(rwMatrices.without4);
          break;
        case 0x6:
-          parameterWrapper->setVector(rwMatrices.nullspace);
+          parameterWrapper->setVector(rwMatrices.nullspaceVector);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
@ -378,15 +381,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(idleModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed);
          break;
        case 0x6:
-          parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
          break;
        case 0x7:
-          parameterWrapper->set(idleModeControllerParameters.desatOn);
+          parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
          break;
        case 0x8:
+          parameterWrapper->set(idleModeControllerParameters.desatOn);
+          break;
+        case 0x9:
          parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
          break;
        default:
@ -411,48 +417,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(targetModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed);
          break;
        case 0x6:
-          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
          break;
        case 0x7:
-          parameterWrapper->set(targetModeControllerParameters.desatOn);
+          parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
          break;
        case 0x8:
-          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(targetModeControllerParameters.desatOn);
          break;
        case 0x9:
-          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
+          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
          break;
        case 0xA:
-          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
          break;
        case 0xB:
-          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
+          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
          break;
        case 0xC:
-          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
          break;
        case 0xD:
-          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
          break;
        case 0xE:
-          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
          break;
        case 0xF:
-          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
          break;
        case 0x10:
-          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
+          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
          break;
        case 0x11:
-          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
+          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
          break;
        case 0x12:
-          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
          break;
        case 0x13:
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          break;
+        case 0x14:
          parameterWrapper->set(targetModeControllerParameters.blindRotRate);
          break;
        default:
@ -477,30 +486,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed);
          break;
        case 0x6:
-          parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
          break;
        case 0x7:
-          parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
+          parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
          break;
        case 0x8:
-          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
          break;
        case 0x9:
-          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
+          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
          break;
        case 0xA:
-          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
          break;
        case 0xB:
-          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
          break;
        case 0xC:
-          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
          break;
        case 0xD:
+          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
+          break;
+        case 0xE:
          parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
          break;
        default:
@ -525,27 +537,30 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed);
          break;
        case 0x6:
-          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
          break;
        case 0x7:
-          parameterWrapper->set(nadirModeControllerParameters.desatOn);
+          parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
          break;
        case 0x8:
-          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(nadirModeControllerParameters.desatOn);
          break;
        case 0x9:
-          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
+          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
          break;
        case 0xA:
-          parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
+          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
          break;
        case 0xB:
-          parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
          break;
        case 0xC:
+          parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
+          break;
+        case 0xD:
          parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
          break;
        default:
@ -570,21 +585,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
          break;
        case 0x5:
-          parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
+          parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed);
          break;
        case 0x6:
-          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
+          parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
          break;
        case 0x7:
-          parameterWrapper->set(inertialModeControllerParameters.desatOn);
+          parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
          break;
        case 0x8:
-          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(inertialModeControllerParameters.desatOn);
          break;
        case 0x9:
-          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
+          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
          break;
        case 0xA:
+          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
+          break;
+        case 0xB:
          parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
          break;
        case 0xC:
@ -696,7 +714,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
          parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
          break;
        case 0x5:
-          parameterWrapper->set(magnetorquerParameter.dipolMax);
+          parameterWrapper->set(magnetorquerParameter.dipoleMax);
          break;
        case 0x6:
          parameterWrapper->set(magnetorquerParameter.torqueDuration);
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@ -766,6 +766,7 @@ 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 susBrightnessThreshold = 0.7;
  } susHandlingParameters;

  struct GyrHandlingParameters {
@ -781,9 +782,9 @@ class AcsParameters : public HasParametersIF {

    /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
     * assumed to be equal for the same class of sensors */
-    float gyr02variance[3] = {pow(3.0e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
-                              pow(3.0e-3, 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
-                              pow(4.3e-3, 2)};  // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
+    float gyr02variance[3] = {pow(4.6e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
+                              pow(4.6e-3, 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
+                              pow(6.1e-3, 2)};  // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
    float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
    uint8_t preferAdis = false;
    float gyrFilterWeight = 0.6;
@ -816,7 +817,7 @@ class AcsParameters : public HasParametersIF {
        {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
    double without4[4][3] = {
        {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
-    double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
+    double nullspaceVector[4] = {-1, 1, -1, 1};
  } rwMatrices;

  struct SafeModeControllerParameters {
@ -840,7 +841,9 @@ class AcsParameters : public HasParametersIF {
    double om = 0.3;
    double omMax = 1 * M_PI / 180;
    double qiMin = 0.1;
+
    double gainNullspace = 0.01;
+    double nullspaceSpeed = 32500;  // 0.1 RPM

    double desatMomentumRef[3] = {0, 0, 0};
    double deSatGainFactor = 1000;
@ -933,7 +936,7 @@ class AcsParameters : public HasParametersIF {
    double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
    double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
    double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
-    double dipolMax = 0.2;  // [Am^2]
+    double dipoleMax = 0.2;  // [Am^2]

    uint16_t torqueDuration = 300;  // [ms]
  } magnetorquerParameter;
--- a/mission/controller/acs/ActuatorCmd.cpp
+++ b/mission/controller/acs/ActuatorCmd.cpp
@ -5,11 +5,6 @@
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>

-#include <cmath>
-
-#include "util/CholeskyDecomposition.h"
-#include "util/MathOperations.h"
-
 ActuatorCmd::ActuatorCmd() {}

 ActuatorCmd::~ActuatorCmd() {}
@ -25,24 +20,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
  }
 }

-void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
-                                int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
-                                double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
-  using namespace Math;
-
-  // Calculating the commanded speed in RPM for every reaction wheel
+void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
+                                const int32_t speedRw2, const int32_t speedRw3,
+                                const double sampleTime, const double inertiaWheel,
+                                const int32_t maxRwSpeed, const double *rwTorque,
+                                int32_t *rwCmdSpeed) {
+  // group RW speed values (in 0.1 [RPM]) in vector
  int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
+
+  // calculate required RW speed as sum of current RW speed and RW speed delta
+  // delta w_rw = delta t / I_RW * torque_RW [rad/s]
  double deltaSpeed[4] = {0, 0, 0, 0};
-  double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
-  //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
-  double factor = sampleTime / inertiaWheel * radToRpm;
-  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
+  const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
  VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
+
+  // convert double to int32
+  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
  for (int i = 0; i < 4; i++) {
    deltaSpeedInt[i] = std::round(deltaSpeed[i]);
  }
+
+  // sum of current RW speed and RW speed delta
  VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
-  VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
+
+  // crop values which would exceed the maximum possible RPM
  for (uint8_t i = 0; i < 4; i++) {
    if (rwCmdSpeed[i] > maxRwSpeed) {
      rwCmdSpeed[i] = maxRwSpeed;
@ -52,24 +53,25 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee
  }
 }

-void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
-                              const double *inverseAlignment, double maxDipol) {
-  // Convert to actuator frame
-  double dipolMomentActuatorDouble[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
-                                     1);
-  // Scaling along largest element if dipol exceeds maximum
+void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
+                               const double *dipoleMoment, int16_t *dipoleMomentActuator) {
+  // convert to actuator frame
+  double dipoleMomentActuatorDouble[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3,
+                                     3, 1);
+  // scaling along largest element if dipole exceeds maximum
  uint8_t maxIdx = 0;
-  VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
-  double maxAbsValue = std::abs(dipolMomentActuatorDouble[maxIdx]);
-  if (maxAbsValue > maxDipol) {
-    double scalingFactor = maxDipol / maxAbsValue;
-    VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
-                                        dipolMomentActuatorDouble, 3);
+  VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
+  double maxAbsValue = std::abs(dipoleMomentActuatorDouble[maxIdx]);
+  if (maxAbsValue > maxDipole) {
+    double scalingFactor = maxDipole / maxAbsValue;
+    VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
+                                        dipoleMomentActuatorDouble, 3);
  }
  // scale dipole from 1 Am^2 to 1e^-4 Am^2
-  VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
+  VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble,
+                                      3);
  for (int i = 0; i < 3; i++) {
-    dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
+    dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
  }
 }
--- a/mission/controller/acs/ActuatorCmd.h
+++ b/mission/controller/acs/ActuatorCmd.h
@ -1,9 +1,7 @@
 #ifndef ACTUATORCMD_H_
 #define ACTUATORCMD_H_

-#include "MultiplicativeKalmanFilter.h"
-#include "SensorProcessing.h"
-#include "SensorValues.h"
+#include <cmath>

 class ActuatorCmd {
 public:
@ -19,29 +17,30 @@ class ActuatorCmd {
  void scalingTorqueRws(double *rwTrq, double maxTorque);

  /*
-   * @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
-   * as Input to the RWs
-   * @param:	rwTrqIn 			given torque from pointing controller
-   * 			rwTrqNS				Nullspace torque
+   * @brief:	cmdSpeedToRws()		Calculates the RPM for the reaction wheel configuration,
+   * given the required torque calculated by the controller. Will also scale down the RPM of the
+   * wheels if they exceed the maximum possible RPM
+   * @param:	rwTrq 			    given torque from pointing controller
   * 			rwCmdSpeed			output revolutions per minute for every
   * reaction wheel
   */
-  void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3,
-                     const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime,
-                     int32_t maxRwSpeed, double inertiaWheel);
+  void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+                     const int32_t speedRw3, const double sampleTime, const double inertiaWheel,
+                     const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed);

  /*
-   * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
+   * @brief:	cmdDipoleMtq()			gives the commanded dipole moment for the
+   * magnetorquer
   *
-   * @param:	dipolMoment 		given dipol moment in spacecraft frame
-   * 			dipolMomentActuator	resulting dipol moment in actuator reference frame
+   * @param:	dipoleMoment 			given dipole moment in spacecraft frame
+   * 			dipoleMomentActuator	resulting dipole moment in actuator reference frame
   */
-  void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
-                   const double *inverseAlignment, double maxDipol);
+  void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
+                    const double *dipoleMoment, int16_t *dipoleMomentActuator);

 protected:
 private:
+  static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
 };

 #endif /* ACTUATORCMD_H_ */
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }

-void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
+void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
+                                double targetSatRotRate[3]) {
  //-------------------------------------------------------------------------------------
  //	Calculation of target quaternion to sun
  //-------------------------------------------------------------------------------------
@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
  //----------------------------------------------------------------------------
  //	Calculation of reference rotation rate
  //----------------------------------------------------------------------------
-  refSatRate[0] = 0;
-  refSatRate[1] = 0;
-  refSatRate[2] = 0;
+  int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
+  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
 }

 void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
@ -412,7 +412,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl

 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                          double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
-                          double errorQuat[4], double errorSatRotRate[3], double errorAngle) {
+                          double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
  // First calculate error quaternion between current and target orientation
  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
  // Last calculate add rotation from reference quaternion
@ -424,26 +424,17 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
  // Calculate error angle
  errorAngle = QuaternionOperations::getAngle(errorQuat, true);

-  // Only give back error satellite rotational rate if orientation has already been aquired
-  if (errorAngle < 2. / 180. * M_PI) {
-    // First combine the target and reference satellite rotational rates
-    double combinedRefSatRotRate[3] = {0, 0, 0};
-    VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
-    // Then substract the combined required satellite rotational rates from the actual rate
-    VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate,
-                                       3);
-  } else {
-    // If orientation has not been aquired yet set satellite rotational rate to zero
-    errorSatRotRate = 0;
-  }
-
-  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
-  // under 150 arcsec ??
+  // Calculate error satellite rotational rate
+  // First combine the target and reference satellite rotational rates
+  double combinedRefSatRotRate[3] = {0, 0, 0};
+  VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
+  // Then subtract the combined required satellite rotational rates from the actual rate
+  VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3);
 }

 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                          double targetSatRotRate[3], double errorQuat[4],
-                          double errorSatRotRate[3], double errorAngle) {
+                          double errorSatRotRate[3], double &errorAngle) {
  // First calculate error quaternion between current and target orientation
  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
  // Keep scalar part of quaternion positive
@ -453,17 +444,8 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
  // Calculate error angle
  errorAngle = QuaternionOperations::getAngle(errorQuat, true);

-  // Only give back error satellite rotational rate if orientation has already been aquired
-  if (errorAngle < 2. / 180. * M_PI) {
-    // Then substract the combined required satellite rotational rates from the actual rate
-    VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
-  } else {
-    // If orientation has not been aquired yet set satellite rotational rate to zero
-    errorSatRotRate = 0;
-  }
-
-  // target flag in matlab, importance, does look like it only gives feedback if pointing control is
-  // under 150 arcsec ??
+  // Calculate error satellite rotational rate
+  VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
 }

 void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
@ -471,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
  //-------------------------------------------------------------------------------------
  //	Calculation of target rotation rate
  //-------------------------------------------------------------------------------------
-  double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
-                       (timeSavedQuaternion.tv_sec +
-                        timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
+  double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
+                       (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
+  if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
+    std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
+  }
  if (timeElapsed < timeElapsedMax) {
+    double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
+    QuaternionOperations::inverse(quatInertialTarget, q);
+    QuaternionOperations::inverse(savedQuaternion, qS);
    double qDiff[4] = {0, 0, 0, 0};
-    VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
+    VectorOperations<double>::subtract(q, qS, 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 tgtQuatVec[3] = {q[0], q[1], q[2]};
+    double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
    double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
-    VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
+    VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
-    VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
+    VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
    VectorOperations<double>::add(sum1, sum2, sum, 3);
    VectorOperations<double>::subtract(sum, sum3, sum, 3);
    double omegaRefNew[3] = {0, 0, 0};
@ -531,10 +518,6 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
    return returnvalue::OK;
  } else {
-    // 	    @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 invalid the pointing control is destined to fail.
    return returnvalue::FAILED;
  }
 }
--- a/mission/controller/acs/Guidance.h
+++ b/mission/controller/acs/Guidance.h
@ -15,7 +15,7 @@ class Guidance {
  void getTargetParamsSafe(double sunTargetSafe[3]);
  ReturnValue_t solarArrayDeploymentComplete();

-  // Function to get the target quaternion and refence rotation rate from gps position and
+  // Function to get the target quaternion and reference rotation rate from gps position and
  // position of the ground station
  void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
                               double refDirB[3], double quatBI[4], double targetQuat[4],
@ -25,9 +25,10 @@ class Guidance {
  void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
                       double targetSatRotRate[3]);

-  // Function to get the target quaternion and refence rotation rate for sun pointing after ground
+  // Function to get the target quaternion and reference rotation rate for sun pointing after ground
  // station
-  void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);
+  void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
+                        double targetSatRotRate[3]);

  // Function to get the target quaternion and refence rotation rate from gps position for Nadir
  // pointing
@ -37,15 +38,15 @@ class Guidance {
                                   double targetQuat[4], double refSatRate[3]);

  // @note: Calculates the error quaternion between the current orientation and the target
-  // quaternion, considering a reference quaternion. Additionally the difference between the  actual
+  // quaternion, considering a reference quaternion. Additionally the difference between the actual
  // and a desired satellite rotational rate is calculated, again considering a reference rotational
  // rate. Lastly gives back the error angle of the error quaternion.
  void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                  double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
-                  double errorQuat[4], double errorSatRotRate[3], double errorAngle);
+                  double errorQuat[4], double errorSatRotRate[3], double &errorAngle);
  void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                  double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
-                  double errorAngle);
+                  double &errorAngle);

  void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
                          double *targetSatRotRate);
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@ -30,10 +30,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
  // ------------------------------------------------
  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
@ -45,14 +42,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
    {
      PoolReadGuard pg(mgmDataProcessed);
      if (pg.getReadResult() == returnvalue::OK) {
-        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));
+        std::memcpy(mgmDataProcessed->mgm0vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm1vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm2vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm3vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgm4vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(double));
+        std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double));
        mgmDataProcessed->setValidity(false, true);
        std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
        mgmDataProcessed->magIgrfModel.setValid(gpsValid);
@ -210,63 +206,68 @@ void SensorProcessing::processSus(
  sunIjkModel[0] = cos(eclipticLongitude);
  sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
  sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
+
+  uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  if (sus0valid) {
-    sus0valid = susConverter.checkSunSensorData(sus0Value);
+    susBrightness[0] = susConverter.checkSunSensorData(sus0Value);
  }
  if (sus1valid) {
-    sus1valid = susConverter.checkSunSensorData(sus1Value);
+    susBrightness[1] = susConverter.checkSunSensorData(sus1Value);
  }
  if (sus2valid) {
-    sus2valid = susConverter.checkSunSensorData(sus2Value);
+    susBrightness[2] = susConverter.checkSunSensorData(sus2Value);
  }
  if (sus3valid) {
-    sus3valid = susConverter.checkSunSensorData(sus3Value);
+    susBrightness[3] = susConverter.checkSunSensorData(sus3Value);
  }
  if (sus4valid) {
-    sus4valid = susConverter.checkSunSensorData(sus4Value);
+    susBrightness[4] = susConverter.checkSunSensorData(sus4Value);
  }
  if (sus5valid) {
-    sus5valid = susConverter.checkSunSensorData(sus5Value);
+    susBrightness[5] = susConverter.checkSunSensorData(sus5Value);
  }
  if (sus6valid) {
-    sus6valid = susConverter.checkSunSensorData(sus6Value);
+    susBrightness[6] = susConverter.checkSunSensorData(sus6Value);
  }
  if (sus7valid) {
-    sus7valid = susConverter.checkSunSensorData(sus7Value);
+    susBrightness[7] = susConverter.checkSunSensorData(sus7Value);
  }
  if (sus8valid) {
-    sus8valid = susConverter.checkSunSensorData(sus8Value);
+    susBrightness[8] = susConverter.checkSunSensorData(sus8Value);
  }
  if (sus9valid) {
-    sus9valid = susConverter.checkSunSensorData(sus9Value);
+    susBrightness[9] = susConverter.checkSunSensorData(sus9Value);
  }
  if (sus10valid) {
-    sus10valid = susConverter.checkSunSensorData(sus10Value);
+    susBrightness[10] = susConverter.checkSunSensorData(sus10Value);
  }
  if (sus11valid) {
-    sus11valid = susConverter.checkSunSensorData(sus11Value);
+    susBrightness[11] = susConverter.checkSunSensorData(sus11Value);
  }

-  if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
-      !sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
+  bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid,  sus5valid,
+                       sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
+  bool allInvalid =
+      susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold);
+
+  if (allInvalid) {
    {
      PoolReadGuard pg(susDataProcessed);
      if (pg.getReadResult() == returnvalue::OK) {
-        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));
+        std::memcpy(susDataProcessed->sus0vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus1vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus2vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus3vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus4vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus5vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus6vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus7vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus8vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus9vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus10vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->sus11vec.value, ZERO_VEC_F, 3 * sizeof(float));
+        std::memcpy(susDataProcessed->susVecTot.value, ZERO_VEC_D, 3 * sizeof(double));
+        std::memcpy(susDataProcessed->susVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double));
        susDataProcessed->setValidity(false, true);
        std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
        susDataProcessed->sunIjkModel.setValid(true);
@ -274,118 +275,78 @@ void SensorProcessing::processSus(
    }
    return;
  }
-  // WARNING: NOT TRANSFORMED IN BODY FRAME YET
-  // Transformation into Geomtry Frame
-  float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
-        sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
-        sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
-        sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};

-  if (sus0valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus0orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
-                                             susParameters->sus0coeffBeta),
-        sus0VecBody, 3, 3, 1);
-  }
-  if (sus1valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus1orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
-                                             susParameters->sus1coeffBeta),
-        sus1VecBody, 3, 3, 1);
-  }
-  if (sus2valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus2orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
-                                             susParameters->sus2coeffBeta),
-        sus2VecBody, 3, 3, 1);
-  }
-  if (sus3valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus3orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
-                                             susParameters->sus3coeffBeta),
-        sus3VecBody, 3, 3, 1);
-  }
-  if (sus4valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus4orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
-                                             susParameters->sus4coeffBeta),
-        sus4VecBody, 3, 3, 1);
-  }
-  if (sus5valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus5orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
-                                             susParameters->sus5coeffBeta),
-        sus5VecBody, 3, 3, 1);
-  }
-  if (sus6valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus6orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
-                                             susParameters->sus6coeffBeta),
-        sus6VecBody, 3, 3, 1);
-  }
-  if (sus7valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus7orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
-                                             susParameters->sus7coeffBeta),
-        sus7VecBody, 3, 3, 1);
-  }
-  if (sus8valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus8orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
-                                             susParameters->sus8coeffBeta),
-        sus8VecBody, 3, 3, 1);
-  }
-  if (sus9valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus9orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
-                                             susParameters->sus9coeffBeta),
-        sus9VecBody, 3, 3, 1);
-  }
-  if (sus10valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus10orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
-                                             susParameters->sus10coeffBeta),
-        sus10VecBody, 3, 3, 1);
-  }
-  if (sus11valid) {
-    MatrixOperations<float>::multiply(
-        susParameters->sus11orientationMatrix[0],
-        susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
-                                             susParameters->sus11coeffBeta),
-        sus11VecBody, 3, 3, 1);
-  }
+  float susVecSensor[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
+                               {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  float susVecBody[12][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0},
+                             {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

-  /* ------ Mean Value: susDirEst ------ */
-  bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid,  sus5valid,
-                       sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
-  float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
-                              sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
-                              sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
-                             {sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
-                              sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
-                              sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
-                             {sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
-                              sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
-                              sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
+  if (susValid[0]) {
+    susConverter.calculateSunVector(susVecSensor[0], sus0Value);
+    MatrixOperations<float>::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0],
+                                      susVecBody[0], 3, 3, 1);
+  }
+  if (susValid[1]) {
+    susConverter.calculateSunVector(susVecSensor[1], sus1Value);
+    MatrixOperations<float>::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1],
+                                      susVecBody[1], 3, 3, 1);
+  }
+  if (susValid[2]) {
+    susConverter.calculateSunVector(susVecSensor[2], sus2Value);
+    MatrixOperations<float>::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2],
+                                      susVecBody[2], 3, 3, 1);
+  }
+  if (susValid[3]) {
+    susConverter.calculateSunVector(susVecSensor[3], sus3Value);
+    MatrixOperations<float>::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3],
+                                      susVecBody[3], 3, 3, 1);
+  }
+  if (susValid[4]) {
+    susConverter.calculateSunVector(susVecSensor[4], sus4Value);
+    MatrixOperations<float>::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4],
+                                      susVecBody[4], 3, 3, 1);
+  }
+  if (susValid[5]) {
+    susConverter.calculateSunVector(susVecSensor[5], sus5Value);
+    MatrixOperations<float>::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5],
+                                      susVecBody[5], 3, 3, 1);
+  }
+  if (susValid[6]) {
+    susConverter.calculateSunVector(susVecSensor[6], sus6Value);
+    MatrixOperations<float>::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6],
+                                      susVecBody[6], 3, 3, 1);
+  }
+  if (susValid[7]) {
+    susConverter.calculateSunVector(susVecSensor[7], sus7Value);
+    MatrixOperations<float>::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7],
+                                      susVecBody[7], 3, 3, 1);
+  }
+  if (susValid[8]) {
+    susConverter.calculateSunVector(susVecSensor[8], sus8Value);
+    MatrixOperations<float>::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8],
+                                      susVecBody[8], 3, 3, 1);
+  }
+  if (susValid[9]) {
+    susConverter.calculateSunVector(susVecSensor[9], sus9Value);
+    MatrixOperations<float>::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9],
+                                      susVecBody[9], 3, 3, 1);
+  }
+  if (susValid[10]) {
+    susConverter.calculateSunVector(susVecSensor[10], sus10Value);
+    MatrixOperations<float>::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10],
+                                      susVecBody[10], 3, 3, 1);
+  }
+  if (susValid[11]) {
+    susConverter.calculateSunVector(susVecSensor[11], sus11Value);
+    MatrixOperations<float>::multiply(susParameters->sus11orientationMatrix[0], susVecSensor[11],
+                                      susVecBody[11], 3, 3, 1);
+  }

  double susMeanValue[3] = {0, 0, 0};
  for (uint8_t i = 0; i < 12; i++) {
-    if (validIds[i]) {
-      susMeanValue[0] += susVecBody[0][i];
-      susMeanValue[1] += susVecBody[1][i];
-      susMeanValue[2] += susVecBody[2][i];
-    }
+    susMeanValue[0] += susVecBody[i][0];
+    susMeanValue[1] += susVecBody[i][1];
+    susMeanValue[2] += susVecBody[i][2];
  }
  double susVecTot[3] = {0.0, 0.0, 0.0};
  VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
@ -406,29 +367,29 @@ void SensorProcessing::processSus(
  {
    PoolReadGuard pg(susDataProcessed);
    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus0vec.value, susVecBody[0], 3 * sizeof(float));
      susDataProcessed->sus0vec.setValid(sus0valid);
-      std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus1vec.value, susVecBody[1], 3 * sizeof(float));
      susDataProcessed->sus1vec.setValid(sus1valid);
-      std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus2vec.value, susVecBody[2], 3 * sizeof(float));
      susDataProcessed->sus2vec.setValid(sus2valid);
-      std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus3vec.value, susVecBody[3], 3 * sizeof(float));
      susDataProcessed->sus3vec.setValid(sus3valid);
-      std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus4vec.value, susVecBody[4], 3 * sizeof(float));
      susDataProcessed->sus4vec.setValid(sus4valid);
-      std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus5vec.value, susVecBody[5], 3 * sizeof(float));
      susDataProcessed->sus5vec.setValid(sus5valid);
-      std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus6vec.value, susVecBody[6], 3 * sizeof(float));
      susDataProcessed->sus6vec.setValid(sus6valid);
-      std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus7vec.value, susVecBody[7], 3 * sizeof(float));
      susDataProcessed->sus7vec.setValid(sus7valid);
-      std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus8vec.value, susVecBody[8], 3 * sizeof(float));
      susDataProcessed->sus8vec.setValid(sus8valid);
-      std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus9vec.value, susVecBody[9], 3 * sizeof(float));
      susDataProcessed->sus9vec.setValid(sus9valid);
-      std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus10vec.value, susVecBody[10], 3 * sizeof(float));
      susDataProcessed->sus10vec.setValid(sus10valid);
-      std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
+      std::memcpy(susDataProcessed->sus11vec.value, susVecBody[11], 3 * sizeof(float));
      susDataProcessed->sus11vec.setValid(sus11valid);
      std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
      susDataProcessed->susVecTot.setValid(true);
@ -459,12 +420,11 @@ 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));
-        std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double));
-        std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyr0vec.value, ZERO_VEC_D, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyr1vec.value, ZERO_VEC_D, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyr2vec.value, ZERO_VEC_D, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyr3vec.value, ZERO_VEC_D, 3 * sizeof(double));
+        std::memcpy(gyrDataProcessed->gyrVecTot.value, ZERO_VEC_D, 3 * sizeof(double));
        gyrDataProcessed->setValidity(false, true);
      }
    }
--- a/mission/controller/acs/SensorProcessing.h
+++ b/mission/controller/acs/SensorProcessing.h
@ -23,6 +23,9 @@ class SensorProcessing {
               acsctrl::GpsDataProcessed *gpsDataProcessed,
               const AcsParameters *acsParameters);  // Will call protected functions
 private:
+  static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
+  static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
+
 protected:
  // short description needed for every function
  void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
--- a/mission/controller/acs/SusConverter.cpp
+++ b/mission/controller/acs/SusConverter.cpp
@ -1,121 +1,64 @@
 #include "SusConverter.h"

-#include <fsfw/datapoollocal/LocalPoolVariable.h>
-#include <fsfw/datapoollocal/LocalPoolVector.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <math.h>

-#include <iostream>
-
-bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
-  if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh ||
+uint64_t SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
+  if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH ||
      susChannel[0] > susChannel[GNDREF]) {
-    return false;
+    return 0;
  }
-  if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh ||
+  if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH ||
      susChannel[1] > susChannel[GNDREF]) {
-    return false;
+    return 0;
  };
-  if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh ||
+  if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH ||
      susChannel[2] > susChannel[GNDREF]) {
-    return false;
+    return 0;
  };
-  if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh ||
+  if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH ||
      susChannel[3] > susChannel[GNDREF]) {
-    return false;
+    return 0;
  };

-  susChannelValueSum =
+  uint64_t susChannelValueSum =
      4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]);
-  if ((susChannelValueSum < susChannelValueSumHigh) &&
-      (susChannelValueSum > susChannelValueSumLow)) {
-    return false;
+  if (susChannelValueSum < SUS_ALBEDO_CHECK) {
+    return 0;
  };
-  return true;
+  return susChannelValueSum;
 }

-void SusConverter::calcAngle(const uint16_t susChannel[6]) {
-  float xout, yout;
-  float s = 0.03;  // s=[mm] gap between diodes
-  uint8_t d = 5;   // d=[mm] edge length of the quadratic aperture
-  uint8_t h = 1;   // h=[mm] distance between diodes and aperture
-  int ch0, ch1, ch2, ch3;
+bool SusConverter::checkValidity(bool* susValid, const uint64_t brightness[12],
+                                 const float threshold) {
+  uint8_t maxBrightness = 0;
+  VectorOperations<uint64_t>::maxValue(brightness, 12, &maxBrightness);
+  if (brightness[maxBrightness] == 0) {
+    return true;
+  }
+  for (uint8_t idx = 0; idx < 12; idx++) {
+    if ((idx != maxBrightness) and (brightness[idx] < threshold * brightness[maxBrightness])) {
+      susValid[idx] = false;
+      continue;
+    }
+    susValid[idx] = true;
+  }
+  return false;
+}
+
+void SusConverter::calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]) {
  // Substract measurement values from GNDREF zero current threshold
-  ch0 = susChannel[GNDREF] - susChannel[0];
-  ch1 = susChannel[GNDREF] - susChannel[1];
-  ch2 = susChannel[GNDREF] - susChannel[2];
-  ch3 = susChannel[GNDREF] - susChannel[3];
+  float ch0 = susChannel[GNDREF] - susChannel[0];
+  float ch1 = susChannel[GNDREF] - susChannel[1];
+  float ch2 = susChannel[GNDREF] - susChannel[2];
+  float ch3 = susChannel[GNDREF] - susChannel[3];

  // Calculation of x and y
-  xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]
-  yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]
+  float xout = ((D - S) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]
+  float yout = ((D - S) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3);  //[mm]

  // Calculation of the angles
-  alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI);  //[°]
-  alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI);  //[°]
-}
-
-void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
-  uint8_t index, k, l;
-
-  // while loop iterates above all calibration cells to use the different calibration functions in
-  // each cell
-  k = 0;
-  while (k < 3) {
-    k++;
-    l = 0;
-    while (l < 3) {
-      l++;
-      // if-condition to check in which cell the data point has to be
-      if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) &&
-           alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) &&
-          (alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) &&
-           alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) {
-        index = (3 * (k - 1) + l) - 1;  // calculate the index of the datapoint for the right cell
-        alphaBetaCalibrated[0] =
-            coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] +
-            coeffAlpha[index][2] * alphaBetaRaw[1] +
-            coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
-            coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
-            coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
-            coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
-            coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
-            coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
-            coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1];  //[°]
-        alphaBetaCalibrated[1] =
-            coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] +
-            coeffBeta[index][2] * alphaBetaRaw[1] +
-            coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
-            coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
-            coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
-            coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
-            coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
-            coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
-            coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1];  //[°]
-      }
-    }
-  }
-}
-
-float* SusConverter::calculateSunVector() {
-  // Calculate the normalized Sun Vector
-  sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
-                              (sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
-                                    powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
-  sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
-                              (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
-                                    powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
-  sunVectorSensorFrame[2] =
-      -(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
-                   powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
-
-  return sunVectorSensorFrame;
-}
-
-float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6],
-                                             const float coeffAlpha[9][10],
-                                             const float coeffBeta[9][10]) {
-  calcAngle(susChannel);
-  calibration(coeffAlpha, coeffBeta);
-  return calculateSunVector();
+  sunVectorSensorFrame[0] = -xout;
+  sunVectorSensorFrame[1] = -yout;
+  sunVectorSensorFrame[2] = H;
+  VectorOperations<float>::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3);
 }
--- a/mission/controller/acs/SusConverter.h
+++ b/mission/controller/acs/SusConverter.h
@ -1,8 +1,4 @@
-#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
-#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
-
-#include <fsfw/datapoollocal/LocalPoolVector.h>
-#include <stdint.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>

 #include "AcsParameters.h"

@ -10,41 +6,26 @@ class SusConverter {
 public:
  SusConverter() {}

-  bool checkSunSensorData(const uint16_t susChannel[6]);
-
-  void calcAngle(const uint16_t susChannel[6]);
-  void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]);
-  float* calculateSunVector();
-
-  float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10],
-                                 const float coeffBeta[9][10]);
+  uint64_t checkSunSensorData(const uint16_t susChannel[6]);
+  bool checkValidity(bool* susValid, const uint64_t brightness[12], const float threshold);
+  void calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]);

 private:
-  float alphaBetaRaw[2];          //[°]
-  float alphaBetaCalibrated[2];   //[°]
-  float sunVectorSensorFrame[3];  //[-]
-
-  bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,
-                        returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,
-                        returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK};
-
  static const uint8_t GNDREF = 4;
-  uint16_t susChannelValueCheckHigh =
-      4096;  //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check
-  uint8_t susChannelValueCheckLow =
-      0;  //[Bit]low borderline for the channel values of one sun sensor for validity Check
-  uint16_t susChannelValueSumHigh =
-      100;  // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by
-            // the reflection of sunlight from the moon/earth
-  uint8_t susChannelValueSumLow =
-      0;  //[Bit]low borderline for check if the sun sensor is illuminated
-          // by the sun or by the reflection of sunlight from the moon/earth
-  uint8_t completeCellWidth = 140,
-          halfCellWidth = 70;  //[°] Width of the calibration cells --> necessary for checking in
-                               // which cell a data point should be
-  uint16_t susChannelValueSum = 0;
+  // =2^12[Bit]high borderline for the channel values of one sun sensor for validity Check
+  static constexpr uint16_t SUS_CHANNEL_VALUE_HIGH = 4096;
+  // [Bit]low borderline for the channel values of one sun sensor for validity Check
+  static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0;
+  // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the
+  // reflection of sunlight from the moon/earth
+  static constexpr uint16_t SUS_ALBEDO_CHECK = 1000;
+  // [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the  reflection
+  // of sunlight from the moon/earth
+  static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0;
+
+  static constexpr float S = 0.03;  // S=[mm] gap between diodes
+  static constexpr float D = 5;     // D=[mm] edge length of the quadratic aperture
+  static constexpr float H = 2.5;   // H=[mm] distance between diodes and aperture

  AcsParameters acsParameters;
 };
-
-#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */
--- a/mission/controller/acs/control/Detumble.cpp
+++ b/mission/controller/acs/control/Detumble.cpp
@ -7,8 +7,10 @@ Detumble::Detumble() {}

 Detumble::~Detumble() {}

-uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
-                                   const bool magFieldRateValid, const bool useFullDetumbleLaw) {
+acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
+                                                 const bool satRotRateValid,
+                                                 const bool magFieldRateValid,
+                                                 const bool useFullDetumbleLaw) {
  if (not magFieldValid) {
    return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
  } else if (satRotRateValid and useFullDetumbleLaw) {
--- a/mission/controller/acs/control/Detumble.h
+++ b/mission/controller/acs/control/Detumble.h
@ -11,8 +11,9 @@ class Detumble {
  Detumble();
  virtual ~Detumble();

-  uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
-                           const bool magFieldRateValid, const bool useFullDetumbleLaw);
+  acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
+                                         const bool magFieldRateValid,
+                                         const bool useFullDetumbleLaw);

  void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
                   double gain);
--- a/mission/controller/acs/control/PtgCtrl.cpp
+++ b/mission/controller/acs/control/PtgCtrl.cpp
@ -5,9 +5,6 @@
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/globalfunctions/sign.h>
-#include <math.h>
-
-#include "../util/MathOperations.h"

 PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }

@ -32,12 +29,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
  double qErrorLaw[3] = {0, 0, 0};

  for (int i = 0; i < 3; i++) {
-    if (abs(qError[i]) < qErrorMin) {
+    if (std::abs(qError[i]) < qErrorMin) {
      qErrorLaw[i] = qErrorMin;
    } else {
-      qErrorLaw[i] = abs(qError[i]);
+      qErrorLaw[i] = std::abs(qError[i]);
    }
  }
+
  double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);

  double gain1 = cInt * omMax / qErrorLawNorm;
@ -73,7 +71,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
  double pErrorSign[3] = {0, 0, 0};

  for (int i = 0; i < 3; i++) {
-    if (abs(pError[i]) > 1) {
+    if (std::abs(pError[i]) > 1) {
      pErrorSign[i] = sign(pError[i]);
    } else {
      pErrorSign[i] = pError[i];
@ -98,61 +96,92 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
  VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
 }

+void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+                           const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+                           const int32_t speedRw3, double *rwTrqNs) {
+  // concentrate RW speeds as vector and convert to double
+  double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
+                        static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
+  VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
+  VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
+
+  // calculate RPM offset utilizing the nullspace
+  double rpmOffset[4] = {0, 0, 0, 0};
+  double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC;
+  VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed,
+                                      rpmOffset, 4);
+
+  // calculate resulting angular momentum
+  double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0};
+  VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
+  VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
+                                      rwAngMomentum, 4);
+
+  // calculate resulting torque
+  double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+  MatrixOperations<double>::multiply(acsParameters->rwMatrices.nullspaceVector,
+                                     acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4,
+                                     1, 4);
+  MatrixOperations<double>::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1);
+  VectorOperations<double>::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs,
+                                      4);
+}
+
 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) || !(pointingLawParameters->desatOn)) {
-    mgtDpDes[0] = 0;
-    mgtDpDes[1] = 0;
-    mgtDpDes[2] = 0;
+                              const double *magFieldB, const bool magFieldBValid,
+                              const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
+                              const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
+  if (not magFieldBValid or not pointingLawParameters->desatOn) {
    return;
  }

-  // calculating momentum of satellite and momentum of reaction wheels
-  double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
-  double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
-                                      momentumRwU, 4);
-  MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
-                                     momentumRw, 3, 4, 1);
-  double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
-                                     momentumSat, 3, 3, 1);
-  VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
-  // calculating momentum error
-  double deltaMomentum[3] = {0, 0, 0};
-  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 = (pointingLawParameters->deSatGainFactor) / normMag;
-  VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
-}
+  // concentrate RW speeds as vector and convert to double
+  double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
+                        static_cast<double>(speedRw2), static_cast<double>(speedRw3)};

-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};
-  double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
-  // conversion to [rad/s] for further calculations
-  VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
-  VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
-  double diffRwSpeed[4] = {0, 0, 0, 0};
-  VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
-  VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
-                                      wheelMomentum, 4);
-  double gainNs = pointingLawParameters->gainNullspace;
-  double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
-                                                acsParameters->rwMatrices.nullspace,
-                                                *nullSpaceMatrix, 4);
-  MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
-  VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
-  VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
+  // convert magFieldB from uT to T
+  double magFieldBT[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
+
+  // calculate angular momentum of the satellite
+  double angMomentumSat[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
+                                     angMomentumSat, 3, 3, 1);
+
+  // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
+  // relocate RW speed zero to nullspace RW speed
+  double refSpeedRws[4] = {0, 0, 0, 0};
+  VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
+                                      pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
+  VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
+  // convert speed from 10 RPM to 1 RPM
+  VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
+  // convert to rad/s
+  VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
+  // calculate angular momentum of each RW
+  double angMomentumRwU[4] = {0, 0, 0, 0};
+  VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
+                                      angMomentumRwU, 4);
+  // convert RW angular momentum to body RF
+  double angMomentumRw[3] = {0, 0, 0};
+  MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU,
+                                     angMomentumRw, 3, 4, 1);
+
+  // calculate total angular momentum
+  double angMomentumTotal[3] = {0, 0, 0};
+  VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
+
+  // calculating momentum error
+  double deltaAngMomentum[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
+                                     deltaAngMomentum, 3);
+
+  // resulting magnetic dipole command
+  double crossAngMomentumMagField[3] = {0, 0, 0};
+  VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
+  double factor =
+      pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
+  VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
 }

 void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
@ -169,15 +198,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
      if (rwCmdSpeeds[i] != 0) {
        if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
            rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
-          if (currRwSpeed[i] == 0) {
-            if (rwCmdSpeeds[i] > 0) {
-              rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
-            } else if (rwCmdSpeeds[i] < 0) {
-              rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
-            }
-          } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) {
+          if (rwCmdSpeeds[i] > currRwSpeed[i]) {
            rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
-          } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) {
+          } else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
            rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
          }
        }
--- a/mission/controller/acs/control/PtgCtrl.h
+++ b/mission/controller/acs/control/PtgCtrl.h
@ -1,13 +1,10 @@
 #ifndef PTGCTRL_H_
 #define PTGCTRL_H_

+#include <math.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/SensorValues.h>
 #include <stdio.h>
-#include <string.h>
-#include <time.h>
-
-#include "../AcsParameters.h"
-#include "../SensorValues.h"
-#include "eive/resultClassIds.h"

 class PtgCtrl {
  /*
@ -29,14 +26,14 @@ class PtgCtrl {
  void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
              const double *deltaRate, const double *rwPseudoInv, double *torqueRws);

-  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(AcsParameters::PointingLawParameters *pointingLawParameters,
-                    const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
-                    const int32_t *speedRw3, double *rwTrqNs);
+                    const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+                    const int32_t speedRw3, double *rwTrqNs);
+
+  void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
+                       const double *magFieldB, const bool magFieldBValid, const double *satRate,
+                       const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+                       const int32_t speedRw3, double *mgtDpDes);

  /* @brief: Commands the stiction torque in case wheel speed is to low
   * 		 torqueCommand  modified torque after antistiction
@ -45,6 +42,7 @@ class PtgCtrl {

 private:
  const AcsParameters *acsParameters;
+  static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
 };

 #endif /* ACS_CONTROL_PTGCTRL_H_ */
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter

 SafeCtrl::~SafeCtrl() {}

-uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
-                                   const bool satRotRateValid, const bool sunDirValid,
-                                   const uint8_t mekfEnabled, const uint8_t dampingEnabled) {
+acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+                                                 const bool satRotRateValid, const bool sunDirValid,
+                                                 const uint8_t mekfEnabled,
+                                                 const uint8_t dampingEnabled) {
  if (not magFieldValid) {
    return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
  } else if (mekfEnabled and mekfValid) {
--- a/mission/controller/acs/control/SafeCtrl.h
+++ b/mission/controller/acs/control/SafeCtrl.h
@ -12,9 +12,9 @@ class SafeCtrl {
  SafeCtrl(AcsParameters *acsParameters_);
  virtual ~SafeCtrl();

-  uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
-                           const bool satRotRateValid, const bool sunDirValid,
-                           const uint8_t mekfEnabled, const uint8_t dampingEnabled);
+  acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+                                         const bool satRotRateValid, const bool sunDirValid,
+                                         const uint8_t mekfEnabled, const uint8_t dampingEnabled);

  void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
                const double *quatBI, const double *sunDirRefB, double *magMomB,
--- a/mission/genericFactory.cpp
+++ b/mission/genericFactory.cpp
@ -49,6 +49,7 @@
 #include "mission/system/acs/acsModeTree.h"
 #include "mission/system/tcs/tcsModeTree.h"
 #include "mission/tcs/defs.h"
+#include "mission/tmtc/Service15TmStorage.h"
 #include "mission/tmtc/tmFilters.h"
 #include "objects/systemObjectList.h"
 #include "tmtc/pusIds.h"
@ -58,15 +59,13 @@ using persTmStore::PersistentTmStores;
 #if OBSW_ADD_TCPIP_SERVERS == 1
 #if OBSW_ADD_TMTC_UDP_SERVER == 1
 // UDP server includes
-#include "devices/gpioIds.h"
-#include "fsfw/osal/common/UdpTcPollingTask.h"
-#include "fsfw/osal/common/UdpTmTcBridge.h"
+#include <fsfw/osal/common/UdpTcPollingTask.h>
+#include <fsfw/osal/common/UdpTmTcBridge.h>
 #endif
 #if OBSW_ADD_TMTC_TCP_SERVER == 1
 // TCP server includes
-#include "fsfw/osal/common/TcpTmTcBridge.h"
-#include "fsfw/osal/common/TcpTmTcServer.h"
-#include "mission/tmtc/Service15TmStorage.h"
+#include <fsfw/osal/common/TcpTmTcBridge.h>
+#include <fsfw/osal/common/TcpTmTcServer.h>
 #endif
 #endif

@ -234,16 +233,17 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun

  // PUS service stack
  new Service1TelecommandVerification(objects::PUS_SERVICE_1_VERIFICATION, config::EIVE_PUS_APID,
-                                      pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL, 40);
+                                      pus::PUS_SERVICE_1, objects::PUS_TM_FUNNEL,
+                                      config::VERIFICATION_SERVICE_QUEUE_DEPTH);
  new Service2DeviceAccess(objects::PUS_SERVICE_2_DEVICE_ACCESS, config::EIVE_PUS_APID,
                           pus::PUS_SERVICE_2, 3, 10);
  new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID,
-                           pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH);
+                           pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH, 16);
  new Service5EventReporting(
      PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5),
      80, 160);
  new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID,
-                                 pus::PUS_SERVICE_8, 16, 60);
+                                 pus::PUS_SERVICE_8, config::ACTION_SERVICE_QUEUE_DEPTH, 16, 60);
  new Service9TimeManagement(
      PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9));

--- a/mission/power/BpxBatteryHandler.cpp
+++ b/mission/power/BpxBatteryHandler.cpp
@ -1,4 +1,5 @@
 #include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/arrayprinter.h>
 #include <mission/power/BpxBatteryHandler.h>

 BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
@ -51,6 +52,9 @@ void BpxBatteryHandler::fillCommandAndReplyMap() {
  insertInCommandAndReplyMap(bpxBat::RESET_COUNTERS, 1, nullptr, EMPTY_REPLY_LEN);
  insertInCommandAndReplyMap(bpxBat::CONFIG_CMD, 1, nullptr, EMPTY_REPLY_LEN);
  insertInCommandAndReplyMap(bpxBat::CONFIG_GET, 1, &cfgSet, CONFIG_GET_REPLY_LEN);
+  insertInCommandAndReplyMap(bpxBat::CONFIG_SET, 1, nullptr, EMPTY_REPLY_LEN);
+  insertInCommandAndReplyMap(bpxBat::MAN_HEAT_ON, 1, nullptr, MAN_HEAT_REPLY_LEN);
+  insertInCommandAndReplyMap(bpxBat::MAN_HEAT_OFF, 1, nullptr, MAN_HEAT_REPLY_LEN);
 }

 ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
@ -155,7 +159,7 @@ ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remai
    case (bpxBat::PING):
    case (bpxBat::MAN_HEAT_ON):
    case (bpxBat::MAN_HEAT_OFF): {
-      if (remainingSize != PING_REPLY_LEN) {
+      if (remainingSize != MAN_HEAT_REPLY_LEN) {
        return DeviceHandlerIF::LENGTH_MISSMATCH;
      }
      break;
--- a/mission/power/GomspaceDeviceHandler.cpp
+++ b/mission/power/GomspaceDeviceHandler.cpp
@ -595,8 +595,8 @@ ReturnValue_t GomspaceDeviceHandler::parsePduHkTable(PDU::PduCoreHk& coreHk, PDU
  for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) {
    coreHk.voltages[idx] = as<uint16_t>(packet + 0x12 + (idx * 2));
  }
-  auxHk.vcc.value = as<int16_t>(packet + 0x24);
-  auxHk.vbat.value = as<int16_t>(packet + 0x26);
+  coreHk.vcc.value = as<int16_t>(packet + 0x24);
+  coreHk.vbat.value = as<int16_t>(packet + 0x26);
  coreHk.temperature = as<int16_t>(packet + 0x28) * 0.1;

  for (uint8_t idx = 0; idx < 3; idx++) {
--- a/mission/power/P60DockHandler.cpp
+++ b/mission/power/P60DockHandler.cpp
@ -66,7 +66,7 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) {
  }
  coreHk.battMode = newBattMode;

-  auxHk.heaterOn = *(packet + 0x57);
+  auxHk.heaterForBp4PackOn = *(packet + 0x57);
  auxHk.converter5VStatus = *(packet + 0x58);

  for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
@ -111,6 +111,8 @@ void P60DockHandler::parseHkTableReply(const uint8_t *packet) {
  }
  coreHk.setValidity(true, true);
  auxHk.setValidity(true, true);
+  // No BP4 pack, no this is always invalid.
+  auxHk.heaterForBp4PackOn.setValid(false);
 }

 ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
--- a/mission/power/bpxBattDefs.h
+++ b/mission/power/bpxBattDefs.h
@ -48,6 +48,7 @@ static constexpr uint32_t CFG_SET_ID = CONFIG_GET;
 static constexpr size_t GET_HK_REPLY_LEN = 23;
 static constexpr size_t PING_REPLY_LEN = 3;
 static constexpr size_t EMPTY_REPLY_LEN = 2;
+static constexpr size_t MAN_HEAT_REPLY_LEN = 3;
 static constexpr size_t CONFIG_GET_REPLY_LEN = 5;

 static constexpr uint8_t PORT_PING = 1;
@ -219,6 +220,7 @@ class BpxBatteryCfg : public StaticLocalDataSet<bpxBat::CFG_ENTRIES> {
    if (size < 3) {
      return SerializeIF::STREAM_TOO_SHORT;
    }
+
    battheatermode.value = data[0];
    battheaterLow.value = data[1];
    battheaterHigh.value = data[2];
--- a/mission/power/gsDefs.h
+++ b/mission/power/gsDefs.h
@ -260,7 +260,8 @@ class HkTableDataset : public StaticLocalDataSet<32> {
  lp_var_t<uint16_t> resetcause = lp_var_t<uint16_t>(sid.objectId, pool::P60DOCK_RESETCAUSE, this);

  /** Battery heater control only possible on BP4 packs */
-  lp_var_t<uint8_t> heaterOn = lp_var_t<uint8_t>(sid.objectId, pool::P60DOCK_HEATER_ON, this);
+  lp_var_t<uint8_t> heaterForBp4PackOn =
+      lp_var_t<uint8_t>(sid.objectId, pool::P60DOCK_HEATER_ON, this);
  lp_var_t<uint8_t> converter5VStatus =
      lp_var_t<uint8_t>(sid.objectId, pool::P60DOCK_CONV_5V_ENABLE_STATUS, this);

@ -404,6 +405,11 @@ class PduCoreHk : public StaticLocalDataSet<9> {
  /** Battery mode: 1 = Critical, 2 = Safe, 3 = Normal, 4 = Full */
  lp_var_t<uint8_t> battMode = lp_var_t<uint8_t>(sid.objectId, pool::PDU_BATT_MODE, this);
  lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, pool::PDU_TEMPERATURE, this);
+
+  /** Measured VCC */
+  lp_var_t<int16_t> vcc = lp_var_t<int16_t>(sid.objectId, pool::PDU_VCC, this);
+  /** Measured VBAT */
+  lp_var_t<int16_t> vbat = lp_var_t<int16_t>(sid.objectId, pool::PDU_VBAT, this);
 };

 class PduConfig : public StaticLocalDataSet<32> {
@ -451,11 +457,6 @@ class PduAuxHk : public StaticLocalDataSet<36> {

  PduAuxHk(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {}

-  /** Measured VCC */
-  lp_var_t<int16_t> vcc = lp_var_t<int16_t>(sid.objectId, pool::PDU_VCC, this);
-  /** Measured VBAT */
-  lp_var_t<int16_t> vbat = lp_var_t<int16_t>(sid.objectId, pool::PDU_VBAT, this);
-
  /** Output converter enable status */
  lp_vec_t<uint8_t, 3> converterEnable =
      lp_vec_t<uint8_t, 3>(sid.objectId, pool::PDU_CONV_EN, this);
--- a/mission/sysDefs.h
+++ b/mission/sysDefs.h
@ -44,6 +44,8 @@ static constexpr char VERSION_FILE_NAME[] = "version.txt";
 static constexpr char REBOOT_FILE_NAME[] = "reboot.txt";
 static constexpr char TIME_FILE_NAME[] = "time_backup.txt";

+static constexpr uint32_t SYS_ROM_BASE_ADDR = 0x80000000;
+
 static constexpr ActionId_t ANNOUNCE_VERSION = 1;
 static constexpr ActionId_t ANNOUNCE_CURRENT_IMAGE = 2;
 static constexpr ActionId_t ANNOUNCE_BOOT_COUNTS = 3;
@ -113,6 +115,10 @@ static constexpr Event TRYING_I2C_RECOVERY = event::makeEvent(SUBSYSTEM_ID, 10,
 static constexpr Event I2C_REBOOT = event::makeEvent(SUBSYSTEM_ID, 11, severity::HIGH);
 //! [EXPORT] : [COMMENT] PDEC recovery through reset was not possible, performing full reboot.
 static constexpr Event PDEC_REBOOT = event::makeEvent(SUBSYSTEM_ID, 12, severity::HIGH);
+//! [EXPORT] : [COMMENT] Version information of the firmware (not OBSW).
+//! 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 FIRMWARE_INFO = event::makeEvent(SUBSYSTEM_ID, 13, severity::INFO);

 class ListDirectoryCmdBase {
 public:  // TODO: Packet definition for clean deserialization
--- a/mission/tcs/Tmp1075Handler.cpp
+++ b/mission/tcs/Tmp1075Handler.cpp
@ -86,8 +86,11 @@ ReturnValue_t Tmp1075Handler::scanForReply(const uint8_t *start, size_t remainin
 ReturnValue_t Tmp1075Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
  switch (id) {
    case TMP1075::GET_TEMP: {
-      int16_t tempValueRaw = 0;
-      tempValueRaw = packet[0] << 4 | packet[1] >> 4;
+      // Convert 12 bit MSB first raw temperature to 16 bit first.
+      int16_t tempValueRaw = static_cast<uint16_t>((packet[0] << 8) | packet[1]) >> 4;
+      // Sign extension to 16 bits: If the sign bit is set, fill up with ones on the left.
+      tempValueRaw = (packet[0] & 0x80) ? (tempValueRaw | 0xF000) : tempValueRaw;
+      // 0.0625 is the sensor sensitivity.
      float tempValue = ((static_cast<float>(tempValueRaw)) * 0.0625);
 #if OBSW_DEBUG_TMP1075 == 1
      sif::info << "Tmp1075 with object id: 0x" << std::hex << getObjectId()
--- a/mission/tcs/defs.h
+++ b/mission/tcs/defs.h
@ -5,10 +5,10 @@

 namespace heater {
 enum Switch : uint8_t {
-  HEATER_0_OBC_BRD,
-  HEATER_1_PLOC_PROC_BRD,
+  HEATER_0_PLOC_PROC_BRD,
+  HEATER_1_PCDU_PDU,
  HEATER_2_ACS_BRD,
-  HEATER_3_PCDU_PDU,
+  HEATER_3_OBC_BRD,
  HEATER_4_CAMERA,
  HEATER_5_STR,
  HEATER_6_DRO,
--- a/mission/tmtc/PersistentTmStore.cpp
+++ b/mission/tmtc/PersistentTmStore.cpp
@ -268,6 +268,10 @@ ReturnValue_t PersistentTmStore::getNextDumpPacket(PusTmReader& reader, bool& fi
    // restore the file dump, but for now do not trust the file.
    std::error_code e;
    std::filesystem::remove(dumpParams.dirEntry.path().c_str(), e);
+    if (dumpParams.dirEntry.path() == activeFile) {
+      activeFile == std::nullopt;
+      assignAndOrCreateMostRecentFile();
+    }
    fileHasSwapped = true;
    return loadNextDumpFile();
  }
--- a/release_checklist.md
+++ b/release_checklist.md
@ -7,7 +7,9 @@ OBSW Release Checklist
 2. Re-run the generators with `generators/gen.py all`
 3. Re-run the auto-formatters with the `scripts/auto-formatter.sh` script
 4. Verify that the Q7S, Q7S EM and Host build are working
-5. Wait for CI/CD results
+5. Update `CHANGELOG.md`: Add new `unreleased` section, convert old unreleased section to
+   header containing version number and release date.
+6. Wait for CI/CD results

 # Post-Release

--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 6182369e4f40872c5c26e59be25d5fa79339176a
+Subproject commit 1bb8bea8d92fef2c9ec58ea657b04b56635c16dd