diff --git a/CHANGELOG.md b/CHANGELOG.md index fea76a29..c24a5b96 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,16 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- PLOC SUPV latchup report could not be handled previously. +- Bugfix in PLOC SUPV latchup report parsing. + +# [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. @@ -23,12 +33,31 @@ will consitute of a breaking change warranting a new major release: ## 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. -- PLOC SUPV latchup report could not be handled previously. -- Bugfix in PLOC SUPV latchup report parsing. +- 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. +>>>>>>> origin/main # [v7.5.5] 2024-01-22 @@ -54,7 +83,6 @@ will consitute of a breaking change warranting a new major release: ## Fixed - Fixed faulty scaling within the QUEST algorithm. ->>>>>>> 16fa3d1e269cba5af4a22c7ba59e6a8e4a5980f5 # [v7.5.1] 2023-12-13 diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b8eb93e..8957c6bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,8 +10,8 @@ cmake_minimum_required(VERSION 3.13) set(OBSW_VERSION_MAJOR 7) -set(OBSW_VERSION_MINOR 5) -set(OBSW_VERSION_REVISION 5) +set(OBSW_VERSION_MINOR 6) +set(OBSW_VERSION_REVISION 0) # set(CMAKE_VERBOSE TRUE) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index d5b3de46..c1174d8a 100644 --- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp +++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 318 translations. + * @brief Auto-generated event translation file. Contains 319 translations. * @details - * Generated on: 2023-12-13 11:29:45 + * Generated on: 2024-01-30 09:10:05 */ #include "translateEvents.h" @@ -94,7 +94,7 @@ 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"; @@ -103,6 +103,7 @@ const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFO 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"; @@ -505,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): @@ -522,6 +523,8 @@ const char *translateEvents(Event event) { 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): diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index ac61a526..9de30a3c 100644 --- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp +++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-12-13 11:29:45 + * Generated on: 2024-01-30 09:10:05 */ #include "translateObjects.h" diff --git a/fsfw b/fsfw index e64e8b27..b5e7179a 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit e64e8b274d436502d5c5b87865b9006e52e4b1aa +Subproject commit b5e7179af1da085b9be598b4897f2664012781af diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 97eeaf37..e02f0638 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -88,7 +88,7 @@ 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 @@ -97,6 +97,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 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 diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index 8ef110db..52acf3dd 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -454,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 @@ -487,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 @@ -509,9 +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 -0x6a01;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;1;ACS_CTRL;mission/controller/AcsController.h -0x6a02;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;2;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 diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 97eeaf37..e02f0638 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -88,7 +88,7 @@ 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 @@ -97,6 +97,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 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 diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 76184dce..72d47797 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -454,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 @@ -499,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 @@ -593,9 +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 -0x6a01;ACSCTRL_WriteFileFailed;Writing the TLE to the file has failed.;1;ACS_CTRL;mission/controller/AcsController.h -0x6a02;ACSCTRL_ReadFileFailed;Reading the TLE to the file has failed.;2;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 diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index d5b3de46..c1174d8a 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 318 translations. + * @brief Auto-generated event translation file. Contains 319 translations. * @details - * Generated on: 2023-12-13 11:29:45 + * Generated on: 2024-01-30 09:10:05 */ #include "translateEvents.h" @@ -94,7 +94,7 @@ 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"; @@ -103,6 +103,7 @@ const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFO 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"; @@ -505,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): @@ -522,6 +523,8 @@ const char *translateEvents(Event event) { 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): diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index 30696975..7ab40047 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 179 translations. - * Generated on: 2023-12-13 11:29:45 + * Generated on: 2024-01-30 09:10:05 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index d5b3de46..c1174d8a 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 318 translations. + * @brief Auto-generated event translation file. Contains 319 translations. * @details - * Generated on: 2023-12-13 11:29:45 + * Generated on: 2024-01-30 09:10:05 */ #include "translateEvents.h" @@ -94,7 +94,7 @@ 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"; @@ -103,6 +103,7 @@ const char *PTG_CTRL_NO_ATTITUDE_INFORMATION_STRING = "PTG_CTRL_NO_ATTITUDE_INFO 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"; @@ -505,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): @@ -522,6 +523,8 @@ const char *translateEvents(Event event) { 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): diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index 30696975..7ab40047 100644 --- a/linux/fsfwconfig/objects/translateObjects.cpp +++ b/linux/fsfwconfig/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 179 translations. - * Generated on: 2023-12-13 11:29:45 + * Generated on: 2024-01-30 09:10:05 */ #include "translateObjects.h" diff --git a/linux/payload/FreshSupvHandler.cpp b/linux/payload/FreshSupvHandler.cpp index 09b56d14..346cd8f2 100644 --- a/linux/payload/FreshSupvHandler.cpp +++ b/linux/payload/FreshSupvHandler.cpp @@ -1123,7 +1123,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->sendMessageFrom(getCommandQueue(), &actionMsg, MessageQueueIF::NO_QUEUE); + 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 " diff --git a/mission/acs/RwHandler.h b/mission/acs/RwHandler.h index a8903b81..87689fe6 100644 --- a/mission/acs/RwHandler.h +++ b/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); diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 3a62b51f..67fcf024 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -66,8 +66,10 @@ enum Source : uint8_t { 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] Multiple RWs are invalid, uncommandable and therefore higher ACS modes //! cannot be maintained. static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8e7ae8d4..264e9f24 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -49,7 +49,7 @@ 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; } @@ -195,6 +195,8 @@ void AcsController::performAttitudeControl() { mekfInvalidFlag = false; } + handleDetumbling(); + switch (mode) { case acs::SAFE: switch (submode) { @@ -284,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 (attitudeEstimationData.satRotRateMekf.isValid() and - VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } - } else if (acsParameters.safeModeControllerParameters.useGyr) { - if (gyrDataProcessed.gyrVecTot.isValid() and - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } - } else if (fusedRotRateData.rotRateTotal.isValid() and - VectorOperations::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], @@ -346,33 +321,6 @@ void AcsController::performDetumble() { actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); - if (acsParameters.safeModeControllerParameters.useMekf) { - if (attitudeEstimationData.satRotRateMekf.isValid() and - VectorOperations::norm(attitudeEstimationData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } - } else if (acsParameters.safeModeControllerParameters.useGyr) { - if (gyrDataProcessed.gyrVecTot.isValid() and - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } - } else if (fusedRotRateData.rotRateTotal.isValid() and - VectorOperations::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], @@ -412,6 +360,7 @@ void AcsController::performPointingCtrl() { 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], @@ -445,10 +394,10 @@ void AcsController::performPointingCtrl() { break; } - uint8_t enableAntiStiction = true; + bool allRwAvailable = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); - if (result == returnvalue::FAILED) { + if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { triggerEvent(acs::MULTIPLE_RW_INVALID); @@ -456,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 @@ -475,7 +426,7 @@ void AcsController::performPointingCtrl() { 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); @@ -486,7 +437,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); - enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET: @@ -499,7 +449,7 @@ void AcsController::performPointingCtrl() { 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); @@ -510,7 +460,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); - enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET_GS: @@ -520,7 +469,7 @@ void AcsController::performPointingCtrl() { 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); @@ -531,7 +480,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); - enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; case acs::PTG_NADIR: @@ -544,7 +492,7 @@ void AcsController::performPointingCtrl() { 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); @@ -555,7 +503,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); - enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case acs::PTG_INERTIAL: @@ -567,7 +514,7 @@ void AcsController::performPointingCtrl() { 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); @@ -578,7 +525,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); - enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; @@ -590,9 +536,7 @@ 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); @@ -604,6 +548,62 @@ void AcsController::performPointingCtrl() { acsParameters.rwHandlingParameters.rampTime); } +void AcsController::handleDetumbling() { + switch (detumbleState) { + case DetumbleState::NO_DETUMBLE: + if (fusedRotRateData.rotRateTotal.isValid() and + VectorOperations::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); + detumbleState = DetumbleState::PTG_TO_SAFE_TRANSITION; + break; + case DetumbleState::PTG_TO_SAFE_TRANSITION: + if (mode == acs::AcsMode::SAFE) { + detumbleState = DetumbleState::DETUMBLE_FROM_SAFE; + } + break; + case DetumbleState::DETUMBLE_FROM_SAFE: + detumbleCounter = 0; + // Triggers detumble mode transition in subsystem + triggerEvent(acs::SAFE_RATE_VIOLATION); + startTransition(mode, acs::SafeSubmode::DETUMBLE); + detumbleState = DetumbleState::IN_DETUMBLE; + break; + case DetumbleState::IN_DETUMBLE: + if (fusedRotRateData.rotRateTotal.isValid() and + VectorOperations::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); @@ -885,6 +885,25 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, } void AcsController::modeChanged(Mode_t mode, Submode_t submode) { + 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); + } + } + guidance.resetValues(); return ExtendedControllerBase::modeChanged(mode, submode); } @@ -1081,7 +1100,7 @@ ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) { tleFile << "\n"; tleFile.write(reinterpret_cast(tle + 69), 69); } else { - return WRITE_FILE_FAILED; + return acsctrl::WRITE_FILE_FAILED; } tleFile.close(); return returnvalue::OK; @@ -1105,12 +1124,12 @@ ReturnValue_t AcsController::readTleFromFs(uint8_t *line1, uint8_t *line2) { std::memcpy(line2, tleLine2.c_str(), 69); } else { triggerEvent(acs::TLE_FILE_READ_FAILED); - return READ_FILE_FAILED; + return acsctrl::READ_FILE_FAILED; } tleFile.close(); } else { triggerEvent(acs::TLE_FILE_READ_FAILED); - return READ_FILE_FAILED; + return acsctrl::READ_FILE_FAILED; } return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index aa2b9411..ed893479 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -46,11 +46,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint16_t startAtIndex) override; protected: - void performAttitudeControl(); - 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}; @@ -101,6 +96,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes enum class InternalState { STARTUP, INITIAL_DELAY, READY }; InternalState internalState = InternalState::STARTUP; + enum class DetumbleState { + NO_DETUMBLE, + DETUMBLE_FROM_PTG, + PTG_TO_SAFE_TRANSITION, + DETUMBLE_FROM_SAFE, + IN_DETUMBLE + }; + DetumbleState detumbleState = DetumbleState::NO_DETUMBLE; + /** Device command IDs */ static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0; static const DeviceCommandId_t RESET_MEKF = 0x1; @@ -108,14 +112,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const DeviceCommandId_t UPDATE_TLE = 0x3; static const DeviceCommandId_t READ_TLE = 0x4; - 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); - //! [EXPORT] : [COMMENT] Writing the TLE to the file has failed. - static constexpr ReturnValue_t WRITE_FILE_FAILED = MAKE_RETURN_CODE(1); - //! [EXPORT] : [COMMENT] Reading the TLE to the file has failed. - static constexpr ReturnValue_t READ_FILE_FAILED = MAKE_RETURN_CODE(2); - ReturnValue_t initialize() override; ReturnValue_t handleCommandMessage(CommandMessage* message) override; void performControlOperation() override; @@ -134,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, diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index a6ce6c5d..8816eed9 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -333,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); @@ -432,9 +432,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(idleModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(idleModeControllerParameters.useMekf); break; default: @@ -471,42 +468,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(targetModeControllerParameters.useMekf); break; - case 0xB: + case 0xA: parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; - case 0xC: + case 0xB: parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; - case 0xD: + case 0xC: parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; - case 0xE: + case 0xD: parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; - case 0xF: + case 0xE: parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; - case 0x10: + case 0xF: parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; - case 0x11: + case 0x10: parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; - case 0x12: + case 0x11: parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; - case 0x13: + case 0x12: parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; - case 0x14: + case 0x13: parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); break; - case 0x15: + case 0x14: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -543,24 +537,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(gsTargetModeControllerParameters.useMekf); break; - case 0xB: + case 0xA: parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; - case 0xC: + case 0xB: parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; - case 0xD: + case 0xC: parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; - case 0xE: + case 0xD: parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); break; - case 0xF: + case 0xE: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -597,21 +588,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(nadirModeControllerParameters.useMekf); break; - case 0xB: + case 0xA: parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; - case 0xC: + case 0xB: parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; - case 0xD: + case 0xC: parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); break; - case 0xE: + case 0xD: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: @@ -648,18 +636,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(inertialModeControllerParameters.useMekf); break; - case 0xB: + case 0xA: parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; - case 0xC: + case 0xB: parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; - case 0xD: + case 0xC: parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f11a673b..09804612 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -812,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; @@ -863,7 +863,6 @@ 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; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 10d1393c..12957ab2 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -1,15 +1,5 @@ #include "Guidance.h" -#include -#include -#include -#include -#include - -#include -#include -#include - Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } Guidance::~Guidance() {} @@ -431,7 +421,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::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); // Then subtract the combined required satellite rotational rates from the actual rate @@ -455,44 +449,16 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do } void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta, - double quatInertialTarget[4], double *refSatRate) { - //------------------------------------------------------------------------------------- - // Calculation of target rotation rate - //------------------------------------------------------------------------------------- - if (VectorOperations::norm(savedQuaternion, 4) == 0) { - std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + double quatIX[4], double *refSatRate) { + if (VectorOperations::norm(quatIXprev, 4) == 0) { + std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } - if (timeDelta < timeElapsedMax and timeDelta != 0.0) { - 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::subtract(q, qS, qDiff, 4); - VectorOperations::mulScalar(qDiff, 1. / timeDelta, 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::cross(tgtQuatVec, qDiffVec, sum1); - VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, q[3], sum3, 3); - VectorOperations::add(sum1, sum2, sum, 3); - VectorOperations::subtract(sum, sum3, sum, 3); - double omegaRefNew[3] = {0, 0, 0}; - VectorOperations::mulScalar(sum, -2, omegaRefNew, 3); - - VectorOperations::mulScalar(omegaRefNew, 2, refSatRate, 3); - VectorOperations::subtract(refSatRate, omegaRefSaved, refSatRate, 3); - omegaRefSaved[0] = omegaRefNew[0]; - omegaRefSaved[1] = omegaRefNew[1]; - omegaRefSaved[2] = omegaRefNew[2]; + if (timeDelta != 0.0) { + QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate); } else { - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; + std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double)); } - - std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev)); } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, @@ -506,22 +472,27 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { - std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); - return returnvalue::OK; + 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.without2, 12 * sizeof(double)); - return returnvalue::OK; + 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.without3, 12 * sizeof(double)); - return returnvalue::OK; + 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.without4, 12 * sizeof(double)); - return returnvalue::OK; - } else { - return returnvalue::FAILED; + 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 diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 1cf5b8a9..bdf6d7fb 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -1,11 +1,19 @@ #ifndef GUIDANCE_H_ #define GUIDANCE_H_ +#include +#include +#include +#include +#include +#include +#include +#include #include -#include "../controllerdefinitions/AcsCtrlDefinitions.h" -#include "AcsParameters.h" -#include "SensorValues.h" +#include +#include +#include class Guidance { public: @@ -14,6 +22,7 @@ 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 @@ -59,9 +68,11 @@ class Guidance { private: const AcsParameters *acsParameters; + static constexpr double ZERO_VEC3[3] = {0, 0, 0}; + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; + bool strBlindAvoidFlag = false; - double 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"; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index fd5b9f29..0294e8b6 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -39,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}; @@ -111,9 +111,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters VectorOperations::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(speedRw0), static_cast(speedRw1), static_cast(speedRw2), static_cast(speedRw3)}; @@ -220,6 +224,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee } } } + } else { + rwCmdSpeeds[i] = 0; } } } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index a07849ec..66402b0d 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -33,7 +33,8 @@ class PtgCtrl { 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); diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index dd4c3f57..023e9379 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -4,15 +4,12 @@ #include #include #include -#include -#include -#include +#include #include +#include #include -#include "fsfw/serviceinterface.h" - template 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++) { diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 83a74552..c830ffac 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -1,6 +1,7 @@ #ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ #define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ +#include #include #include @@ -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, diff --git a/mission/power/PcduHandler.cpp b/mission/power/PcduHandler.cpp index 0561288c..1596d522 100644 --- a/mission/power/PcduHandler.cpp +++ b/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(objects::IPC_STORE); - if (IPCStore == nullptr) { + ipcStore = ObjectManager::instance()->get(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); diff --git a/mission/power/PcduHandler.h b/mission/power/PcduHandler.h index 86d3747f..73df73ef 100644 --- a/mission/power/PcduHandler.h +++ b/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 diff --git a/mission/system/EiveSystem.cpp b/mission/system/EiveSystem.cpp index cd450502..bb0f5e69 100644 --- a/mission/system/EiveSystem.cpp +++ b/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: diff --git a/tmtc b/tmtc index bcdd12ca..e5fe0ab9 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit bcdd12caf05b6a874b0d3ac2b9436c4061545312 +Subproject commit e5fe0ab95ac2f695eb9a533ad825314f359f513a