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};