Merge pull request 'Prep v7.6.1' (#857) from prep-v7.6.1 into main

Reviewed-on: #857

CHANGELOG.md

@@ -16,6 +16,126 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+# [v7.6.1] 2024-02-05
+
+## Changed
+
+- Guidance now uses the coordinate functions from the FSFW.
+- Idle should now point the STR away from the earth
+
+## Fixed
+
+- Fixed bugs in `Guidance::comparePtg` and corrected overloading
+- Detumbling State Machine is now robust to commanded mode changes.
+
+# [v7.6.0] 2024-01-30
+
+- Bumped `eive-tmtc` to v5.13.0
+- Bumped `eive-fsfw`
+
+## Added
+
+- Added new parameter for MPSoC which allows to skip SUPV commanding.
+
+## Changed
+
+- Increased allowed mode transition time for PLOC SUPV.
+- Detumbling can now be triggered from all modes of the `AcsController`. In case the
+  current mode is a higher pointing mode, the STR will be set to faulty, to trigger a
+  transition to safe first. Then, in a second step, a transition to detumble is triggered.
+
+## Fixed
+
+- If the PCDU handler fails reading data from the IPC store, it will
+  not try to do a deserialization anymore.
+- All action commands sent by the PLOC SUPV to itself will have no sender now.
+- RW speed commands get reset to 0 RPM, if the `AcsController` has changed its mode
+  to Safe
+- Antistiction for RWs will set commanded speed to 0 RPM, if a wheel is detected as not
+  working
+- Removed parameter to disable antistiction, as deactivating it would result in the
+  `AcsController` being allowed sending invalid speed commands to the RW Handler, which
+  would then trigger FDIR and turning off the functioning device
+- `RwHandler` returnvalues would use the `INTERFACE_ID` of the `DeviceHandlerBase`
+- The `AcsController` will reset its stored guidance values on mode change and lost
+  orientation.
+- The nullspace controller will only be used if all RWs are available.
+- Calculation of required rotation rate in pointing modes has been fixed to actual
+  calculation of rotation rate from two quaternions.
+- Fixed alignment matrix and pseudo inverses of RWs, to match the wrong definition of
+  positive rotation.
+
+# [v7.5.5] 2024-01-22
+
+## Fixed
+
+- Calculation of error quaternion was done with inverse of the required target quaternion.
+
+# [v7.5.4] 2024-01-16
+
+## Fixed
+
+- Pointing strategy now actually uses fused rotation rate source instead of its valid flag.
+- All datasets now get updated during pointing mode, even if the strategy is a fault one.
+
+# [v7.5.3] 2023-12-19
+
+## Fixed
+
+- Set STR quaternions to invalid in device handler if the solution is not trustworthy.
+
+# [v7.5.2] 2023-12-14
+
+## Fixed
+
+- Fixed faulty scaling within the QUEST algorithm.
+
+# [v7.5.1] 2023-12-13
+
+- `eive-tmtc` v5.12.1
+
+## Changed
+
+- Increased the maximum number of scheduled telecommands from 500 to 4000. Merry Christmas!
+
+## Fixed
+
+- Faulty mapping of input values for QUEST algorithm.
+- Fixed validity check for QUEST algorithm.
+
+# [v7.5.0] 2023-12-06
+
+- `eive-tmtc` v5.12.0
+
+## Changed
+
+- ACS-Board default side changed to B-Side
+- The TLE uploaded now gets stored in a file on the filesystem. It will always be stored on
+  the current active SD Card. After a reboot, the TLE will be read from the filesystem.
+  A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even
+  though it is there.
+- Added action cmd to read the currently stored TLE.
+- Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate
+  the time difference.
+- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing
+  on to the relevant mode functions. It handles all telemetry relevant functions, which were
+  always called, regardless of the mode.
+ 
+## Added
+
+- Higher ACS modes can now be entered without a running `MEKF`. Higher modes will collect their
+  quaternion and rotational rate depending on the available sources.
+- `QUEST` attitude estimation was added to the `AcsController`.
+- The fused rotational rate can now be estimated from `QUEST` and the `STR`.
+
+# [v7.4.1] 2023-12-06
+
+## Fixed
+
+- Schedule SCEX again. Scheduling was removed accidentaly when Payload Task was converted to a PST.
+- SCEX transition was previously 0 seconds.. which did not lead to bugs? In any case it is 5
+  seconds now.
+
 # [v7.4.0] 2023-11-30
 
 - `eive-tmtc` v5.11.0

CMakeLists.txt

@@ -10,8 +10,8 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 7)
-set(OBSW_VERSION_MINOR 4)
-set(OBSW_VERSION_REVISION 0)
+set(OBSW_VERSION_MINOR 6)
+set(OBSW_VERSION_REVISION 1)
 
 # set(CMAKE_VERBOSE TRUE)
 
@@ -64,7 +64,7 @@ include(EiveHelpers)
 option(EIVE_ADD_ETL_LIB "Add ETL library" ON)
 option(EIVE_ADD_JSON_LIB "Add JSON library" ON)
 
-set(OBSW_MAX_SCHEDULED_TCS 500)
+set(OBSW_MAX_SCHEDULED_TCS 4000)
 
 if(EIVE_Q7S_EM)
   set(OBSW_Q7S_EM

bsp_hosted/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 317 translations.
+ * @brief    Auto-generated event translation file. Contains 319 translations.
  * @details
- * Generated on: 2023-11-29 15:14:46
+ * Generated on: 2024-01-30 09:10:05
  */
 #include "translateEvents.h"
 
@@ -94,14 +94,16 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
 const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
-const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
+const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
 const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
 const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY";
 const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
-const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
+const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
 const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
+const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
+const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -504,7 +506,7 @@ const char *translateEvents(Event event) {
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
-      return SAFE_RATE_RECOVERY_STRING;
+      return RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
     case (11203):
@@ -514,11 +516,15 @@ const char *translateEvents(Event event) {
     case (11205):
       return MEKF_AUTOMATIC_RESET_STRING;
     case (11206):
-      return MEKF_INVALID_MODE_VIOLATION_STRING;
+      return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
     case (11207):
       return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11208):
       return TLE_TOO_OLD_STRING;
+    case (11209):
+      return TLE_FILE_READ_FAILED_STRING;
+    case (11210):
+      return PTG_RATE_VIOLATION_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):

bsp_hosted/fsfwconfig/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 175 translations.
- * Generated on: 2023-11-29 15:14:46
+ * Generated on: 2024-01-30 09:10:05
  */
 #include "translateObjects.h"
 

bsp_q7s/em/emObjectFactory.cpp

@@ -175,7 +175,7 @@ void ObjectFactory::produce(void* args) {
   createScexComponents(q7s::UART_SCEX_DEV, pwrSwitcher, *SdCardManager::instance(), false,
                        power::Switches::PDU1_CH5_SOLAR_CELL_EXP_5V);
 #endif
-  createAcsController(true, enableHkSets);
+  createAcsController(true, enableHkSets, *SdCardManager::instance());
   HeaterHandler* heaterHandler;
   createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler);
   createThermalController(*heaterHandler, true);

bsp_q7s/fmObjectFactory.cpp

@@ -130,6 +130,6 @@ void ObjectFactory::produce(void* args) {
 
   createMiscComponents();
   createThermalController(*heaterHandler, false);
-  createAcsController(true, enableHkSets);
+  createAcsController(true, enableHkSets, *SdCardManager::instance());
   satsystem::init(false);
 }

bsp_q7s/objectFactory.cpp

@@ -39,9 +39,7 @@
 #include "bsp_q7s/acs/StrConfigPathGetter.h"
 #include "bsp_q7s/boardtest/Q7STestTask.h"
 #include "bsp_q7s/callbacks/gnssCallback.h"
-#include "bsp_q7s/callbacks/pcduSwitchCb.h"
 #include "bsp_q7s/callbacks/q7sGpioCallbacks.h"
-#include "bsp_q7s/callbacks/rwSpiCallback.h"
 #include "busConf.h"
 #include "ccsdsConfig.h"
 #include "devConf.h"
@@ -69,11 +67,11 @@
 #include "mission/system/acs/acsModeTree.h"
 #include "mission/system/com/SyrlinksFdir.h"
 #include "mission/system/com/comModeTree.h"
+#include "mission/system/payloadModeTree.h"
 #include "mission/system/power/GomspacePowerFdir.h"
 #include "mission/system/tcs/RtdFdir.h"
 #include "mission/system/tcs/TcsBoardAssembly.h"
 #include "mission/system/tcs/tcsModeTree.h"
-#include "mission/system/tree/payloadModeTree.h"
 #include "mission/tmtc/tmFilters.h"
 #include "mission/utility/GlobalConfigHandler.h"
 #include "tmtc/pusIds.h"

dummies/helperFactory.cpp

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

generators/bsp_hosted_events.csv

@@ -88,14 +88,16 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 10804;0x2a34;FILENAME_TOO_LARGE_ERROR;LOW;P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name;fsfw/src/fsfw/cfdp/handler/defs.h
 10805;0x2a35;HANDLING_CFDP_REQUEST_FAILED;LOW;CFDP request handling failed. P2: Returncode.;fsfw/src/fsfw/cfdp/handler/defs.h
 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;The limits for the rotation in safe mode were violated.;mission/acs/defs.h
-11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;The system has recovered from a safe rate rotation violation.;mission/acs/defs.h
+11201;0x2bc1;RATE_RECOVERY;MEDIUM;The system has recovered from a rate rotation violation.;mission/acs/defs.h
 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;Multiple RWs are invalid, uncommandable and therefore higher ACS modes cannot be maintained.;mission/acs/defs.h
 11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h
 11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h
 11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h
-11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h
+11206;0x2bc6;PTG_CTRL_NO_ATTITUDE_INFORMATION;HIGH;For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode.;mission/acs/defs.h
 11207;0x2bc7;SAFE_MODE_CONTROLLER_FAILURE;HIGH;The ACS safe mode controller was not able to compute a solution and has failed. P1: Missing information about magnetic field, P2: Missing information about rotational rate;mission/acs/defs.h
 11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h
+11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h
+11210;0x2bca;PTG_RATE_VIOLATION;MEDIUM;The limits for the rotation in pointing mode were violated.;mission/acs/defs.h
 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/power/defs.h
 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/power/defs.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h

generators/bsp_hosted_returnvalues.csv

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

generators/bsp_q7s_events.csv

@@ -88,14 +88,16 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 10804;0x2a34;FILENAME_TOO_LARGE_ERROR;LOW;P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name;fsfw/src/fsfw/cfdp/handler/defs.h
 10805;0x2a35;HANDLING_CFDP_REQUEST_FAILED;LOW;CFDP request handling failed. P2: Returncode.;fsfw/src/fsfw/cfdp/handler/defs.h
 11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;The limits for the rotation in safe mode were violated.;mission/acs/defs.h
-11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;The system has recovered from a safe rate rotation violation.;mission/acs/defs.h
+11201;0x2bc1;RATE_RECOVERY;MEDIUM;The system has recovered from a rate rotation violation.;mission/acs/defs.h
 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;Multiple RWs are invalid, uncommandable and therefore higher ACS modes cannot be maintained.;mission/acs/defs.h
 11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h
 11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h
 11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h
-11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h
+11206;0x2bc6;PTG_CTRL_NO_ATTITUDE_INFORMATION;HIGH;For a prolonged time, no attitude information was available for the Pointing Controller. Falling back to Safe Mode.;mission/acs/defs.h
 11207;0x2bc7;SAFE_MODE_CONTROLLER_FAILURE;HIGH;The ACS safe mode controller was not able to compute a solution and has failed. P1: Missing information about magnetic field, P2: Missing information about rotational rate;mission/acs/defs.h
 11208;0x2bc8;TLE_TOO_OLD;INFO;The TLE for the SGP4 Propagator has become too old.;mission/acs/defs.h
+11209;0x2bc9;TLE_FILE_READ_FAILED;LOW;The TLE could not be read from the filesystem.;mission/acs/defs.h
+11210;0x2bca;PTG_RATE_VIOLATION;MEDIUM;The limits for the rotation in pointing mode were violated.;mission/acs/defs.h
 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/power/defs.h
 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/power/defs.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/power/defs.h

generators/bsp_q7s_returnvalues.csv

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

generators/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 317 translations.
+ * @brief    Auto-generated event translation file. Contains 319 translations.
  * @details
- * Generated on: 2023-11-29 15:14:46
+ * Generated on: 2024-01-30 09:10:05
  */
 #include "translateEvents.h"
 
@@ -94,14 +94,16 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
 const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
-const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
+const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
 const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
 const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY";
 const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
-const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
+const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
 const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
+const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
+const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -504,7 +506,7 @@ const char *translateEvents(Event event) {
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
-      return SAFE_RATE_RECOVERY_STRING;
+      return RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
     case (11203):
@@ -514,11 +516,15 @@ const char *translateEvents(Event event) {
     case (11205):
       return MEKF_AUTOMATIC_RESET_STRING;
     case (11206):
-      return MEKF_INVALID_MODE_VIOLATION_STRING;
+      return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
     case (11207):
       return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11208):
       return TLE_TOO_OLD_STRING;
+    case (11209):
+      return TLE_FILE_READ_FAILED_STRING;
+    case (11210):
+      return PTG_RATE_VIOLATION_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):

generators/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 179 translations.
- * Generated on: 2023-11-29 15:14:46
+ * Generated on: 2024-01-30 09:10:05
  */
 #include "translateObjects.h"
 

linux/ObjectFactory.cpp

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

linux/ObjectFactory.h

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

linux/fsfwconfig/events/translateEvents.cpp

@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 317 translations.
+ * @brief    Auto-generated event translation file. Contains 319 translations.
  * @details
- * Generated on: 2023-11-29 15:14:46
+ * Generated on: 2024-01-30 09:10:05
  */
 #include "translateEvents.h"
 
@@ -94,14 +94,16 @@ const char *FILESTORE_ERROR_STRING = "FILESTORE_ERROR";
 const char *FILENAME_TOO_LARGE_ERROR_STRING = "FILENAME_TOO_LARGE_ERROR";
 const char *HANDLING_CFDP_REQUEST_FAILED_STRING = "HANDLING_CFDP_REQUEST_FAILED";
 const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
-const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
+const char *RATE_RECOVERY_STRING = "RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
 const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
 const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY";
 const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET";
-const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
+const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFORMATION";
 const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *TLE_TOO_OLD_STRING = "TLE_TOO_OLD";
+const char *TLE_FILE_READ_FAILED_STRING = "TLE_FILE_READ_FAILED";
+const char *PTG_RATE_VIOLATION_STRING = "PTG_RATE_VIOLATION";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -504,7 +506,7 @@ const char *translateEvents(Event event) {
     case (11200):
       return SAFE_RATE_VIOLATION_STRING;
     case (11201):
-      return SAFE_RATE_RECOVERY_STRING;
+      return RATE_RECOVERY_STRING;
     case (11202):
       return MULTIPLE_RW_INVALID_STRING;
     case (11203):
@@ -514,11 +516,15 @@ const char *translateEvents(Event event) {
     case (11205):
       return MEKF_AUTOMATIC_RESET_STRING;
     case (11206):
-      return MEKF_INVALID_MODE_VIOLATION_STRING;
+      return PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING;
     case (11207):
       return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11208):
       return TLE_TOO_OLD_STRING;
+    case (11209):
+      return TLE_FILE_READ_FAILED_STRING;
+    case (11210):
+      return PTG_RATE_VIOLATION_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):

linux/fsfwconfig/objects/translateObjects.cpp

@@ -2,7 +2,7 @@
  * @brief  Auto-generated object translation file.
  * @details
  * Contains 179 translations.
- * Generated on: 2023-11-29 15:14:46
+ * Generated on: 2024-01-30 09:10:05
  */
 #include "translateObjects.h"
 

linux/payload/FreshSupvHandler.cpp

@@ -1115,7 +1115,8 @@ void FreshSupvHandler::handleEvent(EventMessage* eventMessage) {
         if (not isCommandAlreadyActive(supv::SHUTDOWN_MPSOC)) {
           CommandMessage actionMsg;
           ActionMessage::setCommand(&actionMsg, supv::SHUTDOWN_MPSOC, store_address_t::invalid());
-          result = messageQueue->sendMessage(getCommandQueue(), &actionMsg);
+          result = messageQueue->sendMessageFrom(getCommandQueue(), &actionMsg,
+                                                 MessageQueueIF::NO_QUEUE);
           if (result != returnvalue::OK) {
             triggerEvent(supv::SUPV_MPSOC_SHUTDOWN_BUILD_FAILED);
             sif::warning << "PlocSupervisorHandler::handleEvent: Failed to build MPSoC shutdown "

linux/payload/PlocMpsocHandler.cpp

@@ -7,6 +7,7 @@
 #include "OBSWConfig.h"
 #include "fsfw/datapool/PoolReadGuard.h"
 #include "fsfw/globalfunctions/CRC.h"
+#include "fsfw/parameters/HasParametersIF.h"
 
 PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid,
                                    CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper,
@@ -1395,14 +1396,18 @@ bool PlocMpsocHandler::handleHwStartup() {
   return true;
 #endif
   if (powerState == PowerState::IDLE) {
-    if (supv::SUPV_ON) {
-      commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
-      supvTransitionCd.resetTimer();
-      powerState = PowerState::PENDING_STARTUP;
+    if (skipSupvCommandingToOn) {
+      powerState = PowerState::DONE;
     } else {
-      triggerEvent(SUPV_NOT_ON, 1);
-      // Set back to OFF for now, failing the transition.
-      setMode(MODE_OFF);
+      if (supv::SUPV_ON) {
+        commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
+        supvTransitionCd.resetTimer();
+        powerState = PowerState::PENDING_STARTUP;
+      } else {
+        triggerEvent(SUPV_NOT_ON, 1);
+        // Set back to OFF for now, failing the transition.
+        setMode(MODE_OFF);
+      }
     }
   }
   if (powerState == PowerState::SUPV_FAILED) {
@@ -1532,3 +1537,20 @@ ReturnValue_t PlocMpsocHandler::checkModeCommand(Mode_t commandedMode, Submode_t
   }
   return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
 }
+
+ReturnValue_t PlocMpsocHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
+                                             ParameterWrapper* parameterWrapper,
+                                             const ParameterWrapper* newValues,
+                                             uint16_t startAtIndex) {
+  if (uniqueId == mpsoc::ParamId::SKIP_SUPV_ON_COMMANDING) {
+    uint8_t value = 0;
+    newValues->getElement(&value);
+    if (value > 1) {
+      return HasParametersIF::INVALID_VALUE;
+    }
+    parameterWrapper->set(skipSupvCommandingToOn);
+    return returnvalue::OK;
+  }
+  return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
+                                         startAtIndex);
+}

linux/payload/PlocMpsocHandler.h

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

linux/payload/PlocSupvUartMan.cpp

@@ -945,15 +945,7 @@ ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() {
       break;
     }
     case Request::REQUEST_EVENT_BUFFER: {
-      //      result = performEventBufferRequest();
-      //      if (result == returnvalue::OK) {
-      //        triggerEvent(SUPV_EVENT_BUFFER_REQUEST_SUCCESSFUL, result);
-      //      } else if (result == PROCESS_TERMINATED) {
-      //        // Event already triggered
-      //        break;
-      //      } else {
-      //        triggerEvent(SUPV_EVENT_BUFFER_REQUEST_FAILED, result);
-      //      }
+      sif::error << "Requesting event buffer is not implemented" << std::endl;
       break;
     }
     case Request::DEFAULT: {

linux/payload/plocMpsocHelpers.h

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

linux/payload/plocSupvDefs.h

@@ -49,7 +49,7 @@ static const Event SUPV_EXE_ACK_UNKNOWN_COMMAND = MAKE_EVENT(10, severity::LOW);
 extern std::atomic_bool SUPV_ON;
 static constexpr uint32_t INTER_COMMAND_DELAY = 20;
 static constexpr uint32_t BOOT_TIMEOUT_MS = 4000;
-static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 2000;
+static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 3000;
 static constexpr uint32_t MAX_TRANSITION_TIME_TO_OFF_MS = 1000;
 
 namespace result {

linux/scheduling.cpp

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

linux/scheduling.h

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

mission/acs/RwHandler.h

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

mission/acs/defs.h

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

mission/acs/str/StarTrackerHandler.cpp

@@ -1162,7 +1162,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       break;
     }
     case (startracker::REQ_SOLUTION): {
-      result = handleTm(packet, solutionSet, "REQ_SOLUTION");
+      result = handleSolution(packet);
       break;
     }
     case (startracker::REQ_CONTRAST): {
@@ -2438,6 +2438,36 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
   return result;
 }
 
+ReturnValue_t StarTrackerHandler::handleSolution(const uint8_t* rawFrame) {
+  ReturnValue_t result = statusFieldCheck(rawFrame);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  PoolReadGuard pg(&solutionSet);
+  if (pg.getReadResult() != returnvalue::OK) {
+    return result;
+  }
+  const uint8_t* reply = rawFrame + TICKS_OFFSET;
+  solutionSet.setValidityBufferGeneration(false);
+  size_t sizeLeft = fullPacketLen;
+  result = solutionSet.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for solution set: 0x"
+                 << std::hex << std::setw(4) << result << std::dec << std::endl;
+  }
+  solutionSet.setValidityBufferGeneration(true);
+  solutionSet.setValidity(true, true);
+  solutionSet.caliQw.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQx.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQy.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQz.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQw.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQx.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQy.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQz.setValid(solutionSet.isTrustWorthy.value);
+  return result;
+}
+
 ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) {
   ReturnValue_t result = statusFieldCheck(rawFrame);
   if (result != returnvalue::OK) {

mission/acs/str/StarTrackerHandler.h

@@ -527,6 +527,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
                          const char* context);
 
+  ReturnValue_t handleSolution(const uint8_t* rawFrame);
   ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame);
   ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
   ReturnValue_t handleBlobTm(const uint8_t* rawFrame);

mission/acs/str/strHelpers.h

@@ -7,7 +7,7 @@
 #include <fsfw/devicehandlers/DeviceHandlerIF.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
 
-#include "objects/systemObjectList.h"
+#include <optional>
 
 namespace startracker {
 

mission/controller/AcsController.cpp

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

mission/controller/AcsController.h

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

mission/controller/PowerController.cpp

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

mission/controller/PowerController.h

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

mission/controller/acs/AcsParameters.cpp

@@ -24,11 +24,20 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(onBoardParams.sampleTime);
           break;
         case 0x1:
-          parameterWrapper->set(onBoardParams.mekfViolationTimer);
+          parameterWrapper->set(onBoardParams.ptgCtrlLostTimer);
           break;
         case 0x2:
           parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse);
           break;
+        case 0x3:
+          parameterWrapper->set(onBoardParams.fusedRateFromStr);
+          break;
+        case 0x4:
+          parameterWrapper->set(onBoardParams.fusedRateFromQuest);
+          break;
+        case 0x5:
+          parameterWrapper->set(onBoardParams.questFilterWeight);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -324,16 +333,16 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->setMatrix(rwMatrices.pseudoInverse);
           break;
         case 0x2:
-          parameterWrapper->setMatrix(rwMatrices.without1);
+          parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW1);
           break;
         case 0x3:
-          parameterWrapper->setMatrix(rwMatrices.without2);
+          parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW2);
           break;
         case 0x4:
-          parameterWrapper->setMatrix(rwMatrices.without3);
+          parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW3);
           break;
         case 0x5:
-          parameterWrapper->setMatrix(rwMatrices.without4);
+          parameterWrapper->setMatrix(rwMatrices.pseudoInverseWithoutRW4);
           break;
         case 0x6:
           parameterWrapper->setVector(rwMatrices.nullspaceVector);
@@ -423,7 +432,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(idleModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(idleModeControllerParameters.useMekf);
           break;
         default:
           return INVALID_IDENTIFIER_ID;
@@ -459,7 +468,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(targetModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(targetModeControllerParameters.useMekf);
           break;
         case 0xA:
           parameterWrapper->setVector(targetModeControllerParameters.refDirection);
@@ -528,7 +537,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
           break;
         case 0xA:
           parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
@@ -579,7 +588,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(nadirModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(nadirModeControllerParameters.useMekf);
           break;
         case 0xA:
           parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
@@ -627,7 +636,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(inertialModeControllerParameters.desatOn);
           break;
         case 0x9:
-          parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
+          parameterWrapper->set(inertialModeControllerParameters.useMekf);
           break;
         case 0xA:
           parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);

mission/controller/acs/AcsParameters.h

@@ -18,8 +18,11 @@ class AcsParameters : public HasParametersIF {
 
   struct OnBoardParams {
     double sampleTime = 0.4;  // [s]
-    uint16_t mekfViolationTimer = 750;
+    uint16_t ptgCtrlLostTimer = 750;
     uint8_t fusedRateSafeDuringEclipse = true;
+    uint8_t fusedRateFromStr = false;
+    uint8_t fusedRateFromQuest = false;
+    double questFilterWeight = 0.0;
   } onBoardParams;
 
   struct InertiaEIVE {
@@ -75,9 +78,9 @@ class AcsParameters : public HasParametersIF {
                                        {-0.007534, 1.253879, 0.006812},
                                        {-0.037072, 0.006812, 1.313158}};
 
-    float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
-    float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
-    float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
+    double mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
+    double mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
+    double mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
     float mgmVectorFilterWeight = 0.85;
     float mgmDerivativeFilterWeight = 0.99;
     uint8_t useMgm4 = false;
@@ -787,10 +790,10 @@ class AcsParameters : public HasParametersIF {
 
     /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
      * assumed to be equal for the same class of sensors */
-    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)};
+    double gyr02variance[3] = {pow(4.6e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
+                               pow(4.6e-3, 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
+                               pow(6.1e-3, 2)};  // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
+    double gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
     uint8_t preferAdis = false;
     float gyrFilterWeight = 0.6;
   } gyrHandlingParameters;
@@ -809,19 +812,19 @@ class AcsParameters : public HasParametersIF {
   } rwHandlingParameters;
 
   struct RwMatrices {
-    double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
-                                    {0.0000, -0.9205, 0.0000, 0.9205},
-                                    {0.3907, 0.3907, 0.3907, 0.3907}};
+    double alignmentMatrix[3][4] = {{-0.9205, 0.0000, 0.9205, 0.0000},
+                                    {0.0000, 0.9205, 0.0000, -0.9205},
+                                    {-0.3907, -0.3907, -0.3907, -0.3907}};
     double pseudoInverse[4][3] = {
-        {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
-    double without1[4][3] = {
-        {0, 0, 0}, {0.5432, -0.5432, 1.2797}, {-1.0864, 0, 0}, {0.5432, 0.5432, 1.2797}};
-    double without2[4][3] = {
-        {0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 1.0864, 0}};
-    double without3[4][3] = {
-        {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
-    double without4[4][3] = {
-        {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
+        {-0.5432, 0, -0.6399}, {0, 0.5432, -0.6399}, {0.5432, 0, -0.6399}, {0, -0.5432, -0.6399}};
+    double pseudoInverseWithoutRW1[4][3] = {
+        {0, 0, 0}, {-0.5432, 0.5432, -1.2798}, {1.0864, 0, 0}, {-0.5432, -0.5432, -1.2798}};
+    double pseudoInverseWithoutRW2[4][3] = {
+        {-0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, -1.0864, 0}};
+    double pseudoInverseWithoutRW3[4][3] = {
+        {-1.0864, 0, 0}, {0.5432, 0.5432, -1.2798}, {0, 0, 0}, {0.5432, -0.5432, -1.2798}};
+    double pseudoInverseWithoutRW4[4][3] = {
+        {-0.5432, -0.5432, -1.2798}, {0, 1.0864, 0}, {0.5432, -0.5432, -1.2798}, {0, 0, 0}};
     double nullspaceVector[4] = {-1, 1, -1, 1};
   } rwMatrices;
 
@@ -860,7 +863,7 @@ class AcsParameters : public HasParametersIF {
     double desatMomentumRef[3] = {0, 0, 0};
     double deSatGainFactor = 1000;
     uint8_t desatOn = true;
-    uint8_t enableAntiStiction = true;
+    uint8_t useMekf = false;
   } pointingLawParameters;
 
   struct IdleModeControllerParameters : PointingLawParameters {

mission/controller/acs/AttitudeEstimation.cpp

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

mission/controller/acs/AttitudeEstimation.h

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

mission/controller/acs/CMakeLists.txt

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

mission/controller/acs/FusedRotationEstimation.cpp

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

mission/controller/acs/FusedRotationEstimation.h

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

mission/controller/acs/Guidance.cpp

@@ -1,422 +1,203 @@
 #include "Guidance.h"
 
-#include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/math/MatrixOperations.h>
-#include <fsfw/globalfunctions/math/QuaternionOperations.h>
-#include <fsfw/globalfunctions/math/VectorOperations.h>
-#include <math.h>
-
-#include <filesystem>
-
-#include "string.h"
-#include "util/CholeskyDecomposition.h"
-#include "util/MathOperations.h"
-
 Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
 
 Guidance::~Guidance() {}
 
-void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3],
-                                       double sunDirI[3], double refDirB[3], double quatBI[4],
-                                       double targetQuat[4], double targetSatRotRate[3]) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion to groundstation or given latitude, longitude and altitude
-  //-------------------------------------------------------------------------------------
-  // transform longitude, latitude and altitude to ECEF
-  double targetE[3] = {0, 0, 0};
+void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
+                                 const double sunDirI[3], const double posSatF[4],
+                                 double targetQuat[4], double targetSatRotRate[3]) {
+  // positive z-Axis of EIVE in direction of sun
+  double zAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(sunDirI, zAxisXI, 3);
 
-  MathOperations<double>::cartesianFromLatLongAlt(
-      acsParameters->targetModeControllerParameters.latitudeTgt,
-      acsParameters->targetModeControllerParameters.longitudeTgt,
-      acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
+  // determine helper vector to point x-Axis and therefore the STR away from Earth
+  double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
+  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
+  VectorOperations<double>::normalize(posSatI, helperXI, 3);
 
-  // target direction in the ECEF frame
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
+  // construct y-axis from helper vector and z-axis
+  double yAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxisXI, helperXI, yAxisXI);
+  VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
 
-  // transformation between ECEF and ECI frame
-  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
+  // x-axis completes RHS
+  double xAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::cross(yAxisXI, zAxisXI, xAxisXI);
+  VectorOperations<double>::normalize(xAxisXI, xAxisXI, 3);
 
-  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
+  // join transformation matrix
+  double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
+                        {xAxisXI[1], yAxisXI[1], zAxisXI[1]},
+                        {xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
+  QuaternionOperations::fromDcm(dcmXI, targetQuat);
 
-  // transformation between ECEF and Body frame
-  double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-
-  QuaternionOperations::toDcm(quatBI, dcmBI);
-  MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
-
-  // target Direction in the body frame
-  double targetDirB[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
-
-  // rotation quaternion from two vectors
-  double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters->targetModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters->targetModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters->targetModeControllerParameters.refDirection[2];
-  double noramlizedTargetDirB[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
-  VectorOperations<double>::normalize(refDir, refDir, 3);
-  double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
-  double normRefDir = VectorOperations<double>::norm(refDir, 3);
-  double crossDir[3] = {0, 0, 0};
-  double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
-  VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
-  targetQuat[0] = crossDir[0];
-  targetQuat[1] = crossDir[1];
-  targetQuat[2] = crossDir[2];
-  targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
-  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
-
-  //-------------------------------------------------------------------------------------
-  // calculation of reference rotation rate
-  //-------------------------------------------------------------------------------------
-  double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
-  // velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
-  MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
-  double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MatrixOperations<double>::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3);
-  MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
-  VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
-
-  double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
-  double normRefSatRate = normVelSatB / normTargetDirB;
-
-  double satRateDir[3] = {0, 0, 0};
-  VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
-  VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
-  VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3);
-
-  //-------------------------------------------------------------------------------------
-  //	Calculation of reference rotation rate in case of star tracker blinding
-  //-------------------------------------------------------------------------------------
-  if (acsParameters->targetModeControllerParameters.avoidBlindStr) {
-    double sunDirB[3] = {0, 0, 0};
-    MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
-
-    double exclAngle = acsParameters->strParameters.exclusionAngle,
-           blindStart = acsParameters->targetModeControllerParameters.blindAvoidStart,
-           blindEnd = acsParameters->targetModeControllerParameters.blindAvoidStop;
-    double sightAngleSun =
-        VectorOperations<double>::dot(acsParameters->strParameters.boresightAxis, sunDirB);
-
-    if (!(strBlindAvoidFlag)) {
-      double critSightAngle = blindStart * exclAngle;
-      if (sightAngleSun < critSightAngle) {
-        strBlindAvoidFlag = true;
-      }
-    } else {
-      if (sightAngleSun < blindEnd * exclAngle) {
-        double normBlindRefRate = acsParameters->targetModeControllerParameters.blindRotRate;
-        double blindRefRate[3] = {0, 0, 0};
-        if (sunDirB[1] < 0) {
-          blindRefRate[0] = normBlindRefRate;
-          blindRefRate[1] = 0;
-          blindRefRate[2] = 0;
-        } else {
-          blindRefRate[0] = -normBlindRefRate;
-          blindRefRate[1] = 0;
-          blindRefRate[2] = 0;
-        }
-        VectorOperations<double>::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3);
-      } else {
-        strBlindAvoidFlag = false;
-      }
-    }
-  }
-  // revert calculated quaternion from qBX to qIX
-  double quatIB[4] = {0, 0, 0, 1};
-  QuaternionOperations::inverse(quatBI, quatIB);
-  QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
+  // calculate of reference rotation rate
+  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
-                                      double targetQuat[4], double targetSatRotRate[3]) {
+void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
+                                   double velSatF[3], double targetQuat[4],
+                                   double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for target pointing
   //-------------------------------------------------------------------------------------
   // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
-  double targetE[3] = {0, 0, 0};
+  double targetF[3] = {0, 0, 0};
   MathOperations<double>::cartesianFromLatLongAlt(
       acsParameters->targetModeControllerParameters.latitudeTgt,
       acsParameters->targetModeControllerParameters.longitudeTgt,
-      acsParameters->targetModeControllerParameters.altitudeTgt, targetE);
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
-
-  // transformation between ECEF and ECI frame
-  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
-
-  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
+      acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
+  double targetDirF[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(targetF, posSatF, targetDirF, 3);
 
   // target direction in the ECI frame
   double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
+  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
+  CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute);
   VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
 
   // x-axis aligned with target direction
   // this aligns with the camera, E- and S-band antennas
-  double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirI, xAxis, 3);
+  double xAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(targetDirI, xAxisXI, 3);
 
   // transform velocity into inertial frame
-  double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
-  VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
+  double velSatI[3] = {0, 0, 0};
+  CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute);
 
   // orbital normal vector of target and velocity vector
   double orbitalNormalI[3] = {0, 0, 0};
-  VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
+  VectorOperations<double>::cross(posSatI, velSatI, orbitalNormalI);
   VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
 
   // y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
   // resolution
-  double yAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
-  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+  double yAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::cross(orbitalNormalI, xAxisXI, yAxisXI);
+  VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
 
   // z-axis completes RHS
-  double zAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(xAxis, yAxis, zAxis);
+  double zAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::cross(xAxisXI, yAxisXI, zAxisXI);
 
   // join transformation matrix
-  double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
-                        {xAxis[1], yAxis[1], zAxis[1]},
-                        {xAxis[2], yAxis[2], zAxis[2]}};
+  double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
+                        {xAxisXI[1], yAxisXI[1], zAxisXI[1]},
+                        {xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
   QuaternionOperations::fromDcm(dcmIX, targetQuat);
 
-  int8_t timeElapsedMax = acsParameters->targetModeControllerParameters.timeElapsedMax;
-  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
+  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3],
-                               double targetQuat[4], double targetSatRotRate[3]) {
+void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
+                               double sunDirI[3], double targetQuat[4],
+                               double targetSatRotRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for ground station pointing
   //-------------------------------------------------------------------------------------
   // transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
-  double groundStationE[3] = {0, 0, 0};
-
+  double posGroundStationF[3] = {0, 0, 0};
   MathOperations<double>::cartesianFromLatLongAlt(
       acsParameters->gsTargetModeControllerParameters.latitudeTgt,
       acsParameters->gsTargetModeControllerParameters.longitudeTgt,
-      acsParameters->gsTargetModeControllerParameters.altitudeTgt, groundStationE);
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
-
-  // transformation between ECEF and ECI frame
-  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
-
-  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
+      acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
 
   // target direction in the ECI frame
-  double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
-  VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
+  double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
+  CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
+  CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute);
+  VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
 
   // negative x-axis aligned with target direction
   // this aligns with the camera, E- and S-band antennas
-  double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
-  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+  double xAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(groundStationDirI, xAxisXI, 3);
+  VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
 
   // get sun vector model in ECI
   VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
 
   // calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
   // z = sPerpenticular =  s - sParallel = s - (x*s)/norm(x)^2 * x
-  double xDotS = VectorOperations<double>::dot(xAxis, sunDirI);
-  xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
-  double sunParallel[3], zAxis[3];
-  VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
-  VectorOperations<double>::subtract(sunDirI, sunParallel, zAxis, 3);
-  VectorOperations<double>::normalize(zAxis, zAxis, 3);
+  double xDotS = VectorOperations<double>::dot(xAxisXI, sunDirI);
+  xDotS /= pow(VectorOperations<double>::norm(xAxisXI, 3), 2);
+  double sunParallel[3], zAxisXI[3];
+  VectorOperations<double>::mulScalar(xAxisXI, xDotS, sunParallel, 3);
+  VectorOperations<double>::subtract(sunDirI, sunParallel, zAxisXI, 3);
+  VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
 
   // y-axis completes RHS
-  double yAxis[3];
-  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
-  VectorOperations<double>::normalize(yAxis, yAxis, 3);
+  double yAxisXI[3];
+  VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
+  VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
 
   // join transformation matrix
-  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
-                         {xAxis[1], yAxis[1], zAxis[1]},
-                         {xAxis[2], yAxis[2], zAxis[2]}};
-  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
+  double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
+                        {xAxisXI[1], yAxisXI[1], zAxisXI[1]},
+                        {xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
+  QuaternionOperations::fromDcm(dcmXI, targetQuat);
 
-  int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
-  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
+  targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
 }
 
-void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
-                                double targetSatRotRate[3]) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion to sun
-  //-------------------------------------------------------------------------------------
-  //  positive z-Axis of EIVE in direction of sun
-  double zAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(sunDirI, zAxis, 3);
-
-  //  assign helper vector (north pole inertial)
-  double helperVec[3] = {0, 0, 1};
-
-  //  construct y-axis from helper vector and z-axis
-  double yAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxis, helperVec, yAxis);
-  VectorOperations<double>::normalize(yAxis, yAxis, 3);
-
-  //  x-axis completes RHS
-  double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(yAxis, zAxis, xAxis);
-  VectorOperations<double>::normalize(xAxis, xAxis, 3);
-
-  // join transformation matrix
-  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
-                         {xAxis[1], yAxis[1], zAxis[1]},
-                         {xAxis[2], yAxis[2], zAxis[2]}};
-  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
-
-  //----------------------------------------------------------------------------
-  //	Calculation of reference rotation rate
-  //----------------------------------------------------------------------------
-  int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
-  targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
-}
-
-void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
-                                            double targetQuat[4], double refDirB[3],
-                                            double refSatRate[3]) {
+void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3],
+                                  double velSatE[3], double targetQuat[4], double refSatRate[3]) {
   //-------------------------------------------------------------------------------------
   //	Calculation of target quaternion for Nadir pointing
   //-------------------------------------------------------------------------------------
-  double targetDirE[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
-
-  // transformation between ECEF and ECI frame
-  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
-
-  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
-
-  // transformation between ECEF and Body frame
-  double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  QuaternionOperations::toDcm(quatBI, dcmBI);
-  MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
-
-  // target Direction in the body frame
-  double targetDirB[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
-
-  // rotation quaternion from two vectors
-  double refDir[3] = {0, 0, 0};
-  refDir[0] = acsParameters->nadirModeControllerParameters.refDirection[0];
-  refDir[1] = acsParameters->nadirModeControllerParameters.refDirection[1];
-  refDir[2] = acsParameters->nadirModeControllerParameters.refDirection[2];
-  double noramlizedTargetDirB[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
-  VectorOperations<double>::normalize(refDir, refDir, 3);
-  double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
-  double normRefDir = VectorOperations<double>::norm(refDir, 3);
-  double crossDir[3] = {0, 0, 0};
-  double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
-  VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
-  targetQuat[0] = crossDir[0];
-  targetQuat[1] = crossDir[1];
-  targetQuat[2] = crossDir[2];
-  targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
-  VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
-
-  //-------------------------------------------------------------------------------------
-  //	Calculation of reference rotation rate
-  //-------------------------------------------------------------------------------------
-  refSatRate[0] = 0;
-  refSatRate[1] = 0;
-  refSatRate[2] = 0;
-
-  // revert calculated quaternion from qBX to qIX
-  double quatIB[4] = {0, 0, 0, 1};
-  QuaternionOperations::inverse(quatBI, quatIB);
-  QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
-}
-
-void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
-                                           double targetQuat[4], double refSatRate[3]) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target quaternion for Nadir pointing
-  //-------------------------------------------------------------------------------------
-  // transformation between ECEF and ECI frame
-  double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
-  MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
-
-  double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
 
   // satellite position in inertial reference frame
   double posSatI[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
+  CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute);
 
   // negative x-axis aligned with position vector
   // this aligns with the camera, E- and S-band antennas
-  double xAxis[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(posSatI, xAxis, 3);
-  VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
+  double xAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(posSatI, xAxisXI, 3);
+  VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
 
   // make z-Axis parallel to major part of camera resolution
-  double zAxis[3] = {0, 0, 0};
-  double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
-  MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
-  MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
-  VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
-  VectorOperations<double>::cross(xAxis, velocityI, zAxis);
-  VectorOperations<double>::normalize(zAxis, zAxis, 3);
+  double zAxisXI[3] = {0, 0, 0};
+  double velSatI[3] = {0, 0, 0};
+  CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
+  VectorOperations<double>::cross(xAxisXI, velSatI, zAxisXI);
+  VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
 
   // y-Axis completes RHS
-  double yAxis[3] = {0, 0, 0};
-  VectorOperations<double>::cross(zAxis, xAxis, yAxis);
+  double yAxisXI[3] = {0, 0, 0};
+  VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
 
   // join transformation matrix
-  double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
-                         {xAxis[1], yAxis[1], zAxis[1]},
-                         {xAxis[2], yAxis[2], zAxis[2]}};
-  QuaternionOperations::fromDcm(dcmTgt, targetQuat);
+  double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
+                        {xAxisXI[1], yAxisXI[1], zAxisXI[1]},
+                        {xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
+  QuaternionOperations::fromDcm(dcmXI, targetQuat);
 
-  int8_t timeElapsedMax = acsParameters->nadirModeControllerParameters.timeElapsedMax;
-  targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
+  targetRotationRate(timeDelta, targetQuat, refSatRate);
+}
+
+void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) {
+  if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
+    std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
+  }
+  if (timeDelta != 0.0) {
+    QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
+  } else {
+    std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
+  }
+  std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
 }
 
 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                           double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                           double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
-  // First calculate error quaternion between current and target orientation
-  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
-  // Last calculate add rotation from reference quaternion
-  QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
+  // First calculate error quaternion between current and target orientation without reference
+  // quaternion
+  double errorQuatWoRef[4] = {0, 0, 0, 0};
+  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
+  // Then add rotation from reference quaternion
+  QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
   // Keep scalar part of quaternion positive
   if (errorQuat[3] < 0) {
     VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
@@ -425,7 +206,11 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
   errorAngle = QuaternionOperations::getAngle(errorQuat, true);
 
   // Calculate error satellite rotational rate
-  // First combine the target and reference satellite rotational rates
+  // Convert target rotational rate into body RF
+  double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0};
+  QuaternionOperations::inverse(errorQuat, errorQuatInv);
+  QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB);
+  // Combine the target and reference satellite rotational rates
   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
@@ -435,93 +220,43 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
 void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                           double targetSatRotRate[3], double errorQuat[4],
                           double errorSatRotRate[3], double &errorAngle) {
-  // First calculate error quaternion between current and target orientation
-  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
-  // Keep scalar part of quaternion positive
-  if (errorQuat[3] < 0) {
-    VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
-  }
-  // Calculate error angle
-  errorAngle = QuaternionOperations::getAngle(errorQuat, true);
-
-  // Calculate error satellite rotational rate
-  VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
-}
-
-void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
-                                  double *refSatRate) {
-  //-------------------------------------------------------------------------------------
-  //	Calculation of target rotation rate
-  //-------------------------------------------------------------------------------------
-  double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
-                       (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
-  if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
-    std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
-  }
-  if (timeElapsed < timeElapsedMax) {
-    double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
-    QuaternionOperations::inverse(quatInertialTarget, q);
-    QuaternionOperations::inverse(savedQuaternion, qS);
-    double qDiff[4] = {0, 0, 0, 0};
-    VectorOperations<double>::subtract(q, qS, qDiff, 4);
-    VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
-
-    double tgtQuatVec[3] = {q[0], q[1], q[2]};
-    double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
-    double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
-    VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
-    VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
-    VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
-    VectorOperations<double>::add(sum1, sum2, sum, 3);
-    VectorOperations<double>::subtract(sum, sum3, sum, 3);
-    double omegaRefNew[3] = {0, 0, 0};
-    VectorOperations<double>::mulScalar(sum, -2, omegaRefNew, 3);
-
-    VectorOperations<double>::mulScalar(omegaRefNew, 2, refSatRate, 3);
-    VectorOperations<double>::subtract(refSatRate, omegaRefSaved, refSatRate, 3);
-    omegaRefSaved[0] = omegaRefNew[0];
-    omegaRefSaved[1] = omegaRefNew[1];
-    omegaRefSaved[2] = omegaRefNew[2];
-  } else {
-    refSatRate[0] = 0;
-    refSatRate[1] = 0;
-    refSatRate[2] = 0;
-  }
-
-  timeSavedQuaternion = now;
-  savedQuaternion[0] = quatInertialTarget[0];
-  savedQuaternion[1] = quatInertialTarget[1];
-  savedQuaternion[2] = quatInertialTarget[2];
-  savedQuaternion[3] = quatInertialTarget[3];
+  double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0};
+  comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate,
+             errorQuat, errorSatRotRate, errorAngle);
 }
 
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
                                                 double *rwPseudoInv) {
-  bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
-  bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
-  bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
-  bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
+  bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
+  bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
+  bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
+  bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
 
-  if (rw1valid && rw2valid && rw3valid && rw4valid) {
+  if (rw1valid and rw2valid and rw3valid and rw4valid) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
     return returnvalue::OK;
-  } else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
-    return returnvalue::OK;
-  } else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
-    return returnvalue::OK;
-  } else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
-    return returnvalue::OK;
-  } else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
-    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
-    return returnvalue::OK;
-  } else {
-    return returnvalue::FAILED;
+  } else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
+                12 * sizeof(double));
+    return acsctrl::SINGLE_RW_UNAVAILABLE;
+  } else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
+                12 * sizeof(double));
+    return acsctrl::SINGLE_RW_UNAVAILABLE;
+  } else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
+                12 * sizeof(double));
+    return acsctrl::SINGLE_RW_UNAVAILABLE;
+  } else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
+    std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
+                12 * sizeof(double));
+    return acsctrl::SINGLE_RW_UNAVAILABLE;
   }
+  return acsctrl::MULTIPLE_RW_UNAVAILABLE;
 }
 
+void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
+
 void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
   std::error_code e;
   if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or

mission/controller/acs/Guidance.h

@@ -1,11 +1,20 @@
 #ifndef GUIDANCE_H_
 #define GUIDANCE_H_
 
+#include <fsfw/coordinates/CoordinateTransformations.h>
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/math/MatrixOperations.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/SensorValues.h>
+#include <mission/controller/acs/util/MathOperations.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 #include <time.h>
 
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
-#include "AcsParameters.h"
-#include "SensorValues.h"
+#include <cmath>
+#include <filesystem>
+#include <string>
 
 class Guidance {
  public:
@@ -14,33 +23,20 @@ class Guidance {
 
   void getTargetParamsSafe(double sunTargetSafe[3]);
   ReturnValue_t solarArrayDeploymentComplete();
+  void resetValues();
 
-  // Function to get the target quaternion and reference rotation rate from gps position and
-  // position of the ground station
-  void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
-                               double refDirB[3], double quatBI[4], double targetQuat[4],
-                               double targetSatRotRate[3]);
-  void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
-                              double targetSatRotRate[3]);
-  void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
-                       double targetSatRotRate[3]);
+  void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3],
+                         const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
+  void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
+                           double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
+  void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
+                       double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
+  void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3],
+                          double velSatF[3], double targetQuat[4], double refSatRate[3]);
 
-  // Function to get the target quaternion and reference rotation rate for sun pointing after ground
-  // station
-  void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
-                        double targetSatRotRate[3]);
+  void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
+                          double *targetSatRotRate);
 
-  // Function to get the target quaternion and refence rotation rate from gps position for Nadir
-  // pointing
-  void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
-                                    double targetQuat[4], double refDirB[3], double refSatRate[3]);
-  void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
-                                   double targetQuat[4], double refSatRate[3]);
-
-  // @note: Calculates the error quaternion between the current orientation and the target
-  // quaternion, considering a reference quaternion. Additionally the difference between the actual
-  // and a desired satellite rotational rate is calculated, again considering a reference rotational
-  // rate. Lastly gives back the error angle of the error quaternion.
   void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
                   double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                   double errorQuat[4], double errorSatRotRate[3], double &errorAngle);
@@ -48,20 +44,16 @@ class Guidance {
                   double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
                   double &errorAngle);
 
-  void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
-                          double *targetSatRotRate);
-
-  // @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
-  // reation wheel 	maybe can be done in "commanding.h"
   ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv);
 
  private:
   const AcsParameters *acsParameters;
 
+  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
+  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
+
   bool strBlindAvoidFlag = false;
-  timeval timeSavedQuaternion;
-  double savedQuaternion[4] = {0, 0, 0, 0};
-  double omegaRefSaved[3] = {0, 0, 0};
+  double quatIXprev[4] = {0, 0, 0, 0};
 
   static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
   static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -19,7 +19,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
 ReturnValue_t MultiplicativeKalmanFilter::init(
     const double *magneticField_, const bool validMagField_, const double *sunDir_,
     const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-    const bool validMagModel, acsctrl::MekfData *mekfData,
+    const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
     AcsParameters *acsParameters) {  // valids for "model measurements"?
   // check for valid mag/sun
   if (validMagField_ && validSS && validSSModel && validMagModel) {
@@ -191,7 +191,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
     const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
     const bool validGYRs_, const double *magneticField_, const bool validMagField_,
     const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
-    const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
+    const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
     AcsParameters *acsParameters) {
   // Check for GYR Measurements
   int MDF = 0;  // Matrix Dimension Factor
@@ -1090,7 +1090,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   return MEKF_RUNNING;
 }
 
-ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
+ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::AttitudeEstimationData *mekfData) {
   double resetQuaternion[4] = {0, 0, 0, 1};
   double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                         {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
@@ -1100,7 +1100,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
   return MEKF_UNINITIALIZED;
 }
 
-void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
+void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData,
                                                           MekfStatus mekfStatus) {
   {
     PoolReadGuard pg(mekfData);
@@ -1115,8 +1115,9 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek
   }
 }
 
-void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus,
-                                               double quat[4], double satRotRate[3]) {
+void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData,
+                                               MekfStatus mekfStatus, double quat[4],
+                                               double satRotRate[3]) {
   {
     PoolReadGuard pg(mekfData);
     if (pg.getReadResult() == returnvalue::OK) {

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -21,7 +21,7 @@ class MultiplicativeKalmanFilter {
   MultiplicativeKalmanFilter();
   virtual ~MultiplicativeKalmanFilter();
 
-  ReturnValue_t reset(acsctrl::MekfData *mekfData);
+  ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
 
   /* @brief: init() - This function initializes the Kalman Filter and will provide the first
    * quaternion through the QUEST algorithm
@@ -32,8 +32,8 @@ class MultiplicativeKalmanFilter {
    */
   ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
                      const bool validSS, const double *sunDirJ, const bool validSSModel,
-                     const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
-                     AcsParameters *acsParameters);
+                     const double *magFieldJ, const bool validMagModel,
+                     acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
 
   /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
    * for the current step after the initalization
@@ -53,7 +53,7 @@ class MultiplicativeKalmanFilter {
                         const bool validGYRs_, const double *magneticField_,
                         const bool validMagField_, const double *sunDir_, const bool validSS,
                         const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-                        const bool validMagModel, acsctrl::MekfData *mekfData,
+                        const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
                         AcsParameters *acsParameters);
 
   enum MekfStatus : uint8_t {
@@ -99,9 +99,9 @@ class MultiplicativeKalmanFilter {
   double biasGYR[3]; /*Between measured and estimated sat Rate*/
                      /*Parameter INIT*/
   /*Functions*/
-  void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
-  void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
-                     double satRotRate[3]);
+  void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
+  void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
+                     double quat[4], double satRotRate[3]);
 };
 
 #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

mission/controller/acs/Navigation.cpp

@@ -16,7 +16,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
                                   acsctrl::GyrDataProcessed *gyrDataProcessed,
                                   acsctrl::MgmDataProcessed *mgmDataProcessed,
                                   acsctrl::SusDataProcessed *susDataProcessed,
-                                  acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
+                                  acsctrl::AttitudeEstimationData *mekfData,
+                                  AcsParameters *acsParameters) {
   double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
   bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
@@ -41,7 +42,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
   }
 }
 
-void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
+void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
   mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
 }
 
@@ -54,7 +55,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
     {
       PoolReadGuard pg(gpsDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        gpsDataProcessed->source = acs::GpsSource::SPG4;
+        gpsDataProcessed->source = acs::gps::Source::SPG4;
         gpsDataProcessed->source.setValid(true);
         std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
         gpsDataProcessed->gpsPosition.setValid(true);
@@ -66,7 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
     {
       PoolReadGuard pg(gpsDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        gpsDataProcessed->source = acs::GpsSource::NONE;
+        gpsDataProcessed->source = acs::gps::Source::NONE;
         gpsDataProcessed->source.setValid(true);
         std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
         gpsDataProcessed->gpsPosition.setValid(false);

mission/controller/acs/Navigation.h

@@ -17,9 +17,9 @@ class Navigation {
   ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
                         acsctrl::GyrDataProcessed *gyrDataProcessed,
                         acsctrl::MgmDataProcessed *mgmDataProcessed,
-                        acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
-                        AcsParameters *acsParameters);
-  void resetMekf(acsctrl::MekfData *mekfData);
+                        acsctrl::SusDataProcessed *susDataProcessed,
+                        acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters);
+  void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
 
   ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
   ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);

mission/controller/acs/SensorProcessing.cpp

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

mission/controller/acs/SensorProcessing.h

@@ -24,22 +24,21 @@ class SensorProcessing {
   SensorProcessing();
   virtual ~SensorProcessing();
 
-  void process(timeval now, ACS::SensorValues *sensorValues,
+  void process(timeval timeAbsolute, double timeDelta, ACS::SensorValues *sensorValues,
                acsctrl::MgmDataProcessed *mgmDataProcessed,
                acsctrl::SusDataProcessed *susDataProcessed,
                acsctrl::GyrDataProcessed *gyrDataProcessed,
-               acsctrl::GpsDataProcessed *gpsDataProcessed,
-               const AcsParameters *acsParameters);  // Will call protected functions
+               acsctrl::GpsDataProcessed *gpsDataProcessed, const AcsParameters *acsParameters);
+
  private:
   static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
   static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
   static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
 
  protected:
-  // short description needed for every function
   void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
                   const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
-                  const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
+                  const float *mgm4Value, bool mgm4valid, timeval timeAbsolute, double timeDelta,
                   const AcsParameters::MgmHandlingParameters *mgmParameters,
                   acsctrl::GpsDataProcessed *gpsDataProcessed,
                   acsctrl::MgmDataProcessed *mgmDataProcessed);
@@ -52,7 +51,7 @@ class SensorProcessing {
                   bool sus7valid, const uint16_t *sus8Value, bool sus8valid,
                   const uint16_t *sus9Value, bool sus9valid, const uint16_t *sus10Value,
                   bool sus10valid, const uint16_t *sus11Value, bool sus11valid,
-                  timeval timeOfSusMeasurement,
+                  timeval timeAbsolute, double timeDelta,
                   const AcsParameters::SusHandlingParameters *susParameters,
                   const AcsParameters::SunModelParameters *sunModelParameters,
                   acsctrl::SusDataProcessed *susDataProcessed);
@@ -65,7 +64,6 @@ class SensorProcessing {
                   bool gyr2axYvalid, const double gyr2axZvalue, bool gyr2axZvalid,
                   const double gyr3axXvalue, bool gyr3axXvalid, const double gyr3axYvalue,
                   bool gyr3axYvalid, const double gyr3axZvalue, bool gyr3axZvalid,
-                  timeval timeOfGyrMeasurement,
                   const AcsParameters::GyrHandlingParameters *gyrParameters,
                   acsctrl::GyrDataProcessed *gyrDataProcessed);
 
@@ -77,13 +75,9 @@ class SensorProcessing {
   void lowPassFilter(double *newValue, double *oldValue, const double weight);
 
   double savedMgmVecTot[3] = {0.0, 0.0, 0.0};
-  timeval timeOfSavedMagFieldEst;
   double savedSusVecTot[3] = {0.0, 0.0, 0.0};
-  timeval timeOfSavedSusDirEst;
-  bool validMagField = false;
 
   double savedPosSatE[3] = {0.0, 0.0, 0.0};
-  double timeOfSavedPosSatE = 0.0;
   bool validSavedPosSatE = false;
 
   SusConverter susConverter;

mission/controller/acs/control/Detumble.cpp

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

mission/controller/acs/control/Detumble.h

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

mission/controller/acs/control/PtgCtrl.cpp

@@ -10,6 +10,21 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
 
 PtgCtrl::~PtgCtrl() {}
 
+acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
+    const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid,
+    const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) {
+  if (not magFieldValid) {
+    return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
+  } else if (mekfEnabled and mekfValid) {
+    return acs::ControlModeStrategy::PTGCTRL_MEKF;
+  } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
+    return acs::ControlModeStrategy::PTGCTRL_STR;
+  } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
+    return acs::ControlModeStrategy::PTGCTRL_QUEST;
+  }
+  return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
+}
+
 void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
                      const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
                      double *torqueRws) {
@@ -24,7 +39,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
 
   double cInt = 2 * om * zeta;
-  double kInt = 2 * pow(om, 2);
+  double kInt = 2 * om * om;
 
   double qErrorLaw[3] = {0, 0, 0};
 
@@ -96,9 +111,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
   VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
 }
 
-void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
+void PtgCtrl::ptgNullspace(const bool allRwAvabilable,
+                           AcsParameters::PointingLawParameters *pointingLawParameters,
                            const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
                            const int32_t speedRw3, double *rwTrqNs) {
+  if (not allRwAvabilable) {
+    return;
+  }
   // concentrate RW speeds as vector and convert to double
   double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
                         static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
@@ -205,6 +224,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
           }
         }
       }
+    } else {
+      rwCmdSpeeds[i] = 0;
     }
   }
 }

mission/controller/acs/control/PtgCtrl.h

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

mission/controller/acs/control/SafeCtrl.cpp

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

mission/controller/acs/control/SafeCtrl.h

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

mission/controller/acs/util/MathOperations.h

@@ -4,15 +4,12 @@
 #include <fsfw/src/fsfw/globalfunctions/constants.h>
 #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/src/fsfw/globalfunctions/sign.h>
-#include <stdint.h>
-#include <string.h>
-#include <sys/time.h>
+#include <fsfw/src/fsfw/serviceinterface.h>
 
 #include <cmath>
+#include <cstring>
 #include <iostream>
 
-#include "fsfw/serviceinterface.h"
-
 template <typename T1, typename T2 = T1>
 class MathOperations {
  public:
@@ -46,7 +43,7 @@ class MathOperations {
   static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
     int min_idx;
     T1 temp;
-    memcpy(result, matrix, rowSize * colSize * sizeof(*result));
+    std::memcpy(result, matrix, rowSize * colSize * sizeof(*result));
     // One by one move boundary of unsorted subarray
     for (int k = 0; k < rowSize; k++) {
       for (int i = 0; i < colSize - 1; i++) {

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

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

mission/payload/ScexDeviceHandler.cpp

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

mission/pollingSeqTables.cpp

@@ -628,7 +628,13 @@ ReturnValue_t pst::pstPayload(FixedTimeslotTaskIF *thisSequence) {
                         FreshSupvHandler::OpCode::PARSE_TM);
   thisSequence->addSlot(objects::PLOC_SUPERVISOR_HANDLER, length * 0.2,
                         FreshSupvHandler::OpCode::PARSE_TM);
-  static_cast<void>(length);
 
+  thisSequence->addSlot(objects::SCEX, length * 0.6, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::SCEX, length * 0.6, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::SCEX, length * 0.6, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::SCEX, length * 0.6, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::SCEX, length * 0.6, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::SCEX, length * 0.8, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::SCEX, length * 0.8, DeviceHandlerIF::GET_READ);
   return thisSequence->checkSequence();
 }

mission/power/PcduHandler.cpp

@@ -65,8 +65,8 @@ ReturnValue_t PcduHandler::performOperation(uint8_t counter) {
 ReturnValue_t PcduHandler::initialize() {
   ReturnValue_t result;
 
-  IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
-  if (IPCStore == nullptr) {
+  ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+  if (ipcStore == nullptr) {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
 
@@ -162,10 +162,13 @@ void PcduHandler::updateHkTableDataset(store_address_t storeId, LocalPoolDataSet
                                     sizeof(CCSDSTime::CDS_short), dataset);
   const uint8_t* packet_ptr = nullptr;
   size_t size = 0;
-  result = IPCStore->getData(storeId, &packet_ptr, &size);
+  result = ipcStore->getData(storeId, &packet_ptr, &size);
   if (result != returnvalue::OK) {
-    sif::error << "PCDUHandler::updateHkTableDataset: Failed to get data from IPCStore."
-               << std::endl;
+    sif::error << "PCDUHandler::updateHkTableDataset: Failed to get data from IPC store, result 0x"
+               << std::hex << std::setw(4) << std::setfill('0') << result << std::dec
+               << std::setfill(' ') << std::endl;
+    result = ipcStore->deleteData(storeId);
+    return;
   }
   result = packetUpdate.deSerialize(&packet_ptr, &size, SerializeIF::Endianness::MACHINE);
   if (result != returnvalue::OK) {
@@ -173,7 +176,7 @@ void PcduHandler::updateHkTableDataset(store_address_t storeId, LocalPoolDataSet
                   "in hk table dataset"
                << std::endl;
   }
-  result = IPCStore->deleteData(storeId);
+  result = ipcStore->deleteData(storeId);
   if (result != returnvalue::OK) {
     sif::error << "PCDUHandler::updateHkTableDataset: Failed to delete data in IPCStore"
                << std::endl;
@@ -396,7 +399,7 @@ ReturnValue_t PcduHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO
   setParamMessage.serialize(&commandPtr, &serializedLength, maxSize, SerializeIF::Endianness::BIG);
 
   store_address_t storeAddress;
-  result = IPCStore->addData(&storeAddress, command, sizeof(command));
+  result = ipcStore->addData(&storeAddress, command, sizeof(command));
 
   CommandMessage message;
   ActionMessage::setCommand(&message, GOMSPACE::PARAM_SET, storeAddress);

mission/power/PcduHandler.h

@@ -94,7 +94,7 @@ class PcduHandler : public PowerSwitchIF,
    * Pointer to the IPCStore.
    * This caches the pointer received from the objectManager in the constructor.
    */
-  StorageManagerIF* IPCStore = nullptr;
+  StorageManagerIF* ipcStore = nullptr;
 
   /**
    * Message queue to communicate with other objetcs. Used for example to receive

mission/system/CMakeLists.txt

@@ -1,5 +1,4 @@
 add_subdirectory(objects)
-add_subdirectory(tree)
 add_subdirectory(acs)
 add_subdirectory(tcs)
 add_subdirectory(com)
@@ -8,4 +7,4 @@ add_subdirectory(power)
 target_sources(
   ${LIB_EIVE_MISSION}
   PRIVATE systemTree.cpp DualLanePowerStateMachine.cpp EiveSystem.cpp
-          treeUtil.cpp SharedPowerAssemblyBase.cpp)
+          treeUtil.cpp SharedPowerAssemblyBase.cpp payloadModeTree.cpp)

mission/system/EiveSystem.cpp

@@ -174,6 +174,7 @@ ReturnValue_t EiveSystem::initialize() {
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW));
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL));
+  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(acs::PTG_RATE_VIOLATION));
   return Subsystem::initialize();
 }
 
@@ -224,6 +225,16 @@ void EiveSystem::handleEventMessages() {
             }
             break;
           }
+          case acs::PTG_RATE_VIOLATION: {
+            CommandMessage msg;
+            HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
+            ReturnValue_t result = MessageQueueSenderIF::sendMessage(
+                strQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
+            if (result != returnvalue::OK) {
+              sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed"
+                         << std::endl;
+            }
+          }
         }
         break;
       default:

mission/system/acs/StrFdir.cpp

@@ -6,7 +6,7 @@ StrFdir::StrFdir(object_id_t strObject)
     : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
 
 ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
-  if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
+  if (event->getEvent() == acs::PTG_CTRL_NO_ATTITUDE_INFORMATION) {
     setFaulty(event->getEvent());
     return returnvalue::OK;
   }

mission/system/acs/acsModeTree.cpp

@@ -106,7 +106,7 @@ Subsystem& satsystem::acs::init() {
   // Build TARGET PT transition 0
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
   iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
-  iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TRANS_0.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TRANS_0.second);
   check(ACS_SUBSYSTEM.addTable(
@@ -161,6 +161,7 @@ void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::IMTQ_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
   iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
   iht(objects::ACS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
+  iht(objects::SUS_BOARD_ASS, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
   iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_OFF_TRANS_1.second);
   check(ss.addTable(TableEntry(ACS_TABLE_OFF_TRANS_1.first, &ACS_TABLE_OFF_TRANS_1.second)), ctxc);
 
@@ -199,13 +200,13 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::ACS_CONTROLLER, acs::AcsMode::SAFE, acs::SafeSubmode::DEFAULT,
       ACS_TABLE_SAFE_TGT.second, true);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true);
-  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_SAFE_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TGT.second, true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TGT.second, true);
   check(ss.addTable(&ACS_TABLE_SAFE_TGT.second, ACS_TABLE_SAFE_TGT.first, false, true), ctxc);
 
   // Build SAFE transition 0
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_SAFE_TRANS_0.second);
-  iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
   iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_SAFE_TRANS_0.second, true);
   iht(objects::STR_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
   iht(objects::RW_ASSY, OFF, 0, ACS_TABLE_SAFE_TRANS_0.second);
@@ -256,13 +257,13 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true);
-  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_IDLE_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TGT.second, true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TGT.second, true);
   ss.addTable(&ACS_TABLE_IDLE_TGT.second, ACS_TABLE_IDLE_TGT.first, false, true);
 
   // Build IDLE transition 0
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
-  iht(objects::ACS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
   iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_IDLE_TRANS_0.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_IDLE_TRANS_0.second);
@@ -306,8 +307,8 @@ void buildTargetPtSequence(Subsystem& ss, ModeListEntry& eh) {
   // Build TARGET PT table
   iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true);
-  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_TGT.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_TGT.second);
   check(ss.addTable(&ACS_TABLE_PTG_TARGET_TGT.second, ACS_TABLE_PTG_TARGET_TGT.first, false, true),
@@ -355,8 +356,8 @@ void buildTargetPtNadirSequence(Subsystem& ss, ModeListEntry& eh) {
   // Build TARGET PT table
   iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_NADIR, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
-  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_NADIR_TGT.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_NADIR_TGT.second);
   check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_NADIR_TGT.first,
@@ -408,8 +409,8 @@ void buildTargetPtGsSequence(Subsystem& ss, ModeListEntry& eh) {
   // Build TARGET PT table
   iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_TARGET_GS, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
-  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_GS_TGT.second, true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_GS_TGT.second);
   check(ss.addTable(
@@ -461,8 +462,10 @@ void buildTargetPtInertialSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::ACS_CONTROLLER, acs::AcsMode::PTG_INERTIAL, 0,
       ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   iht(objects::IMTQ_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
-  iht(objects::SUS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true);
-  iht(objects::ACS_BOARD_ASS, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second, true);
+  iht(objects::SUS_BOARD_ASS, NML, duallane::A_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second,
+      true);
+  iht(objects::ACS_BOARD_ASS, NML, duallane::B_SIDE, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second,
+      true);
   iht(objects::RW_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   iht(objects::STR_ASSY, NML, 0, ACS_TABLE_PTG_TARGET_INERTIAL_TGT.second);
   check(ss.addTable(TableEntry(ACS_TABLE_PTG_TARGET_INERTIAL_TGT.first,

mission/system/systemTree.cpp

@@ -6,15 +6,13 @@
 #include <mission/sysDefs.h>
 #include <mission/system/com/comModeTree.h>
 
-#include <atomic>
-
 #include "eive/objects.h"
 #include "mission/com/defs.h"
 #include "mission/sysDefs.h"
 #include "mission/system/acs/acsModeTree.h"
+#include "mission/system/payloadModeTree.h"
 #include "mission/system/power/epsModeTree.h"
 #include "mission/system/tcs/tcsModeTree.h"
-#include "mission/system/tree/payloadModeTree.h"
 #include "treeUtil.h"
 
 namespace {

mission/system/tree/CMakeLists.txt

@@ -1 +0,0 @@
-target_sources(${LIB_EIVE_MISSION} PRIVATE payloadModeTree.cpp)