diff --git a/CHANGELOG.md b/CHANGELOG.md index 96f9c5f9..5078b7e2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -52,6 +52,14 @@ will consitute of a breaking change warranting a new major release: - Added `I2C_REBOOT` and `PDEC_REBOOT` events for EIVE system component to ensure ground gets informed. +## ACS + +- Commanding from ACS Controller is now enabled. +- Safe Controller was reverted to FLP Design. This also introduces safe mode strategies. + They contain what the controller does and which data it uses. The controller will + automatically based on the available data decide on which strategy to use. If a strategy + is undesirable (e.g. the MEKF should not be used) this can be handeld via setting parameters. + # [v1.44.1] 2023-04-12 - eive-tmtc: v2.22.1 diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index f09c8c6d..4c1fdb55 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 289 translations. + * @brief Auto-generated event translation file. Contains 290 translations. * @details - * Generated on: 2023-04-14 20:05:50 + * Generated on: 2023-04-14 20:23:17 */ #include "translateEvents.h" @@ -97,6 +97,7 @@ const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY"; +const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET"; const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; @@ -482,8 +483,10 @@ const char *translateEvents(Event event) { case (11204): return MEKF_RECOVERY_STRING; case (11205): - return MEKF_INVALID_MODE_VIOLATION_STRING; + return MEKF_AUTOMATIC_RESET_STRING; case (11206): + return MEKF_INVALID_MODE_VIOLATION_STRING; + case (11207): return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11300): return SWITCH_CMD_SENT_STRING; diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 30ff46f9..b89eba11 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 171 translations. - * Generated on: 2023-04-14 20:05:50 + * Generated on: 2023-04-14 20:23:17 */ #include "translateObjects.h" diff --git a/common/config/eive/resultClassIds.h b/common/config/eive/resultClassIds.h index 9bad9ae3..9e2a3f2f 100644 --- a/common/config/eive/resultClassIds.h +++ b/common/config/eive/resultClassIds.h @@ -37,9 +37,6 @@ enum commonClassIds : uint8_t { SUPV_RETURN_VALUES_IF, // SPVRTVIF ACS_CTRL, // ACSCTRL ACS_MEKF, // ACSMEKF - ACS_SAFE, // ACSSAF - ACS_PTG, // ACSPTG - ACS_DETUMBLE, // ACSDTB SD_CARD_MANAGER, // SDMA LOCAL_PARAM_HANDLER, // LPH PERSISTENT_TM_STORE, // PTM diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index 332a38a3..54233239 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -86,13 +86,14 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 10802;0x2a32;SERIALIZATION_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h 10803;0x2a33;FILESTORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h 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 -11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acs/defs.h -11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acs/defs.h -11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acs/defs.h -11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acs/defs.h -11204;0x2bc4;MEKF_RECOVERY;INFO;No description;mission/acs/defs.h -11205;0x2bc5;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acs/defs.h -11206;0x2bc6;SAFE_MODE_CONTROLLER_FAILURE;HIGH;No description;mission/acs/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 +11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;Multiple RWs are invalid, uncommandable and therefore higher ACS modes cannot be maintained.;mission/acs/defs.h +11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h +11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h +11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h +11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h +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 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 @@ -263,9 +264,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;mission/sysDefs.h 14007;0x36b7;REBOOT_COUNTER;INFO;Total reboot counter, which is the sum of the boot count of all individual images.;mission/sysDefs.h 14008;0x36b8;INDIVIDUAL_BOOT_COUNTS;INFO;Get the boot count of the individual images. P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1. P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1.;mission/sysDefs.h -14010;0x36ba;TRYING_I2C_RECOVERY;MEDIUM;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h -14011;0x36bb;I2C_REBOOT;MEDIUM;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h -14012;0x36bc;PDEC_REBOOT;MEDIUM;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h +14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h +14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h +14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index 5b8cb74b..50a86ae3 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -500,7 +500,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h 0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h 0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/SolarArrayDeploymentHandler.h -0x6900;ACSCTRL_FileDeletionFailed;No description;0;ACS_CTRL;mission/controller/AcsController.h +0x6900;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;0;ACS_CTRL;mission/controller/AcsController.h 0x6a02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h @@ -509,9 +509,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a07;ACSMEKF_MekfNotFinite;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a08;ACSMEKF_MekfInitialized;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a09;ACSMEKF_MekfRunning;No description;9;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h -0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h -0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h -0x7000;PTM_DumpDone;No description;0;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h -0x7001;PTM_BusyDumping;No description;1;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h -0x7100;TMS_IsBusy;No description;0;TM_SINK;mission/tmtc/DirectTmSinkIF.h +0x6d00;PTM_DumpDone;No description;0;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h +0x6d01;PTM_BusyDumping;No description;1;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h +0x6e00;TMS_IsBusy;No description;0;TM_SINK;mission/tmtc/DirectTmSinkIF.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index 332a38a3..54233239 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -86,13 +86,14 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 10802;0x2a32;SERIALIZATION_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h 10803;0x2a33;FILESTORE_ERROR;LOW;No description;fsfw/src/fsfw/cfdp/handler/defs.h 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 -11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acs/defs.h -11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acs/defs.h -11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acs/defs.h -11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acs/defs.h -11204;0x2bc4;MEKF_RECOVERY;INFO;No description;mission/acs/defs.h -11205;0x2bc5;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acs/defs.h -11206;0x2bc6;SAFE_MODE_CONTROLLER_FAILURE;HIGH;No description;mission/acs/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 +11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;Multiple RWs are invalid, uncommandable and therefore higher ACS modes cannot be maintained.;mission/acs/defs.h +11203;0x2bc3;MEKF_INVALID_INFO;INFO;MEKF was not able to compute a solution. P1: MEKF state on exit;mission/acs/defs.h +11204;0x2bc4;MEKF_RECOVERY;INFO;MEKF is able to compute a solution again.;mission/acs/defs.h +11205;0x2bc5;MEKF_AUTOMATIC_RESET;INFO;MEKF performed an automatic reset after detection of nonfinite values.;mission/acs/defs.h +11206;0x2bc6;MEKF_INVALID_MODE_VIOLATION;HIGH;MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.;mission/acs/defs.h +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 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 @@ -263,9 +264,9 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 14006;0x36b6;CURRENT_IMAGE_INFO;INFO;P1: Current Chip, P2: Current Copy;mission/sysDefs.h 14007;0x36b7;REBOOT_COUNTER;INFO;Total reboot counter, which is the sum of the boot count of all individual images.;mission/sysDefs.h 14008;0x36b8;INDIVIDUAL_BOOT_COUNTS;INFO;Get the boot count of the individual images. P1: First 16 bits boot count of image 0 0, last 16 bits boot count of image 0 1. P2: First 16 bits boot count of image 1 0, last 16 bits boot count of image 1 1.;mission/sysDefs.h -14010;0x36ba;TRYING_I2C_RECOVERY;MEDIUM;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h -14011;0x36bb;I2C_REBOOT;MEDIUM;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h -14012;0x36bc;PDEC_REBOOT;MEDIUM;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h +14010;0x36ba;TRYING_I2C_RECOVERY;HIGH;I2C is unavailable. Trying recovery of I2C bus by power cycling all I2C devices.;mission/sysDefs.h +14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h +14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h 14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h 14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h 14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index e450cf02..20b76df6 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -583,7 +583,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x68b5;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 0x68c0;SPVRTVIF_BufTooSmall;No description;192;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h 0x68c1;SPVRTVIF_NoReplyTimeout;No description;193;SUPV_RETURN_VALUES_IF;linux/payload/plocSupvDefs.h -0x6900;ACSCTRL_FileDeletionFailed;No description;0;ACS_CTRL;mission/controller/AcsController.h +0x6900;ACSCTRL_FileDeletionFailed;File deletion failed and at least one file is still existent.;0;ACS_CTRL;mission/controller/AcsController.h 0x6a02;ACSMEKF_MekfUninitialized;No description;2;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a03;ACSMEKF_MekfNoGyrData;No description;3;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h @@ -592,21 +592,18 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a07;ACSMEKF_MekfNotFinite;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a08;ACSMEKF_MekfInitialized;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a09;ACSMEKF_MekfRunning;No description;9;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h -0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h -0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h -0x6e00;SDMA_OpOngoing;No description;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e01;SDMA_AlreadyOn;No description;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e02;SDMA_AlreadyMounted;No description;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e03;SDMA_AlreadyOff;No description;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e0a;SDMA_StatusFileNexists;No description;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e0b;SDMA_StatusFileFormatInvalid;No description;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e0c;SDMA_MountError;No description;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e0d;SDMA_UnmountError;No description;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e0e;SDMA_SystemCallError;No description;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6e0f;SDMA_PopenCallError;No description;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h -0x6f00;LPH_SdNotReady;No description;0;LOCAL_PARAM_HANDLER;bsp_q7s/memory/LocalParameterHandler.h -0x7000;PTM_DumpDone;No description;0;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h -0x7001;PTM_BusyDumping;No description;1;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h -0x7100;TMS_IsBusy;No description;0;TM_SINK;mission/tmtc/DirectTmSinkIF.h -0x7300;SCBU_KeyNotFound;No description;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h +0x6b00;SDMA_OpOngoing;No description;0;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b01;SDMA_AlreadyOn;No description;1;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b02;SDMA_AlreadyMounted;No description;2;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b03;SDMA_AlreadyOff;No description;3;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b0a;SDMA_StatusFileNexists;No description;10;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b0b;SDMA_StatusFileFormatInvalid;No description;11;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b0c;SDMA_MountError;No description;12;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b0d;SDMA_UnmountError;No description;13;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b0e;SDMA_SystemCallError;No description;14;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6b0f;SDMA_PopenCallError;No description;15;SD_CARD_MANAGER;bsp_q7s/fs/SdCardManager.h +0x6c00;LPH_SdNotReady;No description;0;LOCAL_PARAM_HANDLER;bsp_q7s/memory/LocalParameterHandler.h +0x6d00;PTM_DumpDone;No description;0;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h +0x6d01;PTM_BusyDumping;No description;1;PERSISTENT_TM_STORE;mission/tmtc/PersistentTmStore.h +0x6e00;TMS_IsBusy;No description;0;TM_SINK;mission/tmtc/DirectTmSinkIF.h +0x7000;SCBU_KeyNotFound;No description;0;SCRATCH_BUFFER;bsp_q7s/memory/scratchApi.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index f09c8c6d..4c1fdb55 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 289 translations. + * @brief Auto-generated event translation file. Contains 290 translations. * @details - * Generated on: 2023-04-14 20:05:50 + * Generated on: 2023-04-14 20:23:17 */ #include "translateEvents.h" @@ -97,6 +97,7 @@ const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY"; +const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET"; const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; @@ -482,8 +483,10 @@ const char *translateEvents(Event event) { case (11204): return MEKF_RECOVERY_STRING; case (11205): - return MEKF_INVALID_MODE_VIOLATION_STRING; + return MEKF_AUTOMATIC_RESET_STRING; case (11206): + return MEKF_INVALID_MODE_VIOLATION_STRING; + case (11207): return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11300): return SWITCH_CMD_SENT_STRING; diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index dbd6cc6e..bb0ef28a 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 175 translations. - * Generated on: 2023-04-14 20:05:50 + * Generated on: 2023-04-14 20:23:17 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index f09c8c6d..4c1fdb55 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 289 translations. + * @brief Auto-generated event translation file. Contains 290 translations. * @details - * Generated on: 2023-04-14 20:05:50 + * Generated on: 2023-04-14 20:23:17 */ #include "translateEvents.h" @@ -97,6 +97,7 @@ const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY"; const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID"; const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO"; const char *MEKF_RECOVERY_STRING = "MEKF_RECOVERY"; +const char *MEKF_AUTOMATIC_RESET_STRING = "MEKF_AUTOMATIC_RESET"; const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION"; const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE"; const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT"; @@ -482,8 +483,10 @@ const char *translateEvents(Event event) { case (11204): return MEKF_RECOVERY_STRING; case (11205): - return MEKF_INVALID_MODE_VIOLATION_STRING; + return MEKF_AUTOMATIC_RESET_STRING; case (11206): + return MEKF_INVALID_MODE_VIOLATION_STRING; + case (11207): return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11300): return SWITCH_CMD_SENT_STRING; diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index dbd6cc6e..bb0ef28a 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 175 translations. - * Generated on: 2023-04-14 20:05:50 + * Generated on: 2023-04-14 20:23:17 */ #include "translateObjects.h" diff --git a/mission/acs/defs.h b/mission/acs/defs.h index bfa37cdc..7748562d 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -8,7 +8,7 @@ namespace acs { enum class SimpleSensorMode { NORMAL = 0, OFF = 1 }; -// These modes are the submodes of the ACS controller and the modes of the ACS subsystem. +// These modes are the modes of the ACS controller and of the ACS subsystem. enum AcsMode : Mode_t { OFF = HasModesIF::MODE_OFF, SAFE = 10, @@ -21,23 +21,40 @@ enum AcsMode : Mode_t { enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; -// static constexpr uint8_t ACS_SYSTEM_DETUMBLE_SUBMODE = 1; +enum SafeModeStrategy : uint8_t { + SAFECTRL_OFF = 0, + SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, + SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, + SAFECTRL_ACTIVE_MEKF = 10, + SAFECTRL_WITHOUT_MEKF = 11, + SAFECTRL_ECLIPSE_DAMPING = 12, + SAFECTRL_ECLIPSE_IDELING = 13, + SAFECTRL_DETUMBLE_FULL = 20, + SAFECTRL_DETUMBLE_DETERIORATED = 21, +}; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; -//!< The limits for the rotation in safe mode were violated. -static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); -//!< The system has recovered from a safe rate rotation violation. +//! [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); -//!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained. +//! [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); -//!< MEKF was not able to compute a solution. +//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution. +//! P1: MEKF state on exit static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO); -//!< MEKF is able to compute a solution again. +//! [EXPORT] : [COMMENT] MEKF is able to compute a solution again. static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO); -//!< MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time. -static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(5, severity::HIGH); -//!< The ACS safe mode controller was not able to compute a solution and has failed. -static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(6, severity::HIGH); +//! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values. +static constexpr Event MEKF_AUTOMATIC_RESET = MAKE_EVENT(5, severity::INFO); +//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution during any pointing ACS mode for a +//! prolonged time. +static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH); +//! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has +//! failed. +//! P1: Missing information about magnetic field, P2: Missing information about rotational rate +static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ff00d093..0b49de04 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -151,49 +151,60 @@ void AcsController::performSafe() { if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO); + triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); mekfInvalidFlag = true; } if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { + triggerEvent(acs::MEKF_AUTOMATIC_RESET); navigation.resetMekf(&mekfData); mekfLost = true; } - } else { + } else if (mekfInvalidFlag) { + triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } - // get desired satellite rate and sun direction to align - double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; - guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); - // if MEKF is working + + // get desired satellite rate, sun direction to align to and inertia + double sunTargetDir[3] = {0, 0, 0}; + guidance.getTargetParamsSafe(sunTargetDir); + double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - if (result == MultiplicativeKalmanFilter::MEKF_RUNNING) { - result = safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(), - mgmDataProcessed.magIgrfModel.value, - mgmDataProcessed.magIgrfModel.isValid(), - susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(), - mekfData.satRotRateMekf.value, mekfData.satRotRateMekf.isValid(), - sunTargetDir, satRateSafe, &errAng, magMomMtq); - } else { - result = safeCtrl.safeNoMekf( - now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(), - susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(), - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), - mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), - sunTargetDir, satRateSafe, &errAng, magMomMtq); - } - if (result == returnvalue::FAILED) { - if (not safeCtrlFailureFlag) { - triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE); - safeCtrlFailureFlag = true; - } - safeCtrlFailureCounter++; - if (safeCtrlFailureCounter > 150) { + uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( + mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, + gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), + acsParameters.safeModeControllerParameters.useMekf, + acsParameters.safeModeControllerParameters.dampingDuringEclipse); + switch (safeCtrlStrat) { + case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): + safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, + susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, + magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; - } - } else { - safeCtrlFailureFlag = false; - safeCtrlFailureCounter = 0; + break; + case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF): + safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): + safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, + sunTargetDir, magMomMtq, errAng); + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): + errAng = NAN; + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; + case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + safeCtrlFailure(1, 0); + break; + case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + safeCtrlFailure(0, 1); + break; } actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, @@ -201,7 +212,7 @@ void AcsController::performSafe() { acsParameters.magnetorquerParameter.dipolMax); // detumble check and switch - if (mekfData.satRotRateMekf.isValid() && + if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; @@ -219,10 +230,10 @@ void AcsController::performSafe() { startTransition(mode, acs::SafeSubmode::DETUMBLE); } - updateCtrlValData(errAng); + updateCtrlValData(errAng, safeCtrlStrat); updateActuatorCmdData(cmdDipolMtqs); - // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], - // acsParameters.magnetorquerParameter.torqueDuration); + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquerParameter.torqueDuration); } void AcsController::performDetumble() { @@ -236,20 +247,40 @@ void AcsController::performDetumble() { if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO); + triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); mekfInvalidFlag = true; } if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { + triggerEvent(acs::MEKF_AUTOMATIC_RESET); navigation.resetMekf(&mekfData); + mekfLost = true; } - } else { + } else if (mekfInvalidFlag) { + triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } + uint8_t safeCtrlStrat = detumble.detumbleStrategy( + mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), + mgmDataProcessed.mgmVecTotDerivative.isValid(), + acsParameters.detumbleParameter.useFullDetumbleLaw); double magMomMtq[3] = {0, 0, 0}; - detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, - mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), magMomMtq, - acsParameters.detumbleParameter.gainD); + switch (safeCtrlStrat) { + case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL): + detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, + magMomMtq, acsParameters.detumbleParameter.gainFull); + break; + case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): + detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, + magMomMtq, acsParameters.detumbleParameter.gainBdot); + break; + case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + safeCtrlFailure(1, 0); + break; + case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + safeCtrlFailure(0, 1); + break; + } + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, *acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipolMax); @@ -274,9 +305,8 @@ void AcsController::performDetumble() { disableCtrlValData(); updateActuatorCmdData(cmdDipolMtqs); - // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], - // acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, - // acsParameters.rwHandlingParameters.rampTime); + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquerParameter.torqueDuration); } void AcsController::performPointingCtrl() { @@ -291,21 +321,22 @@ void AcsController::performPointingCtrl() { result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { mekfInvalidCounter++; if (not mekfInvalidFlag) { - triggerEvent(acs::MEKF_INVALID_INFO); + triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value); mekfInvalidFlag = true; } if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { + triggerEvent(acs::MEKF_AUTOMATIC_RESET); navigation.resetMekf(&mekfData); + mekfLost = true; } if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) { // Trigger this so STR FDIR can set the device faulty. EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0); mekfInvalidCounter = 0; } - // commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, - // cmdSpeedRws[0], - // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - // acsParameters.rwHandlingParameters.rampTime); + commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); return; } else { if (mekfInvalidFlag) { @@ -465,10 +496,22 @@ void AcsController::performPointingCtrl() { updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); - // commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], - // acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], - // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - // acsParameters.rwHandlingParameters.rampTime); + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); +} + +void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) { + if (not safeCtrlFailureFlag) { + triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure); + safeCtrlFailureFlag = true; + } + safeCtrlFailureCounter++; + if (safeCtrlFailureCounter > 150) { + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + } } ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, @@ -529,17 +572,19 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque, } } -void AcsController::updateCtrlValData(double errAng) { +void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double)); + std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double)); ctrlValData.tgtQuat.setValid(false); - std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double)); ctrlValData.errQuat.setValid(false); ctrlValData.errAng.value = errAng; ctrlValData.errAng.setValid(true); - std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double)); ctrlValData.tgtRotRate.setValid(false); + ctrlValData.safeStrat.value = safeModeStrat; + ctrlValData.safeStrat.setValid(true); ctrlValData.setValidity(true, false); } } @@ -552,6 +597,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng; std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); + ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF; ctrlValData.setValidity(true, true); } } @@ -559,10 +605,10 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu void AcsController::disableCtrlValData() { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double)); - std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double)); + std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double)); ctrlValData.errAng.value = 0; - std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double)); ctrlValData.setValidity(false, true); } } @@ -644,6 +690,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus); poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0}); // Ctrl Values + localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat); localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 44e87e7e..6ec26c57 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -12,18 +12,17 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include "acs/ActuatorCmd.h" -#include "acs/Guidance.h" -#include "acs/MultiplicativeKalmanFilter.h" -#include "acs/Navigation.h" -#include "acs/SensorProcessing.h" -#include "acs/control/Detumble.h" -#include "acs/control/PtgCtrl.h" -#include "acs/control/SafeCtrl.h" -#include "controllerdefinitions/AcsCtrlDefinitions.h" - class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; @@ -41,9 +40,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void performPointingCtrl(); private: - static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1}; - static constexpr double ZERO_VEC[3] = {0, 0, 0}; - static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.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}; static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0}; bool enableHkSets = false; @@ -85,6 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2; 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); ReturnValue_t initialize() override; @@ -105,6 +105,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void modeChanged(Mode_t mode, Submode_t submode); void announceMode(bool recursive); + void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure); + ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration); ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, @@ -113,7 +115,7 @@ 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(double errAng); + void updateCtrlValData(double errAng, uint8_t safeModeStrat); void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng, const double* tgtRotRate); void disableCtrlValData(); @@ -212,6 +214,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes // Ctrl Values acsctrl::CtrlValData ctrlValData; + PoolEntry safeStrat = PoolEntry(); PoolEntry tgtQuat = PoolEntry(4); PoolEntry errQuat = PoolEntry(4); PoolEntry errAng = PoolEntry(); diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 060e0aac..03500cc3 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -33,18 +33,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x2: // InertiaEIVE switch (parameterId) { case 0x0: - parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrix); - break; - case 0x1: parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixDeployed); break; - case 0x2: + case 0x1: parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixUndeployed); break; - case 0x3: + case 0x2: parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel1); break; - case 0x4: + case 0x3: parameterWrapper->setMatrix(inertiaEIVE.inertiaMatrixPanel3); break; default: @@ -107,6 +104,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x11: parameterWrapper->setVector(mgmHandlingParameters.mgm4variance); break; + case 0x12: + parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight); + break; default: return INVALID_IDENTIFIER_ID; } @@ -260,6 +260,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0xA: parameterWrapper->set(gyrHandlingParameters.preferAdis); break; + case 0xB: + parameterWrapper->set(gyrHandlingParameters.gyrFilterWeight); + break; default: return INVALID_IDENTIFIER_ID; } @@ -321,28 +324,34 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case (0x8): // SafeModeControllerParameters switch (parameterId) { case 0x0: - parameterWrapper->set(safeModeControllerParameters.k_rate_mekf); + parameterWrapper->set(safeModeControllerParameters.k_orthoMekf); break; case 0x1: - parameterWrapper->set(safeModeControllerParameters.k_align_mekf); + parameterWrapper->set(safeModeControllerParameters.k_alignMekf); break; case 0x2: - parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf); + parameterWrapper->set(safeModeControllerParameters.k_parallelMekf); break; case 0x3: - parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf); + parameterWrapper->set(safeModeControllerParameters.k_orthoNonMekf); break; case 0x4: - parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin); + parameterWrapper->set(safeModeControllerParameters.k_alignNonMekf); break; case 0x5: - parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); + parameterWrapper->set(safeModeControllerParameters.k_parallelNonMekf); break; case 0x6: - parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); break; case 0x7: - parameterWrapper->setVector(safeModeControllerParameters.satRateRef); + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); + break; + case 0x8: + parameterWrapper->set(safeModeControllerParameters.useMekf); + break; + case 0x9: + parameterWrapper->set(safeModeControllerParameters.dampingDuringEclipse); break; default: return INVALID_IDENTIFIER_ID; @@ -377,7 +386,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x8: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; - default: return INVALID_IDENTIFIER_ID; } @@ -703,7 +711,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(detumbleParameter.omegaDetumbleEnd); break; case 0x3: - parameterWrapper->set(detumbleParameter.gainD); + parameterWrapper->set(detumbleParameter.gainBdot); + break; + case 0x4: + parameterWrapper->set(detumbleParameter.gainFull); + break; + case 0x5: + parameterWrapper->set(detumbleParameter.useFullDetumbleLaw); break; default: return INVALID_IDENTIFIER_ID; diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 5b185ea4..9600ca7e 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -22,22 +22,27 @@ class AcsParameters : public HasParametersIF { } onBoardParams; struct InertiaEIVE { - double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, - {-0.0001821456, 0.1701302, 0.0004748963}, - {-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021 - // Possible inertia matrices - double inertiaMatrixDeployed[3][3] = {{0.1539829, -0.0001821456, -0.0050135}, - {-0.0001821456, 0.1701302, 0.0004748963}, - {-0.0050135, 0.0004748963, 0.08374296}}; // 19.11.2021 - double inertiaMatrixUndeployed[3][3] = {{0.122485, -0.0001798426, -0.005008}, - {-0.0001798426, 0.162240, 0.000475596}, - {-0.005008, 0.000475596, 0.060136}}; // 19.11.2021 - double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207}, - {-0.0001836122, 0.16619787, 0.0083537}, - {-0.00501207, 0.0083537, 0.07192588}}; // 19.11.2021 - double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767}, - {-0.000178376, 0.166172, -0.007403}, - {-0.005009767, -0.007403, 0.07195314}}; + /* Possible inertia matrices + * 2023-04-14 + * all matrices derived from the CAD model with CAD mass of 8.72 kg + * all matrices are scaled by final measured mass of 8.756 kg + * all matrices are in [kg m^2] + */ + double inertiaMatrixDeployed[3][3] = { + {0.128333461640235, -0.000151805020803, -0.004178414952517}, + {-0.000151805020803, 0.141791062870050, 0.000395791231451}, + {-0.004178414952517, 0.000395791231451, 0.069793610830099}}; + double inertiaMatrixUndeployed[3][3] = { + {0.102082611845982, -0.000149885620646, -0.004174050971653}, + {-0.000149885620646, 0.135214836333048, 0.000396374487363}, + {-0.004174050971653, 0.000396374487363, 0.050118987572848}}; + double inertiaMatrixPanel1[3][3] = {{0.115207454653725, -0.000153027308288, -0.004177193002842}, + {-0.000153027308288, 0.138513727361148, 0.006962185987503}, + {-0.004177193002842, 0.006962185987503, 0.059944939352491}}; + double inertiaMatrixPanel3[3][3] = { + {0.115208618832493, -0.000148663333161, -0.004175272921328}, + {-0.000148663333161, 0.138492171841952, -0.006170020268690}, + {-0.004175272921328, -0.006170020268690, 0.059967659050454}}; } inertiaEIVE; struct MgmHandlingParameters { @@ -72,6 +77,7 @@ class AcsParameters : public HasParametersIF { float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; + float mgmDerivativeFilterWeight = 0.5; } mgmHandlingParameters; struct SusHandlingParameters { @@ -779,7 +785,8 @@ class AcsParameters : public HasParametersIF { pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; - uint8_t preferAdis = true; + uint8_t preferAdis = false; + float gyrFilterWeight = 0.6; } gyrHandlingParameters; struct RwHandlingParameters { @@ -811,18 +818,19 @@ class AcsParameters : public HasParametersIF { } rwMatrices; struct SafeModeControllerParameters { - double k_rate_mekf = 0.00059437; - double k_align_mekf = 0.000056875; + double k_orthoMekf = 4.4e-3; + double k_alignMekf = 4.0e-5; + double k_parallelMekf = 3.75e-4; - double k_rate_no_mekf = 0.00059437; - double k_align_no_mekf = 0.000056875; - - double sunMagAngleMin = 5 * M_PI / 180; + double k_orthoNonMekf = 4.4e-3; + double k_alignNonMekf = 4.0e-5; + double k_parallelNonMekf = 3.75e-4; double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)}; double sunTargetDir[3] = {0, 0, 1}; - double satRateRef[3] = {0, 0, 0}; + uint8_t useMekf = false; + uint8_t dampingDuringEclipse = true; } safeModeControllerParameters; struct PointingLawParameters { @@ -931,8 +939,10 @@ class AcsParameters : public HasParametersIF { struct DetumbleParameter { uint8_t detumblecounter = 75; // 30 s double omegaDetumbleStart = 2 * M_PI / 180; - double omegaDetumbleEnd = 0.4 * M_PI / 180; - double gainD = pow(10.0, -3.3); + double omegaDetumbleEnd = 1 * M_PI / 180; + double gainBdot = pow(10.0, -3.3); + double gainFull = pow(10.0, -2.3); + uint8_t useFullDetumbleLaw = false; } detumbleParameter; }; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 35a2f025..79c6b416 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -539,7 +539,7 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, } } -void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) { +void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) { std::error_code e; if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { @@ -549,8 +549,6 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop, 3 * sizeof(double)); } - std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef, - 3 * sizeof(double)); } ReturnValue_t Guidance::solarArrayDeploymentComplete() { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index f3369092..a52c476a 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -12,7 +12,7 @@ class Guidance { Guidance(AcsParameters *acsParameters_); virtual ~Guidance(); - void getTargetParamsSafe(double sunTargetSafe[3], double satRateRef[3]); + void getTargetParamsSafe(double sunTargetSafe[3]); ReturnValue_t solarArrayDeploymentComplete(); // Function to get the target quaternion and refence rotation rate from gps position and diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 88568573..0f297b21 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1105,11 +1105,9 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek { PoolReadGuard pg(mekfData); if (pg.getReadResult() == returnvalue::OK) { - double unitQuat[4] = {0.0, 0.0, 0.0, 1.0}; - double zeroVec[3] = {0.0, 0.0, 0.0}; - std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double)); + std::memcpy(mekfData->quatMekf.value, ZERO_VEC4, 4 * sizeof(double)); mekfData->quatMekf.setValid(false); - std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double)); + std::memcpy(mekfData->satRotRateMekf.value, ZERO_VEC3, 3 * sizeof(double)); mekfData->satRotRateMekf.setValid(false); mekfData->mekfStatus = mekfStatus; mekfData->setValidity(true, false); diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index fe749890..5a878c4b 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -80,6 +80,9 @@ class MultiplicativeKalmanFilter { static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9); private: + static constexpr double ZERO_VEC3[3] = {0, 0, 0}; + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; + /*Parameters*/ double quaternion_STR_SB[4]; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index b2812f55..c1030c7b 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -141,7 +141,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool mgmVecTotDerivativeValid = false; double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst); - if (timeOfSavedMagFieldEst.tv_sec != 0) { + if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0) { for (uint8_t i = 0; i < 3; i++) { mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff; savedMgmVecTot[i] = mgmVecTot[i]; @@ -150,6 +150,11 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const } timeOfSavedMagFieldEst = timeOfMgmMeasurement; + if (mgmDataProcessed->mgmVecTotDerivative.isValid()) { + lowPassFilter(mgmVecTotDerivative, mgmDataProcessed->mgmVecTotDerivative.value, + mgmParameters->mgmDerivativeFilterWeight); + } + { PoolReadGuard pg(mgmDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -389,7 +394,7 @@ void SensorProcessing::processSus( double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; bool susVecTotDerivativeValid = false; double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst); - if (timeOfSavedSusDirEst.tv_sec != 0) { + if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0) { for (uint8_t i = 0; i < 3; i++) { susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff; savedSusVecTot[i] = susVecTot[i]; @@ -527,6 +532,11 @@ void SensorProcessing::processGyr( gyrVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } } + + if (gyrDataProcessed->gyrVecTot.isValid()) { + lowPassFilter(gyrVecTot, gyrDataProcessed->gyrVecTot.value, gyrParameters->gyrFilterWeight); + } + { PoolReadGuard pg(gyrDataProcessed); if (pg.getReadResult() == returnvalue::OK) { @@ -571,8 +581,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // Calculation of the satellite velocity in earth fixed frame double deltaDistance[3] = {0, 0, 0}; MathOperations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE); - if (validSavedPosSatE && - (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) { + if (validSavedPosSatE and + (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and + (gpsUnixSeconds - timeOfSavedPosSatE) > 0) { VectorOperations::subtract(posSatE, savedPosSatE, deltaDistance, 3); double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE; VectorOperations::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3); @@ -656,3 +667,10 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues, sensorValues->gyr3L3gSet.angVelocZ.value, sensorValues->gyr3L3gSet.angVelocZ.isValid(), now, &acsParameters->gyrHandlingParameters, gyrDataProcessed); } + +void SensorProcessing::lowPassFilter(double *newValue, double *oldValue, const double weight) { + double leftSide[3] = {0, 0, 0}, rightSide[3] = {0, 0, 0}; + VectorOperations::mulScalar(newValue, (1 - weight), leftSide, 3); + VectorOperations::mulScalar(oldValue, weight, rightSide, 3); + VectorOperations::add(leftSide, rightSide, newValue, 3); +} diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index d845c2f3..47449a75 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -13,8 +13,6 @@ class SensorProcessing { public: - void reset(); - SensorProcessing(); virtual ~SensorProcessing(); @@ -59,13 +57,13 @@ class SensorProcessing { const AcsParameters::GyrHandlingParameters *gyrParameters, acsctrl::GyrDataProcessed *gyrDataProcessed); - void processStr(); - void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude, const double gpsUnixSeconds, const bool validGps, const AcsParameters::GpsParameters *gpsParameters, acsctrl::GpsDataProcessed *gpsDataProcessed); + void lowPassFilter(double *newValue, double *oldValue, const double weight); + double savedMgmVecTot[3] = {0.0, 0.0, 0.0}; timeval timeOfSavedMagFieldEst; double savedSusVecTot[3] = {0.0, 0.0, 0.0}; diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 893d6a4b..9ca20862 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -1,61 +1,45 @@ #include "Detumble.h" -#include -#include -#include #include -#include #include -#include "../util/MathOperations.h" - Detumble::Detumble() {} Detumble::~Detumble() {} -ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool magRateValid, - const double *magField, const bool magFieldValid, double *magMom, - double gain) { - if (!magRateValid || !magFieldValid) { - return DETUMBLE_NO_SENSORDATA; +uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + const bool magFieldRateValid, const bool useFullDetumbleLaw) { + if (not magFieldValid) { + return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + } else if (satRotRateValid and useFullDetumbleLaw) { + return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL; + } else if (magFieldRateValid) { + return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } - // convert uT to T - double magFieldT[3], magRateT[3]; - VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); - VectorOperations::mulScalar(magRate, 1e-6, magRateT, 3); - // control law - double factor = -gain / pow(VectorOperations::norm(magFieldT, 3), 2); - VectorOperations::mulScalar(magRateT, factor, magMom, 3); - return returnvalue::OK; } -ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, - double dipolMax) { - if (!magRateValid) { - return DETUMBLE_NO_SENSORDATA; - } - - for (int i = 0; i < 3; i++) { - magMom[i] = -dipolMax * sign(magRate[i]); - } - - return returnvalue::OK; -} - -ReturnValue_t Detumble::bDotLawFull(const double *satRate, const bool *satRateValid, - const double *magField, const bool *magFieldValid, - double *magMom, double gain) { - if (!satRateValid || !magFieldValid) { - return DETUMBLE_NO_SENSORDATA; - } +void Detumble::bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, + double gain) { // convert uT to T - double magFieldT[3]; - VectorOperations::mulScalar(magField, 1e-6, magFieldT, 3); + double magFieldBT[3]; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); // control law - double factor = gain / pow(VectorOperations::norm(magField, 3), 2); + double factor = gain / pow(VectorOperations::norm(magFieldBT, 3), 2); double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0}; - VectorOperations::normalize(magFieldT, magFieldNormed, 3); - VectorOperations::cross(satRate, magFieldNormed, crossProduct); - VectorOperations::mulScalar(crossProduct, factor, magMom, 3); - return returnvalue::OK; + VectorOperations::normalize(magFieldBT, magFieldNormed, 3); + VectorOperations::cross(satRotRateB, magFieldNormed, crossProduct); + VectorOperations::mulScalar(crossProduct, factor, magMomB, 3); +} + +void Detumble::bDotLaw(const double *magRateB, const double *magFieldB, double *magMomB, + double gain) { + // convert uT to T + double magFieldBT[3], magRateBT[3]; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + VectorOperations::mulScalar(magRateB, 1e-6, magRateBT, 3); + // control law + double factor = -gain / pow(VectorOperations::norm(magFieldBT, 3), 2); + VectorOperations::mulScalar(magRateBT, factor, magMomB, 3); } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 2bf3607d..4424896e 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -2,30 +2,22 @@ #define ACS_CONTROL_DETUMBLE_H_ #include +#include #include #include -#include - -#include "../AcsParameters.h" -#include "../SensorValues.h" -#include "eive/resultClassIds.h" class Detumble { public: Detumble(); virtual ~Detumble(); - static const uint8_t INTERFACE_ID = CLASS_ID::ACS_DETUMBLE; - static const ReturnValue_t DETUMBLE_NO_SENSORDATA = MAKE_RETURN_CODE(0x01); + uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + const bool magFieldRateValid, const bool useFullDetumbleLaw); - ReturnValue_t bDotLaw(const double *magRate, const bool magRateValid, const double *magField, - const bool magFieldValid, double *magMom, double gain); + void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, + double gain); - ReturnValue_t bangbangLaw(const double *magRate, const bool magRateValid, double *magMom, - double dipolMax); - - ReturnValue_t bDotLawFull(const double *satRate, const bool *satRateValid, const double *magField, - const bool *magFieldValid, double *magMom, double gain); + void bDotLaw(const double *magRateB, const double *magFieldB, double *magMomB, double gain); private: }; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 23f51f9e..1b20efb9 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -49,8 +49,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters gainMatrixDiagonal[0][0] = gainVector[0]; gainMatrixDiagonal[1][1] = gainVector[1]; gainMatrixDiagonal[2][2] = gainVector[2]; - MatrixOperations::multiply( - *gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3); + MatrixOperations::multiply(*gainMatrixDiagonal, + *(acsParameters->inertiaEIVE.inertiaMatrixDeployed), + *gainMatrix, 3, 3, 3); // Inverse of gainMatrix double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -60,7 +61,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MatrixOperations::multiply( - *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3); + *gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrixDeployed), *pMatrix, 3, 3, 3); MatrixOperations::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3); //------------------------------------------------------------------------------------------------ @@ -85,7 +86,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters // torque for rate error double torqueRate[3] = {0, 0, 0}; - MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate, + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), deltaRate, torqueRate, 3, 3, 1); VectorOperations::mulScalar(torqueRate, cInt, torqueRate, 3); VectorOperations::mulScalar(torqueRate, -1, torqueRate, 3); @@ -116,7 +117,7 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU, momentumRw, 3, 4, 1); double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate, + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, momentumSat, 3, 3, 1); VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); // calculating momentum error diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index f6c6c345..fad72e6b 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -24,9 +24,6 @@ class PtgCtrl { PtgCtrl(AcsParameters *acsParameters_); virtual ~PtgCtrl(); - static const uint8_t INTERFACE_ID = CLASS_ID::ACS_PTG; - static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - /* @brief: Calculates the needed torque for the pointing control mechanism */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 392e32ba..ce50f276 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -1,158 +1,136 @@ #include "SafeCtrl.h" -#include #include #include #include #include -#include "../util/MathOperations.h" - SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } SafeCtrl::~SafeCtrl() {} -ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, - double *magFieldModel, bool magFieldModelValid, - double *sunDirModel, bool sunDirModelValid, double *satRateMekf, - bool rateMekfValid, double *sunDirRef, double *satRatRef, - double *outputAngle, double *outputMagMomB) { - if (!quatBJValid || !magFieldModelValid || !sunDirModelValid || !rateMekfValid) { - return SAFECTRL_MEKF_INPUT_INVALID; +uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, const uint8_t dampingEnabled) { + if (not magFieldValid) { + return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + } else if (mekfEnabled and mekfValid) { + return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF; + } else if (satRotRateValid and sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF; + } else if (dampingEnabled and satRotRateValid and not sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING; + } else if (not dampingEnabled and satRotRateValid and not sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } - - double kRate = acsParameters->safeModeControllerParameters.k_rate_mekf; - double kAlign = acsParameters->safeModeControllerParameters.k_align_mekf; - - // Calc sunDirB ,magFieldB with mekf output and model - double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::dcmFromQuat(quatBJ, *dcmBJ); - double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0}; - MatrixOperations::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1); - MatrixOperations::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1); - - // change unit from uT to T - VectorOperations::mulScalar(magFieldB, 1e-6, magFieldB, 3); - - double crossSun[3] = {0, 0, 0}; - VectorOperations::cross(sunDirRef, sunDirB, crossSun); - double normCrossSun = VectorOperations::norm(crossSun, 3); - - // calc angle alpha between sunDirRef and sunDIr - double dotSun = VectorOperations::dot(sunDirRef, sunDirB); - double alpha = acos(dotSun); - - // Law Torque calculations - double torqueCmd[3] = {0, 0, 0}, torqueAlign[3] = {0, 0, 0}, torqueRate[3] = {0, 0, 0}, - torqueAll[3] = {0, 0, 0}; - - double scalarFac = kAlign * alpha / normCrossSun; - VectorOperations::mulScalar(crossSun, scalarFac, torqueAlign, 3); - - double rateSafeMode[3] = {0, 0, 0}; - VectorOperations::subtract(satRateMekf, satRatRef, rateSafeMode, 3); - VectorOperations::mulScalar(rateSafeMode, -kRate, torqueRate, 3); - - VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); - // Adding factor of inertia for axes - MatrixOperations::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10, - *gainMatrixInertia, 3, - 3); // why only for mekf one and not for no mekf - MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); - - // MagMom B (orthogonal torque) - double torqueMgt[3] = {0, 0, 0}; - VectorOperations::cross(magFieldB, torqueCmd, torqueMgt); - double normMag = VectorOperations::norm(magFieldB, 3); - VectorOperations::mulScalar(torqueMgt, 1 / pow(normMag, 2), outputMagMomB, 3); - - *outputAngle = alpha; - return returnvalue::OK; } -// Will be the version in worst case scenario in event of no working MEKF (nor GYRs) -ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, - double *sunRateB, bool sunRateBValid, double *magFieldB, - bool magFieldBValid, double *magRateB, bool magRateBValid, - double *sunDirRef, double *satRateRef, double *outputAngle, - double *outputMagMomB) { - // Check for invalid Inputs - if (!susDirBValid || !magFieldBValid || !magRateBValid) { - return returnvalue::FAILED; - } - - // change unit from uT to T - double magFieldBT[3] = {0, 0, 0}; +void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, + const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, + double *magMomB, double &errorAngle) { + // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); - // normalize sunDir and magDir - double magDirB[3] = {0, 0, 0}; - VectorOperations::normalize(magFieldBT, magDirB, 3); - VectorOperations::normalize(susDirB, susDirB, 3); + // convert sunDirModel to body rf + double sunDirB[3] = {0, 0, 0}; + QuaternionOperations::multiplyVector(quatBI, sunDirModelI, sunDirB); - // Cosinus angle between sunDir and magDir - double cosAngleSunMag = VectorOperations::dot(magDirB, susDirB); + // calculate angle alpha between sunDirRef and sunDir + double dotSun = VectorOperations::dot(sunDirRefB, sunDirB); + errorAngle = acos(dotSun); - // Rate parallel to sun direction and magnetic field direction - double dotSunRateMag = VectorOperations::dot(sunRateB, magDirB); - double dotmagRateSun = VectorOperations::dot(magRateB, susDirB); - double rateFactor = 1 - pow(cosAngleSunMag, 2); - double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor; - double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor; + splitRotationalRate(satRotRateB, sunDirB); + calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, + acsParameters->safeModeControllerParameters.k_parallelMekf, + acsParameters->safeModeControllerParameters.k_orthoMekf); + calculateAngleErrorTorque(sunDirB, sunDirRefB, + acsParameters->safeModeControllerParameters.k_alignMekf); - // Full rate or estimate - double estSatRate[3] = {0, 0, 0}; - double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0}; - VectorOperations::mulScalar(susDirB, rateParaSun, estSatRateSun, 3); - VectorOperations::add(sunRateB, estSatRateSun, estSatRateSun, 3); - VectorOperations::mulScalar(magDirB, rateParaMag, estSatRateMag, 3); - VectorOperations::add(magRateB, estSatRateMag, estSatRateMag, 3); - VectorOperations::add(estSatRateSun, estSatRateMag, estSatRate, 3); - VectorOperations::mulScalar(estSatRate, 0.5, estSatRate, 3); - - /* Only valid if angle between sun direction and magnetic field direction - * is sufficiently large */ - double angleSunMag = acos(cosAngleSunMag); - if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) { - return returnvalue::FAILED; + // sum of all torques + for (uint8_t i = 0; i < 3; i++) { + cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i]; } - // Rate for Torque Calculation - double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */ - VectorOperations::subtract(estSatRate, satRateRef, diffRate, 3); - - // Torque Align calculation - double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf; - double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf; - - double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); - double crossSusSunRef[3] = {0, 0, 0}; - VectorOperations::cross(sunDirRef, susDirB, crossSusSunRef); - double sinAngleAlignErr = VectorOperations::norm(crossSusSunRef, 3); - - double torqueAlign[3] = {0, 0, 0}; - double angleAlignErr = acos(cosAngleAlignErr); - double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr; - VectorOperations::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3); - - // Torque Rate Calculations - double torqueRate[3] = {0, 0, 0}; - VectorOperations::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3); - - // Final torque - double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; - VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); - MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate, - torqueB, 3, 3, 1); - - // Magnetic moment - double magMomB[3] = {0, 0, 0}; - double crossMagFieldTorque[3] = {0, 0, 0}; - VectorOperations::cross(magFieldBT, torqueB, crossMagFieldTorque); - double magMomFactor = pow(VectorOperations::norm(magFieldBT, 3), 2); - VectorOperations::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); - - std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); - *outputAngle = angleAlignErr; - return returnvalue::OK; + calculateMagneticMoment(magMomB); +} + +void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, + const double *sunDirB, const double *sunDirRefB, double *magMomB, + double &errorAngle) { + // convert magFieldB from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // calculate error angle between sunDirRef and sunDir + double dotSun = VectorOperations::dot(sunDirRefB, sunDirB); + errorAngle = acos(dotSun); + + splitRotationalRate(satRotRateB, sunDirB); + calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, + acsParameters->safeModeControllerParameters.k_parallelNonMekf, + acsParameters->safeModeControllerParameters.k_orthoNonMekf); + calculateAngleErrorTorque(sunDirB, sunDirRefB, + acsParameters->safeModeControllerParameters.k_alignNonMekf); + + // sum of all torques + for (uint8_t i = 0; i < 3; i++) { + cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i]; + } + + calculateMagneticMoment(magMomB); +} + +void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { + // convert magFieldB from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // no error angle available for eclipse + errorAngle = NAN; + + splitRotationalRate(satRotRateB, sunDirRefB); + calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, + acsParameters->safeModeControllerParameters.k_parallelNonMekf, + acsParameters->safeModeControllerParameters.k_orthoNonMekf); + + // sum of all torques + double cmdTorque[3] = {0, 0, 0}; + VectorOperations::add(cmdParallel, cmdOrtho, cmdTorque, 3); + + // calculate magnetic moment to command + calculateMagneticMoment(magMomB); +} + +void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunDirB) { + // split rotational rate into parallel and orthogonal parts + double parallelLength = VectorOperations::dot(satRotRateB, sunDirB) * + pow(VectorOperations::norm(sunDirB, 3), -2); + VectorOperations::mulScalar(sunDirB, parallelLength, satRotRateParallelB, 3); + VectorOperations::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); +} + +void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, + double &errorAngle, const double gainParallel, + const double gainOrtho) { + // calculate torque for parallel rotational rate + VectorOperations::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); + + // calculate torque for orthogonal rotational rate + VectorOperations::mulScalar(satRotRateOrthogonalB, -gainOrtho, cmdOrtho, 3); +} + +void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, + const double gainAlign) { + // calculate torque for alignment + double crossAlign[3] = {0, 0, 0}; + VectorOperations::cross(sunDirRefB, sunDirB, crossAlign); + VectorOperations::mulScalar(crossAlign, gainAlign, cmdAlign, 3); +} + +void SafeCtrl::calculateMagneticMoment(double *magMomB) { + double torqueMgt[3] = {0, 0, 0}; + VectorOperations::cross(magFieldBT, cmdTorque, torqueMgt); + double normMag = VectorOperations::norm(magFieldBT, 3); + VectorOperations::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3); } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index ac5a754a..91625360 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -1,40 +1,52 @@ #ifndef SAFECTRL_H_ #define SAFECTRL_H_ +#include +#include +#include #include #include -#include - -#include "../AcsParameters.h" -#include "../SensorValues.h" -#include "eive/resultClassIds.h" class SafeCtrl { public: SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE; - static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); + uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, const uint8_t dampingEnabled); - ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, - bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, - double *satRateMekf, bool rateMekfValid, double *sunDirRef, - double *satRatRef, // From Guidance (!) - double *outputAngle, double *outputMagMomB); + void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, + const double *quatBI, const double *sunDirRefB, double *magMomB, + double &errorAngle); - ReturnValue_t safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB, - bool sunRateBValid, double *magFieldB, bool magFieldBValid, - double *magRateB, bool magRateBValid, double *sunDirRef, - double *satRateRef, double *outputAngle, double *outputMagMomB); + void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle); + + void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, + double *magMomB, double &errorAngle); + + void splitRotationalRate(const double *satRotRateB, const double *sunDirB); + + void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, + double &errorAngle, const double gainParallel, + const double gainOrtho); + + void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, + const double gainAlign); + + void calculateMagneticMoment(double *magMomB); protected: private: AcsParameters *acsParameters; - double gainMatrixInertia[3][3]; - - double magFieldBState[3]; - timeval magFieldBStateTime; + double magFieldBT[3] = {0, 0, 0}; + double satRotRateParallelB[3] = {0, 0, 0}; + double satRotRateOrthogonalB[3] = {0, 0, 0}; + double cmdParallel[3] = {0, 0, 0}; + double cmdOrtho[3] = {0, 0, 0}; + double cmdAlign[3] = {0, 0, 0}; + double cmdTorque[3] = {0, 0, 0}; }; #endif /* ACS_CONTROL_SAFECTRL_H_ */ diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index f82e75f1..c8dbd275 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -94,6 +94,7 @@ enum PoolIds : lp_id_t { QUAT_MEKF, MEKF_STATUS, // Ctrl Values + SAFE_STRAT, TGT_QUAT, ERROR_QUAT, ERROR_ANG, @@ -112,7 +113,7 @@ static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4; static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t MEKF_SET_ENTRIES = 3; -static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4; +static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; /** @@ -251,6 +252,7 @@ class CtrlValData : public StaticLocalDataSet { public: CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {} + lp_var_t safeStrat = lp_var_t(sid.objectId, SAFE_STRAT, this); lp_vec_t tgtQuat = lp_vec_t(sid.objectId, TGT_QUAT, this); lp_vec_t errQuat = lp_vec_t(sid.objectId, ERROR_QUAT, this); lp_var_t errAng = lp_var_t(sid.objectId, ERROR_ANG, this); diff --git a/tmtc b/tmtc index 1f06ea45..a07f2132 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1f06ea4590d6340990fb8d9534a84e7e9de68951 +Subproject commit a07f21329adcffa963d75769d523b3df34222543