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

extended state machine
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-07-19 13:24:37 +02:00
parent ebedb59929
commit 8451a2f8ef
3 changed files with 72 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
mission/controller/AcsController.cpp
View File

@ -175,20 +175,26 @@ void AcsController::performSafe() {
acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.dampingDuringEclipse); acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) { switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): case (acs::SafeModeStrategy::SAFECTRL_MEKF):
safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
magMomMtq, errAng); magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF): case (acs::SafeModeStrategy::SAFECTRL_GYR):
safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; 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, safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
sunTargetDir, magMomMtq, errAng); sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;

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

@ -12,15 +12,20 @@ SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
const bool satRotRateValid, const bool sunDirValid, const bool satRotRateValid, const bool sunDirValid,
const uint8_t mekfEnabled, const uint8_t mekfEnabled,
const uint8_t gyrEnabled,
const uint8_t dampingEnabled) { 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) {
return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF; return acs::SafeModeStrategy::SAFECTRL_MEKF;
} else if (satRotRateValid and sunDirValid) { } else if (gyrEnabled and satRotRateValid and sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF; return acs::SafeModeStrategy::SAFECTRL_GYR;
} else if (dampingEnabled and satRotRateValid and not sunDirValid) { } else if (not gyrEnabled and sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING; 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) { } else if (not dampingEnabled and satRotRateValid and not sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
} else { } else {
@ -43,7 +48,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, calculateRotationalRateTorque(sunDirB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_parallelMekf,
acsParameters->safeModeControllerParameters.k_orthoMekf); acsParameters->safeModeControllerParameters.k_orthoMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
@ -57,9 +62,8 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
calculateMagneticMoment(magMomB); calculateMagneticMoment(magMomB);
} }
void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
const double *sunDirB, const double *sunDirRefB, double *magMomB, const double *sunDirRefB, double *magMomB, double &errorAngle) {
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);
@ -68,7 +72,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun); errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, calculateRotationalRateTorque(sunDirB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf); acsParameters->safeModeControllerParameters.k_orthoNonMekf);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
@ -82,6 +86,32 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
calculateMagneticMoment(magMomB); 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<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// calculate error angle between sunDirRef and sunDir
double dotSun = VectorOperations<double>::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, void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB, double &errorAngle) { const double *sunDirRefB, double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T // convert magFieldB from uT to T
@ -91,7 +121,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
errorAngle = NAN; errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB); splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, calculateRotationalRateTorque(sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf); acsParameters->safeModeControllerParameters.k_orthoNonMekf);
@ -110,9 +140,8 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
} }
void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, double &errorAngle,
double &errorAngle, const double gainParallel, const double gainParallel, const double gainOrtho) {
const double gainOrtho) {
// calculate torque for parallel rotational rate // calculate torque for parallel rotational rate
VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);

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

@ -14,23 +14,34 @@ 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 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, void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
const double *quatBI, const double *sunDirRefB, double *magMomB, const double *quatBI, const double *sunDirRefB, double *magMomB,
double &errorAngle); double &errorAngle);
void safeNonMekf(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 safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, void safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
double *magMomB, double &errorAngle); 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 splitRotationalRate(const double *satRotRateB, const double *sunDirB);
void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, void calculateRotationalRates(const double *magFieldB, const double *magRateB,
double &errorAngle, const double gainParallel, const double *sunDirB, const double *sunRateB,
const double gainOrtho); double *fusedRotRate);
void calculateRotationalRateTorque(const double *sunDirB, double &errorAngle,
const double gainParallel, const double gainOrtho);
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
const double gainAlign); const double gainAlign);