diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 04009cb2..e163fd67 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -175,20 +175,26 @@ void AcsController::performSafe() { acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): + case (acs::SafeModeStrategy::SAFECTRL_MEKF): safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF): - safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, + case (acs::SafeModeStrategy::SAFECTRL_GYR): + safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): + case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): + safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 43677ccf..8c06c976 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -12,15 +12,20 @@ SafeCtrl::~SafeCtrl() {} acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF; - } else if (satRotRateValid and sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF; - } else if (dampingEnabled and satRotRateValid and not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING; + return acs::SafeModeStrategy::SAFECTRL_MEKF; + } else if (gyrEnabled and satRotRateValid and sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_GYR; + } else if (not gyrEnabled and sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_SUSMGM; + } else if (gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + } else if (not gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else if (not dampingEnabled and satRotRateValid and not sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; } else { @@ -43,7 +48,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_orthoMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, @@ -57,9 +62,8 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, calculateMagneticMoment(magMomB); } -void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, - const double *sunDirB, const double *sunDirRefB, double *magMomB, - double &errorAngle) { +void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -68,7 +72,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, @@ -82,6 +86,32 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, calculateMagneticMoment(magMomB); } +void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB, + const double *rotRateOrthogonalB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { + // convert magFieldB from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // calculate error angle between sunDirRef and sunDir + double dotSun = VectorOperations::dot(sunDirRefB, sunDirB); + errorAngle = acos(dotSun); + + std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB)); + std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB)); + calculateRotationalRateTorque(sunDirB, errorAngle, + acsParameters->safeModeControllerParameters.k_parallelSusMgm, + acsParameters->safeModeControllerParameters.k_orthoSusMgm); + calculateAngleErrorTorque(sunDirB, sunDirRefB, + acsParameters->safeModeControllerParameters.k_alignSusMgm); + + // sum of all torques + for (uint8_t i = 0; i < 3; i++) { + cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i]; + } + + calculateMagneticMoment(magMomB); +} + void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T @@ -91,7 +121,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate errorAngle = NAN; splitRotationalRate(satRotRateB, sunDirRefB); - calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf); @@ -110,9 +140,8 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD VectorOperations::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); } -void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, - double &errorAngle, const double gainParallel, - const double gainOrtho) { +void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, double &errorAngle, + const double gainParallel, const double gainOrtho) { // calculate torque for parallel rotational rate VectorOperations::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 12f9ddb0..d5ca4dca 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -14,23 +14,34 @@ class SafeCtrl { acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled); + const uint8_t mekfEnabled, const uint8_t gyrEnabled, + const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, double &errorAngle); - void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB, - const double *sunDirRefB, double *magMomB, double &errorAngle); + void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle); - void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, - double *magMomB, double &errorAngle); + void safeSusMgm(const double *magFieldB, const double *rotRateParallelB, + const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB, + double *magMomB, double &errorAngle); + + void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle); + + void safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle); void splitRotationalRate(const double *satRotRateB, const double *sunDirB); - void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, - double &errorAngle, const double gainParallel, - const double gainOrtho); + void calculateRotationalRates(const double *magFieldB, const double *magRateB, + const double *sunDirB, const double *sunRateB, + double *fusedRotRate); + + void calculateRotationalRateTorque(const double *sunDirB, double &errorAngle, + const double gainParallel, const double gainOrtho); void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, const double gainAlign);