eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

fixes
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-03-24 15:18:52 +01:00
parent 506c8a3fa6
commit 90db9785ea
2 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
mission/acsDefs.h
View File

@ -21,7 +21,7 @@ enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
//! [EXPORT] : [COMMENT] The system has recovered from a safe rate rotation violation. //! [EXPORT] : [COMMENT] The system has recovered from a safe rate rotation violation.
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
//! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes //! [EXPORT] : [COMMENT] Multiple RWs are invalid, uncommandable and therefore higher ACS modes
@ -30,17 +30,17 @@ static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution. //! [EXPORT] : [COMMENT] MEKF was not able to compute a solution.
//! P1: MEKF state on exit //! P1: MEKF state on exit
static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO); 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); static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO);
//! [EXPORT] : [COMMENT] MEKF performed an automatic recovery after detection of nonfinite values. //! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values.
static constexpr Event MEKF_AUTOMATIC_RECOVERY = MAKE_EVENT(4, severity::INFO); 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 //! [EXPORT] : [COMMENT] MEKF was not able to compute a solution during any pointing ACS mode for a
//! prolonged time. //! prolonged time.
static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(5, severity::HIGH); 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 //! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has
//! failed. //! failed.
//! P1: Missing information about magnetic field, P2: Missing information about rotational rate //! P1: Missing information about magnetic field, P2: Missing information about rotational rate
static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(6, severity::HIGH); static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH);
extern const char* getModeStr(AcsMode mode); extern const char* getModeStr(AcsMode mode);

13
mission/controller/AcsController.cpp
View File

@ -154,13 +154,15 @@ void AcsController::performSafe() {
mekfInvalidFlag = true; mekfInvalidFlag = true;
} }
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY); triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData); navigation.resetMekf(&mekfData);
mekfLost = true; mekfLost = true;
} }
} else { } else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
// get desired satellite rate, sun direction to align to and inertia // get desired satellite rate, sun direction to align to and inertia
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}, double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0},
inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -260,11 +262,12 @@ void AcsController::performDetumble() {
mekfInvalidFlag = true; mekfInvalidFlag = true;
} }
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY); triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData); navigation.resetMekf(&mekfData);
mekfLost = true; mekfLost = true;
} }
} else { } else if (mekfInvalidFlag) {
triggerEvent(acs::MEKF_RECOVERY);
mekfInvalidFlag = false; mekfInvalidFlag = false;
} }
double magMomMtq[3] = {0, 0, 0}; double magMomMtq[3] = {0, 0, 0};
@ -317,7 +320,7 @@ void AcsController::performPointingCtrl() {
mekfInvalidFlag = true; mekfInvalidFlag = true;
} }
if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) { if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
triggerEvent(acs::MEKF_AUTOMATIC_RECOVERY); triggerEvent(acs::MEKF_AUTOMATIC_RESET);
navigation.resetMekf(&mekfData); navigation.resetMekf(&mekfData);
mekfLost = true; mekfLost = true;
} }