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

extended state machine and corrected controller calls
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-20 11:32:36 +02:00
parent 32e2d87143
commit 0d501e5064
3 changed files with 21 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
mission/controller/AcsController.cpp
View File

@ -176,6 +176,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(),
acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse); acsParameters.safeModeControllerParameters.dampingDuringEclipse);
@ -194,7 +195,8 @@ void AcsController::performSafe() {
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
@ -206,6 +208,13 @@ void AcsController::performSafe() {
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
errAng = NAN; errAng = NAN;
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;

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

@ -9,22 +9,22 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {} SafeCtrl::~SafeCtrl() {}
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
const bool satRotRateValid, const bool sunDirValid, const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
const uint8_t mekfEnabled, const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid,
const uint8_t gyrEnabled, 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_MEKF; return acs::SafeModeStrategy::SAFECTRL_MEKF;
} else if (gyrEnabled and satRotRateValid and sunDirValid) { } else if (gyrEnabled and satRotRateValid and sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_GYR; return acs::SafeModeStrategy::SAFECTRL_GYR;
} else if (not gyrEnabled and sunDirValid) { } else if (not gyrEnabled and sunDirValid and fusedRateSplitValid) {
return acs::SafeModeStrategy::SAFECTRL_SUSMGM; return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
} else if (gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { } else if (gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
} else if (not gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { } else if (not gyrEnabled and dampingEnabled and satRotRateValid and fusedRateTotalValid and
not sunDirValid) {
return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; 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;

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

@ -14,8 +14,9 @@ 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 gyrEnabled, const bool fusedRateSplitValid,
const uint8_t dampingEnabled); const bool fusedRateTotalValid, 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,