Merge branch 'main' into mekf-fix
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
This commit is contained in:
commit
c85bef6de4
50
CHANGELOG.md
50
CHANGELOG.md
@ -16,6 +16,56 @@ will consitute of a breaking change warranting a new major release:
|
||||
|
||||
# [unreleased]
|
||||
|
||||
# [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
|
||||
|
||||
@ -10,8 +10,8 @@
|
||||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
set(OBSW_VERSION_MAJOR 7)
|
||||
set(OBSW_VERSION_MINOR 5)
|
||||
set(OBSW_VERSION_REVISION 3)
|
||||
set(OBSW_VERSION_MINOR 6)
|
||||
set(OBSW_VERSION_REVISION 0)
|
||||
|
||||
# set(CMAKE_VERBOSE TRUE)
|
||||
|
||||
|
||||
@ -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):
|
||||
|
||||
@ -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"
|
||||
|
||||
|
||||
2
fsfw
2
fsfw
@ -1 +1 @@
|
||||
Subproject commit e64e8b274d436502d5c5b87865b9006e52e4b1aa
|
||||
Subproject commit b5e7179af1da085b9be598b4897f2664012781af
|
||||
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
@ -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):
|
||||
|
||||
@ -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"
|
||||
|
||||
|
||||
@ -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):
|
||||
|
||||
@ -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"
|
||||
|
||||
|
||||
@ -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 "
|
||||
|
||||
@ -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,6 +1396,9 @@ bool PlocMpsocHandler::handleHwStartup() {
|
||||
return true;
|
||||
#endif
|
||||
if (powerState == PowerState::IDLE) {
|
||||
if (skipSupvCommandingToOn) {
|
||||
powerState = PowerState::DONE;
|
||||
} else {
|
||||
if (supv::SUPV_ON) {
|
||||
commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
|
||||
supvTransitionCd.resetTimer();
|
||||
@ -1405,6 +1409,7 @@ bool PlocMpsocHandler::handleHwStartup() {
|
||||
setMode(MODE_OFF);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (powerState == PowerState::SUPV_FAILED) {
|
||||
setMode(MODE_OFF);
|
||||
powerState = PowerState::IDLE;
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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_ */
|
||||
|
||||
@ -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: {
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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<double>::norm(attitudeEstimationData.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],
|
||||
@ -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<double>::norm(attitudeEstimationData.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],
|
||||
@ -403,14 +351,18 @@ void AcsController::performPointingCtrl() {
|
||||
acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
|
||||
attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
|
||||
fusedRotRateData.rotRateSource.isValid(), useMekf);
|
||||
fusedRotRateData.rotRateSource.value, useMekf);
|
||||
|
||||
if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
|
||||
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);
|
||||
@ -437,15 +389,15 @@ void AcsController::performPointingCtrl() {
|
||||
std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
|
||||
sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
|
||||
<< std::endl;
|
||||
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);
|
||||
@ -453,8 +405,10 @@ void AcsController::performPointingCtrl() {
|
||||
}
|
||||
multipleRwUnavailableCounter++;
|
||||
return;
|
||||
} else {
|
||||
}
|
||||
multipleRwUnavailableCounter = 0;
|
||||
if (result == acsctrl::SINGLE_RW_UNAVAILABLE) {
|
||||
allRwAvailable = false;
|
||||
}
|
||||
|
||||
// Variables required for guidance
|
||||
@ -472,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);
|
||||
@ -483,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:
|
||||
@ -496,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);
|
||||
@ -507,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:
|
||||
@ -517,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);
|
||||
@ -528,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:
|
||||
@ -541,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);
|
||||
@ -552,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:
|
||||
@ -564,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);
|
||||
@ -575,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;
|
||||
@ -587,13 +536,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);
|
||||
}
|
||||
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],
|
||||
@ -601,6 +548,62 @@ 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);
|
||||
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<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);
|
||||
@ -671,7 +674,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));
|
||||
@ -682,13 +685,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));
|
||||
@ -699,21 +702,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::ControlModeStrategy::CTRL_OFF;
|
||||
ctrlValData.safeStrat.value = ctrlStrat;
|
||||
ctrlValData.setValidity(true, true);
|
||||
}
|
||||
}
|
||||
@ -881,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);
|
||||
}
|
||||
|
||||
@ -1077,7 +1100,7 @@ ReturnValue_t AcsController::writeTleToFs(const uint8_t *tle) {
|
||||
tleFile << "\n";
|
||||
tleFile.write(reinterpret_cast<const char *>(tle + 69), 69);
|
||||
} else {
|
||||
return WRITE_FILE_FAILED;
|
||||
return acsctrl::WRITE_FILE_FAILED;
|
||||
}
|
||||
tleFile.close();
|
||||
return returnvalue::OK;
|
||||
@ -1101,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;
|
||||
}
|
||||
|
||||
@ -46,12 +46,8 @@ 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};
|
||||
static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
|
||||
static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
|
||||
@ -100,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;
|
||||
@ -107,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;
|
||||
@ -133,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,
|
||||
@ -143,10 +147,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
void updateActuatorCmdData(const int16_t* mtqTargetDipole);
|
||||
void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
|
||||
const int16_t* mtqTargetDipole);
|
||||
void updateCtrlValData(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);
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -815,19 +815,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;
|
||||
|
||||
@ -866,7 +866,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;
|
||||
|
||||
|
||||
@ -1,15 +1,5 @@
|
||||
#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 <mission/controller/acs/util/MathOperations.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <filesystem>
|
||||
#include <string>
|
||||
|
||||
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
|
||||
|
||||
Guidance::~Guidance() {}
|
||||
@ -418,7 +408,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
||||
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);
|
||||
double invTargetQuat[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::inverse(targetQuat, invTargetQuat);
|
||||
QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat);
|
||||
// Last calculate add rotation from reference quaternion
|
||||
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
|
||||
// Keep scalar part of quaternion positive
|
||||
@ -429,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<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
|
||||
// Then subtract the combined required satellite rotational rates from the actual rate
|
||||
@ -453,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<double>::norm(savedQuaternion, 4) == 0) {
|
||||
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
|
||||
double quatIX[4], double *refSatRate) {
|
||||
if (VectorOperations<double>::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<double>::subtract(q, qS, qDiff, 4);
|
||||
VectorOperations<double>::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<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];
|
||||
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,
|
||||
@ -504,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
|
||||
|
||||
@ -1,11 +1,19 @@
|
||||
#ifndef GUIDANCE_H_
|
||||
#define 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 <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,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";
|
||||
|
||||
@ -21,9 +21,8 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
|
||||
return acs::ControlModeStrategy::PTGCTRL_STR;
|
||||
} else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
|
||||
return acs::ControlModeStrategy::PTGCTRL_QUEST;
|
||||
} else {
|
||||
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
|
||||
}
|
||||
return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
@ -40,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};
|
||||
|
||||
@ -112,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)};
|
||||
@ -221,6 +224,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
rwCmdSpeeds[i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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:
|
||||
|
||||
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 747ad34eec5baa5199de49a1330687508c991550
|
||||
Subproject commit d33013ed58131a69d09145af4f3c7a71766cebd3
|
||||
Loading…
Reference in New Issue
Block a user