diff --git a/CHANGELOG.md b/CHANGELOG.md index 3727efef..6eb71d92 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,14 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Added + +- Added NaN and Inf check for the `MEKF`. If these are detected, the `AcsController` will reset + the `MEKF` on its own once. This way, there will not be an event spam and operators will have + to look into the reason of wrong outputs. To restore the reset ability, an action command has + been added. + + ## Fixed - Fixed transition for dual power lane assemblies: When going from dual side submode to single side @@ -69,8 +77,7 @@ eive-tmtc: v2.19.1 PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/462 - Add `PcduHandlerDummy` component. - Added parameter for timeout until `MEKF_INVALID_MODE_VIOLATION` event is triggered. -- EIVE system: Add boot mode which is also the initial mode. The fallback mode of the boot mode - will be the SAFE mode. The boot mode can also be used to switch as many devices as possible OFF. + ## Fixed - Pointing control of the `AcsController` was still expecting submodes instead of modes. @@ -96,9 +103,6 @@ eive-tmtc: v2.19.1 commanding is done autonomously by the COM subsystem internally or by the operator. This prevents the transmitter from going off during fallbacks to the SAFE mode, which might not always be desired. - -## Changed - - Initialize switch states to a special `SWITCH_STATE_UNKNOWN` (2) variable. Return `PowerSwitchIF::SWITCH_UNKNOWN` in switch state getter if this is the state. - Wait 1 second before commanding SAFE mode. This ensures or at least increases the chance that diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index 32b61bca..aabf8e69 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 278 translations. * @details - * Generated on: 2023-03-15 10:10:04 + * Generated on: 2023-03-21 12:45:57 */ #include "translateEvents.h" diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp index 81710911..ad27211f 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 169 translations. - * Generated on: 2023-03-14 17:08:41 + * Generated on: 2023-03-21 12:45:57 */ #include "translateObjects.h" diff --git a/generators/bsp_hosted_returnvalues.csv b/generators/bsp_hosted_returnvalues.csv index 882bdd05..af3e0146 100644 --- a/generators/bsp_hosted_returnvalues.csv +++ b/generators/bsp_hosted_returnvalues.csv @@ -470,8 +470,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +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 diff --git a/generators/bsp_q7s_returnvalues.csv b/generators/bsp_q7s_returnvalues.csv index 971d2e73..c509e7dc 100644 --- a/generators/bsp_q7s_returnvalues.csv +++ b/generators/bsp_q7s_returnvalues.csv @@ -581,8 +581,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path 0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h 0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h -0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h +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 diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index 32b61bca..aabf8e69 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 278 translations. * @details - * Generated on: 2023-03-15 10:10:04 + * Generated on: 2023-03-21 12:45:57 */ #include "translateEvents.h" diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp index ede7df22..127858d3 100644 --- a/generators/objects/translateObjects.cpp +++ b/generators/objects/translateObjects.cpp @@ -2,7 +2,7 @@ * @brief Auto-generated object translation file. * @details * Contains 173 translations. - * Generated on: 2023-03-14 17:08:41 + * Generated on: 2023-03-21 12:45:57 */ #include "translateObjects.h" diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index 32b61bca..aabf8e69 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** * @brief Auto-generated event translation file. Contains 278 translations. * @details - * Generated on: 2023-03-15 10:10:04 + * Generated on: 2023-03-21 12:45:57 */ #include "translateEvents.h" diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp index ede7df22..127858d3 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 173 translations. - * Generated on: 2023-03-14 17:08:41 + * Generated on: 2023-03-21 12:45:57 */ #include "translateObjects.h" diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index e3a5ed5e..c87f460c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -55,6 +55,10 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t navigation.resetMekf(&mekfData); return HasActionsIF::EXECUTION_FINISHED; } + case RESTORE_MEKF_NONFINITE_RECOVERY: { + mekfLost = false; + return HasActionsIF::EXECUTION_FINISHED; + } default: { return HasActionsIF::INVALID_ACTION_ID; } @@ -149,6 +153,10 @@ void AcsController::performSafe() { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { + navigation.resetMekf(&mekfData); + mekfLost = true; + } } else { mekfInvalidFlag = false; } @@ -231,6 +239,9 @@ void AcsController::performDetumble() { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { + navigation.resetMekf(&mekfData); + } } else { mekfInvalidFlag = false; } @@ -282,6 +293,9 @@ void AcsController::performPointingCtrl() { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { + navigation.resetMekf(&mekfData); + } 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); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 3c309bd4..fd17d606 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -64,6 +64,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint16_t mekfInvalidCounter = 0; bool safeCtrlFailureFlag = false; uint8_t safeCtrlFailureCounter = 0; + uint8_t resetMekfCount = 0; + bool mekfLost = false; int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0}; @@ -78,6 +80,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes /** Device command IDs */ static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0; static const DeviceCommandId_t RESET_MEKF = 0x1; + static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL; static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0); diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 77a3ef00..88568573 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1080,6 +1080,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst( MatrixOperations::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6); + if (not(MathOperations::checkVectorIsFinite(propagatedQuaternion, 4)) || + not(MathOperations::checkMatrixIsFinite(initialQuaternion, 6, 6))) { + updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE); + return MEKF_NOT_FINITE; + } + updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst); return MEKF_RUNNING; } diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index ceb98339..fe749890 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -62,6 +62,7 @@ class MultiplicativeKalmanFilter { NO_MODEL_VECTORS = 2, NO_SUS_MGM_STR_DATA = 3, COVARIANCE_INVERSION_FAILED = 4, + NOT_FINITE = 5, INITIALIZED = 10, RUNNING = 11, }; @@ -74,8 +75,9 @@ class MultiplicativeKalmanFilter { static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5); static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED = returnvalue::makeCode(IF_MEKF_ID, 6); - static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7); - static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8); + static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7); + static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8); + static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9); private: /*Parameters*/ diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index f8537740..b344451a 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -404,6 +404,26 @@ class MathOperations { std::memcpy(inverse, identity, sizeof(identity)); return 0; // successful inversion } + + static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) { + for (uint8_t i = 0; i < size; i++) { + if (not isfinite(inputVector[i])) { + return false; + } + } + return true; + } + + static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) { + for (uint8_t col = 0; col < cols; col++) { + for (uint8_t row = 0; row < rows; row++) { + if (not isfinite(inputMatrix[row * cols + cols])) { + return false; + } + } + } + return true; + } }; #endif /* ACS_MATH_MATHOPERATIONS_H_ */ diff --git a/tmtc b/tmtc index e5a09e14..b32a9273 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit e5a09e148b45d0380dc6d9a1a88002bab4b0376c +Subproject commit b32a9273ae4186a440685ffde7618f017cfdb268