diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 253c6aca..2f82ca9f 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -67,9 +67,9 @@ static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO); static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO); //! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values. 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 -//! prolonged time. -static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH); +//! [EXPORT] : [COMMENT] For a prolonged time, no attitude information was available for the +//! Pointing Controller. Falling back to Safe Mode. +static constexpr Event PTG_CTRL_NO_ATTITUDE_INFORMATION = MAKE_EVENT(6, severity::HIGH); //! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has //! failed. //! P1: Missing information about magnetic field, P2: Missing information about rotational rate diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8b484fb4..47c5d77e 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -369,6 +369,21 @@ void AcsController::performDetumble() { } void AcsController::performPointingCtrl() { + acs::ControlModeStrategy ptgCtrlStrat = + ptgCtrl.pointingCtrlStrategy(magFieldValid, mekfValid, satRotRateValid, sunDirValid, + fusedRateTotalValid, mekfEnabled, gyrEnabled, dampingEnabled); + + if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { + ptgCtrlLostCounter++; + if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { + triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); + ptgCtrlLostCounter = 0; + } + commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); + } + uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 1b184b80..f67595ee 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -72,7 +72,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes uint8_t detumbleCounter = 0; uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = false; - uint16_t mekfInvalidCounter = 0; + uint16_t ptgCtrlLostCounter = 0; bool safeCtrlFailureFlag = false; uint8_t safeCtrlFailureCounter = 0; uint8_t resetMekfCount = 0; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 7b867a46..551f7cd3 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -24,7 +24,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(onBoardParams.sampleTime); break; case 0x1: - parameterWrapper->set(onBoardParams.mekfViolationTimer); + parameterWrapper->set(onBoardParams.ptgCtrlLostTimer); break; case 0x2: parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f62928a0..cbebb52a 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -18,7 +18,7 @@ class AcsParameters : public HasParametersIF { struct OnBoardParams { double sampleTime = 0.4; // [s] - uint16_t mekfViolationTimer = 750; + uint16_t ptgCtrlLostTimer = 750; uint8_t fusedRateSafeDuringEclipse = true; } onBoardParams; diff --git a/mission/system/acs/StrFdir.cpp b/mission/system/acs/StrFdir.cpp index 97a4162a..83bd27fa 100644 --- a/mission/system/acs/StrFdir.cpp +++ b/mission/system/acs/StrFdir.cpp @@ -6,7 +6,7 @@ StrFdir::StrFdir(object_id_t strObject) : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {} ReturnValue_t StrFdir::eventReceived(EventMessage* event) { - if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) { + if (event->getEvent() == acs::PTG_CTRL_NO_ATTITUDE_INFORMATION) { setFaulty(event->getEvent()); return returnvalue::OK; }