From 9cdc932ad805978eda3173258af5fcd936fcac33 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Mar 2023 15:36:46 +0100 Subject: [PATCH 01/10] also reset propagatedQuaternion --- mission/controller/acs/MultiplicativeKalmanFilter.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 77a3ef00..2ada8ab1 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1089,6 +1089,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); + std::memcpy(propagatedQuaternion, resetQuaternion, 4 * sizeof(double)); std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); return MEKF_UNINITIALIZED; From 63f45ec27e64387d6fa40f071ee2d4c97c86e6e2 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Mar 2023 15:39:43 +0100 Subject: [PATCH 02/10] added finite checks for vectors and matrices --- mission/controller/acs/util/MathOperations.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) 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_ */ From 79bd349ac76a08caaab1e7c87c9d0ca2760f72c5 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Mar 2023 15:58:48 +0100 Subject: [PATCH 03/10] added finite check for MEKF --- mission/controller/acs/MultiplicativeKalmanFilter.cpp | 6 ++++++ mission/controller/acs/MultiplicativeKalmanFilter.h | 6 ++++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index 2ada8ab1..e752da39 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*/ From e77766ae593ba8efc905226854671b4d5ec1263f Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Mar 2023 16:14:32 +0100 Subject: [PATCH 04/10] reset the MEKF in case its results are not finite --- mission/controller/AcsController.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b4e7c5d3..d513d114 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -271,6 +271,9 @@ void AcsController::performPointingCtrl() { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; } + if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE) { + navigation.resetMekf(&mekfData); + } if (mekfInvalidCounter == 5) { // Trigger this so STR FDIR can set the device faulty. EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0); From 4d74c8ec4381cfa3a5018803d2dd7d5c84c664d5 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Mar 2023 16:24:13 +0100 Subject: [PATCH 05/10] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7f9681d3..af4cef0a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,8 @@ will consitute of a breaking change warranting a new major release: ## Added - Added `EXECUTE_SHELL_CMD` action command for `CoreController` to execute arbitrary Linux commands. +- Added NaN and Inf check for the `MEKF`. If these are detected, the `AcsController` will reset + the `MEKF` on its own. ## Fixed From f358719778fff29646ac91959d030c17f2a5f6f7 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 13 Mar 2023 16:33:48 +0100 Subject: [PATCH 06/10] added event in case of safe mode controller failure --- mission/acsDefs.h | 2 ++ mission/controller/AcsController.cpp | 9 ++++++++- mission/controller/AcsController.h | 3 +++ 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/mission/acsDefs.h b/mission/acsDefs.h index a724bbd7..7ba8dc13 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -32,6 +32,8 @@ static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, 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(4, 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(5, severity::HIGH); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d513d114..5eb4abac 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -173,7 +173,14 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq); } if (result == returnvalue::FAILED) { - // ToDo: this should never ever happen or we are dead. prob add an event at least + if (not doomFlag) { + triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE); + doomFlag = true; + } + doomCounter++; + if (doomCounter > 5) { + doomFlag = false; + } } actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c0376127..829ab099 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -62,6 +62,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = false; uint8_t mekfInvalidCounter = 0; + bool doomFlag = false; + uint8_t doomCounter = 0; + int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0}; From ccf1fcaa7910126165564a1df57dd5e3fc4c811a Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 15 Mar 2023 10:06:24 +0100 Subject: [PATCH 07/10] stuff --- mission/controller/AcsController.cpp | 12 +++++++----- mission/controller/AcsController.h | 4 ++-- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8e7b1b9a..220ba178 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -173,14 +173,16 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq); } if (result == returnvalue::FAILED) { - if (not doomFlag) { + if (not safeCtrlFailureFlag) { triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE); - doomFlag = true; + safeCtrlFailureFlag = true; } - doomCounter++; - if (doomCounter > 5) { - doomFlag = false; + safeCtrlFailureCounter++; + if (safeCtrlFailureCounter > 50) { + safeCtrlFailureFlag = false; } + } else { + safeCtrlFailureFlag = false; } actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 2b0cda17..3c309bd4 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -62,8 +62,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = false; uint16_t mekfInvalidCounter = 0; - bool doomFlag = false; - uint8_t doomCounter = 0; + bool safeCtrlFailureFlag = false; + uint8_t safeCtrlFailureCounter = 0; int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int16_t cmdDipolMtqs[3] = {0, 0, 0}; From c4fe772fe7e76a2d07c6af8d7de2d794682a78ec Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 15 Mar 2023 10:08:58 +0100 Subject: [PATCH 08/10] changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3c1891d6..dfa43418 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Added + +- Failure of Safe Mode Ctrl will now trigger an event. As this can only be caused by sensors not + being in the correct mode, the assemblies should take care that this will never happen and no + additional FDIR is needed. + # [v1.37.2] 2023-03-14 - Changed `PoolManager` bugfix implementation in the FSFW. From 102228100e61f3f0808e3af56ee84a8dd19ed3d3 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 15 Mar 2023 10:10:47 +0100 Subject: [PATCH 09/10] reran event gens --- bsp_hosted/fsfwconfig/events/translateEvents.cpp | 7 +++++-- generators/bsp_hosted_events.csv | 1 + generators/bsp_q7s_events.csv | 1 + generators/events/translateEvents.cpp | 7 +++++-- linux/fsfwconfig/events/translateEvents.cpp | 7 +++++-- 5 files changed, 17 insertions(+), 6 deletions(-) diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp index e0f9c58f..32b61bca 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 277 translations. + * @brief Auto-generated event translation file. Contains 278 translations. * @details - * Generated on: 2023-03-14 17:08:41 + * Generated on: 2023-03-15 10:10:04 */ #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_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"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -464,6 +465,8 @@ const char *translateEvents(Event event) { return MEKF_INVALID_INFO_STRING; case (11204): return MEKF_INVALID_MODE_VIOLATION_STRING; + case (11205): + return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv index ec5050cc..2f10693c 100644 --- a/generators/bsp_hosted_events.csv +++ b/generators/bsp_hosted_events.csv @@ -91,6 +91,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h 11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h 11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h +11205;0x2bc5;SAFE_MODE_CONTROLLER_FAILURE;HIGH;No description;mission/acsDefs.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/devices/devicedefinitions/powerDefinitions.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/devices/devicedefinitions/powerDefinitions.h 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv index ec5050cc..2f10693c 100644 --- a/generators/bsp_q7s_events.csv +++ b/generators/bsp_q7s_events.csv @@ -91,6 +91,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h 11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h 11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h +11205;0x2bc5;SAFE_MODE_CONTROLLER_FAILURE;HIGH;No description;mission/acsDefs.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/devices/devicedefinitions/powerDefinitions.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/devices/devicedefinitions/powerDefinitions.h 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp index e0f9c58f..32b61bca 100644 --- a/generators/events/translateEvents.cpp +++ b/generators/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 277 translations. + * @brief Auto-generated event translation file. Contains 278 translations. * @details - * Generated on: 2023-03-14 17:08:41 + * Generated on: 2023-03-15 10:10:04 */ #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_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"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -464,6 +465,8 @@ const char *translateEvents(Event event) { return MEKF_INVALID_INFO_STRING; case (11204): return MEKF_INVALID_MODE_VIOLATION_STRING; + case (11205): + return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp index e0f9c58f..32b61bca 100644 --- a/linux/fsfwconfig/events/translateEvents.cpp +++ b/linux/fsfwconfig/events/translateEvents.cpp @@ -1,7 +1,7 @@ /** - * @brief Auto-generated event translation file. Contains 277 translations. + * @brief Auto-generated event translation file. Contains 278 translations. * @details - * Generated on: 2023-03-14 17:08:41 + * Generated on: 2023-03-15 10:10:04 */ #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_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"; const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED"; const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED"; @@ -464,6 +465,8 @@ const char *translateEvents(Event event) { return MEKF_INVALID_INFO_STRING; case (11204): return MEKF_INVALID_MODE_VIOLATION_STRING; + case (11205): + return SAFE_MODE_CONTROLLER_FAILURE_STRING; case (11300): return SWITCH_CMD_SENT_STRING; case (11301): From 95a4d88ed465489ca9b9a26e5570134d2c50e19f Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 15 Mar 2023 10:17:24 +0100 Subject: [PATCH 10/10] cleanup --- .../acs/MultiplicativeKalmanFilter.cpp | 7 ------- .../acs/MultiplicativeKalmanFilter.h | 6 ++---- mission/controller/acs/util/MathOperations.h | 20 ------------------- 3 files changed, 2 insertions(+), 31 deletions(-) diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index e752da39..77a3ef00 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -1080,12 +1080,6 @@ 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; } @@ -1095,7 +1089,6 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) { double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}}; std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double)); - std::memcpy(propagatedQuaternion, resetQuaternion, 4 * sizeof(double)); std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double)); updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED); return MEKF_UNINITIALIZED; diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index fe749890..ceb98339 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -62,7 +62,6 @@ class MultiplicativeKalmanFilter { NO_MODEL_VECTORS = 2, NO_SUS_MGM_STR_DATA = 3, COVARIANCE_INVERSION_FAILED = 4, - NOT_FINITE = 5, INITIALIZED = 10, RUNNING = 11, }; @@ -75,9 +74,8 @@ 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_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); + static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7); + static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8); private: /*Parameters*/ diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h index b344451a..f8537740 100644 --- a/mission/controller/acs/util/MathOperations.h +++ b/mission/controller/acs/util/MathOperations.h @@ -404,26 +404,6 @@ 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_ */