diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1a89a271..b2c3948d 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -174,47 +174,31 @@ void AcsController::performSafe() { mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid()); switch (safeCtrlStrat) { - case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_MEKF): + case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, satRateSafe, inertiaEive, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_NONMEKF): + case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF): safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, susDataProcessed.susVecTot.value, sunTargetDir, satRateSafe, inertiaEive, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_DAMPING): + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, satRateSafe, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (SafeCtrl::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): - if (not safeCtrlFailureFlag) { - triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 1, 0); - safeCtrlFailureFlag = true; - } - safeCtrlFailureCounter++; - if (safeCtrlFailureCounter > 150) { - safeCtrlFailureFlag = false; - safeCtrlFailureCounter = 0; - } + case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + safeCtrlFailure(1, 0); break; - case (SafeCtrl::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): - if (not safeCtrlFailureFlag) { - triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 0, 1); - safeCtrlFailureFlag = true; - } - safeCtrlFailureCounter++; - if (safeCtrlFailureCounter > 150) { - safeCtrlFailureFlag = false; - safeCtrlFailureCounter = 0; - } + case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + safeCtrlFailure(0, 1); break; } @@ -270,11 +254,27 @@ void AcsController::performDetumble() { triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } + uint8_t safeCtrlStrat = detumble.detumbleStrategy(mgmDataProcessed.mgmVecTot.isValid(), + gyrDataProcessed.gyrVecTot.isValid(), + mgmDataProcessed.mgmVecTotDerivative.isValid()); double magMomMtq[3] = {0, 0, 0}; - detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, - mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), magMomMtq, - acsParameters.detumbleParameter.gainD); + switch (safeCtrlStrat) { + case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL): + detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, + magMomMtq, acsParameters.detumbleParameter.gainD); + break; + case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): + detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, + magMomMtq, acsParameters.detumbleParameter.gainD); + break; + case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + safeCtrlFailure(1, 0); + break; + case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + safeCtrlFailure(0, 1); + break; + } + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, *acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipolMax); @@ -498,6 +498,18 @@ void AcsController::performPointingCtrl() { // acsParameters.rwHandlingParameters.rampTime); } +void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) { + if (not safeCtrlFailureFlag) { + triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure); + safeCtrlFailureFlag = true; + } + safeCtrlFailureCounter++; + if (safeCtrlFailureCounter > 150) { + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + } +} + ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration) { { @@ -581,7 +593,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng; std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); - ctrlValData.safeStrat.value = SafeCtrl::SafeModeStrategy::SAFECTRL_OFF; + ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF; ctrlValData.setValidity(true, true); } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 04412967..10f32720 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -12,18 +12,17 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include "acs/ActuatorCmd.h" -#include "acs/Guidance.h" -#include "acs/MultiplicativeKalmanFilter.h" -#include "acs/Navigation.h" -#include "acs/SensorProcessing.h" -#include "acs/control/Detumble.h" -#include "acs/control/PtgCtrl.h" -#include "acs/control/SafeCtrl.h" -#include "controllerdefinitions/AcsCtrlDefinitions.h" - class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF { public: static constexpr dur_millis_t INIT_DELAY = 500; @@ -106,6 +105,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void modeChanged(Mode_t mode, Submode_t submode); void announceMode(bool recursive); + void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure); + ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration); ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,