Merge pull request 'Revert ACS Safe Mode Controller to FLP Design' (#466) from acs-flp-safe into develop
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Reviewed-on: #466 Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
commit
aa54dbbd10
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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"
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
@ -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;
|
||||
|
||||
@ -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"
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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"
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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<double>::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);
|
||||
|
||||
@ -12,18 +12,17 @@
|
||||
#include <mission/acs/imtqHelpers.h>
|
||||
#include <mission/acs/rwHelpers.h>
|
||||
#include <mission/acs/susMax1227Helpers.h>
|
||||
#include <mission/controller/acs/ActuatorCmd.h>
|
||||
#include <mission/controller/acs/Guidance.h>
|
||||
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
|
||||
#include <mission/controller/acs/Navigation.h>
|
||||
#include <mission/controller/acs/SensorProcessing.h>
|
||||
#include <mission/controller/acs/control/Detumble.h>
|
||||
#include <mission/controller/acs/control/PtgCtrl.h>
|
||||
#include <mission/controller/acs/control/SafeCtrl.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
#include <mission/utility/trace.h>
|
||||
|
||||
#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<uint8_t> safeStrat = PoolEntry<uint8_t>();
|
||||
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
|
||||
PoolEntry<double> errQuat = PoolEntry<double>(4);
|
||||
PoolEntry<double> errAng = PoolEntry<double>();
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
|
||||
@ -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() {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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];
|
||||
|
||||
|
||||
@ -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<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
|
||||
if (validSavedPosSatE &&
|
||||
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
|
||||
if (validSavedPosSatE and
|
||||
(gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax) and
|
||||
(gpsUnixSeconds - timeOfSavedPosSatE) > 0) {
|
||||
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
|
||||
double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
|
||||
VectorOperations<double>::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<double>::mulScalar(newValue, (1 - weight), leftSide, 3);
|
||||
VectorOperations<double>::mulScalar(oldValue, weight, rightSide, 3);
|
||||
VectorOperations<double>::add(leftSide, rightSide, newValue, 3);
|
||||
}
|
||||
|
||||
@ -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};
|
||||
|
||||
@ -1,61 +1,45 @@
|
||||
#include "Detumble.h"
|
||||
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/sign.h>
|
||||
#include <math.h>
|
||||
|
||||
#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<double>::mulScalar(magField, 1e-6, magFieldT, 3);
|
||||
VectorOperations<double>::mulScalar(magRate, 1e-6, magRateT, 3);
|
||||
// control law
|
||||
double factor = -gain / pow(VectorOperations<double>::norm(magFieldT, 3), 2);
|
||||
VectorOperations<double>::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<double>::mulScalar(magField, 1e-6, magFieldT, 3);
|
||||
double magFieldBT[3];
|
||||
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
||||
// control law
|
||||
double factor = gain / pow(VectorOperations<double>::norm(magField, 3), 2);
|
||||
double factor = gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
|
||||
double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(magFieldT, magFieldNormed, 3);
|
||||
VectorOperations<double>::cross(satRate, magFieldNormed, crossProduct);
|
||||
VectorOperations<double>::mulScalar(crossProduct, factor, magMom, 3);
|
||||
return returnvalue::OK;
|
||||
VectorOperations<double>::normalize(magFieldBT, magFieldNormed, 3);
|
||||
VectorOperations<double>::cross(satRotRateB, magFieldNormed, crossProduct);
|
||||
VectorOperations<double>::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<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
||||
VectorOperations<double>::mulScalar(magRateB, 1e-6, magRateBT, 3);
|
||||
// control law
|
||||
double factor = -gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
|
||||
VectorOperations<double>::mulScalar(magRateBT, factor, magMomB, 3);
|
||||
}
|
||||
|
||||
@ -2,30 +2,22 @@
|
||||
#define ACS_CONTROL_DETUMBLE_H_
|
||||
|
||||
#include <fsfw/returnvalues/returnvalue.h>
|
||||
#include <mission/acs/defs.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#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:
|
||||
};
|
||||
|
||||
@ -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<double>::multiply(
|
||||
*gainMatrixDiagonal, *(acsParameters->inertiaEIVE.inertiaMatrix), *gainMatrix, 3, 3, 3);
|
||||
MatrixOperations<double>::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<double>::multiply(
|
||||
*gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrix), *pMatrix, 3, 3, 3);
|
||||
*gainMatrixInverse, *(acsParameters->inertiaEIVE.inertiaMatrixDeployed), *pMatrix, 3, 3, 3);
|
||||
MatrixOperations<double>::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<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), deltaRate,
|
||||
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), deltaRate,
|
||||
torqueRate, 3, 3, 1);
|
||||
VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
|
||||
VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);
|
||||
@ -116,7 +117,7 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP
|
||||
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
|
||||
momentumRw, 3, 4, 1);
|
||||
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), satRate,
|
||||
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
|
||||
momentumSat, 3, 3, 1);
|
||||
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
|
||||
// calculating momentum error
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -1,158 +1,136 @@
|
||||
#include "SafeCtrl.h"
|
||||
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <math.h>
|
||||
|
||||
#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<double>::dcmFromQuat(quatBJ, *dcmBJ);
|
||||
double sunDirB[3] = {0, 0, 0}, magFieldB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBJ, sunDirModel, sunDirB, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmBJ, magFieldModel, magFieldB, 3, 3, 1);
|
||||
|
||||
// change unit from uT to T
|
||||
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3);
|
||||
|
||||
double crossSun[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(sunDirRef, sunDirB, crossSun);
|
||||
double normCrossSun = VectorOperations<double>::norm(crossSun, 3);
|
||||
|
||||
// calc angle alpha between sunDirRef and sunDIr
|
||||
double dotSun = VectorOperations<double>::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<double>::mulScalar(crossSun, scalarFac, torqueAlign, 3);
|
||||
|
||||
double rateSafeMode[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(satRateMekf, satRatRef, rateSafeMode, 3);
|
||||
VectorOperations<double>::mulScalar(rateSafeMode, -kRate, torqueRate, 3);
|
||||
|
||||
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
|
||||
// Adding factor of inertia for axes
|
||||
MatrixOperations<double>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
|
||||
*gainMatrixInertia, 3,
|
||||
3); // why only for mekf one and not for no mekf
|
||||
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
|
||||
|
||||
// MagMom B (orthogonal torque)
|
||||
double torqueMgt[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(magFieldB, torqueCmd, torqueMgt);
|
||||
double normMag = VectorOperations<double>::norm(magFieldB, 3);
|
||||
VectorOperations<double>::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<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
||||
|
||||
// normalize sunDir and magDir
|
||||
double magDirB[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
|
||||
VectorOperations<double>::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<double>::dot(magDirB, susDirB);
|
||||
// calculate angle alpha between sunDirRef and sunDir
|
||||
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
|
||||
errorAngle = acos(dotSun);
|
||||
|
||||
// Rate parallel to sun direction and magnetic field direction
|
||||
double dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
|
||||
double dotmagRateSun = VectorOperations<double>::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<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
|
||||
VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
|
||||
VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
|
||||
VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
|
||||
VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
|
||||
VectorOperations<double>::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<double>::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<double>::dot(sunDirRef, susDirB);
|
||||
double crossSusSunRef[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
|
||||
double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
|
||||
|
||||
double torqueAlign[3] = {0, 0, 0};
|
||||
double angleAlignErr = acos(cosAngleAlignErr);
|
||||
double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
|
||||
VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
|
||||
|
||||
// Torque Rate Calculations
|
||||
double torqueRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
|
||||
|
||||
// Final torque
|
||||
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
|
||||
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate,
|
||||
torqueB, 3, 3, 1);
|
||||
|
||||
// Magnetic moment
|
||||
double magMomB[3] = {0, 0, 0};
|
||||
double crossMagFieldTorque[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
|
||||
double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
|
||||
VectorOperations<double>::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<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
||||
|
||||
// calculate error angle between sunDirRef and sunDir
|
||||
double dotSun = VectorOperations<double>::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<double>::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<double>::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<double>::dot(satRotRateB, sunDirB) *
|
||||
pow(VectorOperations<double>::norm(sunDirB, 3), -2);
|
||||
VectorOperations<double>::mulScalar(sunDirB, parallelLength, satRotRateParallelB, 3);
|
||||
VectorOperations<double>::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<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);
|
||||
|
||||
// calculate torque for orthogonal rotational rate
|
||||
VectorOperations<double>::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<double>::cross(sunDirRefB, sunDirB, crossAlign);
|
||||
VectorOperations<double>::mulScalar(crossAlign, gainAlign, cmdAlign, 3);
|
||||
}
|
||||
|
||||
void SafeCtrl::calculateMagneticMoment(double *magMomB) {
|
||||
double torqueMgt[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
|
||||
double normMag = VectorOperations<double>::norm(magFieldBT, 3);
|
||||
VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
|
||||
}
|
||||
|
||||
@ -1,40 +1,52 @@
|
||||
#ifndef SAFECTRL_H_
|
||||
#define SAFECTRL_H_
|
||||
|
||||
#include <eive/resultClassIds.h>
|
||||
#include <mission/acs/defs.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#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_ */
|
||||
|
||||
@ -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<CTRL_VAL_SET_ENTRIES> {
|
||||
public:
|
||||
CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}
|
||||
|
||||
lp_var_t<uint8_t> safeStrat = lp_var_t<uint8_t>(sid.objectId, SAFE_STRAT, this);
|
||||
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
|
||||
lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
|
||||
lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
|
||||
|
||||
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 1f06ea4590d6340990fb8d9534a84e7e9de68951
|
||||
Subproject commit a07f21329adcffa963d75769d523b3df34222543
|
||||
Loading…
Reference in New Issue
Block a user