eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Improve Safe Strat State Machine #777

Merged
muellerr merged 5 commits from safe-strat-improvement into main 2023-08-15 13:19:15 +02:00
Conversation 0 Commits 5 Files Changed 5 +26 -17
3 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit d82a6ece5a - Show all commits

6
mission/controller/AcsController.cpp
View File

@ -204,8 +204,7 @@ void AcsController::performSafe() {
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(), fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse); acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) { switch (safeCtrlStrat) {
@ -223,7 +222,8 @@ void AcsController::performSafe() {
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value, safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value, fusedRotRateData.rotRateOrthogonal.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;

25
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -9,10 +9,12 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {} SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy( acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool satRotRateValid, const bool sunDirValid,
const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid, const bool fusedRateTotalValid,
const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) { const uint8_t mekfEnabled,
const uint8_t gyrEnabled,
const uint8_t dampingEnabled) {
if (not magFieldValid) { if (not magFieldValid) {
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
} else if (mekfEnabled and mekfValid) { } else if (mekfEnabled and mekfValid) {
@ -20,7 +22,7 @@ acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
} else if (sunDirValid) { } else if (sunDirValid) {
if (gyrEnabled and satRotRateValid) { if (gyrEnabled and satRotRateValid) {
return acs::SafeModeStrategy::SAFECTRL_GYR; return acs::SafeModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and fusedRateSplitValid) { } else if (not gyrEnabled and fusedRateTotalValid) {
return acs::SafeModeStrategy::SAFECTRL_SUSMGM; return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
} else { } else {
return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
@ -95,9 +97,10 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const
calculateMagneticMoment(magMomB); calculateMagneticMoment(magMomB);
} }
void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB, void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
const double *rotRateOrthogonalB, const double *sunDirB, const double *rotRateParallelB, const double *rotRateOrthogonalB,
const double *sunDirRefB, double *magMomB, double &errorAngle) { const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -105,8 +108,14 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel
double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB); double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
if (VectorOperations<double>::norm(rotRateParallelB, 3) != 0 and
VectorOperations<double>::norm(rotRateOrthogonalB, 3) != 0) {
std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB)); std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB)); std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
} else {
splitRotationalRate(rotRateTotalB, sunDirB);
}
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm, calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_orthoSusMgm); acsParameters->safeModeControllerParameters.k_orthoSusMgm);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,

8
mission/controller/acs/control/SafeCtrl.h
View File

@ -14,7 +14,6 @@ class SafeCtrl {
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid, const bool satRotRateValid, const bool sunDirValid,
const bool fusedRateSplitValid,
const bool fusedRateTotalValid, const uint8_t mekfEnabled, const bool fusedRateTotalValid, const uint8_t mekfEnabled,
const uint8_t gyrEnabled, const uint8_t dampingEnabled); const uint8_t gyrEnabled, const uint8_t dampingEnabled);
@ -25,9 +24,10 @@ class SafeCtrl {
void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB, void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirRefB, double *magMomB, double &errorAngle); const double *sunDirRefB, double *magMomB, double &errorAngle);
void safeSusMgm(const double *magFieldB, const double *rotRateParallelB, void safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB, const double *rotRateParallelB, const double *rotRateOrthogonalB,
double *magMomB, double &errorAngle); const double *sunDirB, const double *sunDirRefB, double *magMomB,
double &errorAngle);
void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB, void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle); const double *sunDirRefB, double *magMomB, double &errorAngle);