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

use safeCtrl even if SUS and MGM vectors are too close
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-08-15 09:11:57 +02:00
parent 8447b3c41e
commit d82a6ece5a
3 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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