diff --git a/mission/acsDefs.h b/mission/acsDefs.h index 7ba8dc13..dcfc17b0 100644 --- a/mission/acsDefs.h +++ b/mission/acsDefs.h @@ -30,10 +30,12 @@ static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM); static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH); //!< MEKF was not able to compute a solution. static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO); +//!< 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(4, severity::HIGH); +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(5, severity::HIGH); +static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(6, severity::HIGH); extern const char* getModeStr(AcsMode mode); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b275960b..4b865ee3 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -289,6 +289,7 @@ void AcsController::performPointingCtrl() { &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) { + mekfInvalidCounter++; if (not mekfInvalidFlag) { triggerEvent(acs::MEKF_INVALID_INFO); mekfInvalidFlag = true; @@ -299,15 +300,18 @@ void AcsController::performPointingCtrl() { 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; } - mekfInvalidCounter++; // commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, // cmdSpeedRws[0], // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], // acsParameters.rwHandlingParameters.rampTime); return; } else { - mekfInvalidFlag = false; + if (mekfInvalidFlag) { + triggerEvent(acs::MEKF_RECOVERY); + mekfInvalidFlag = false; + } mekfInvalidCounter = 0; } uint8_t enableAntiStiction = true;