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

this gonna be fun
Some checks failed
EIVE/eive-obsw/pipeline/head Build queued...
Details
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-11-02 16:59:09 +01:00
parent 65f907448c
commit 60a348a08f
9 changed files with 90 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
mission/controller/AcsController.cpp
View File

@ -240,27 +240,27 @@ void AcsController::performSafe() {
guidance.getTargetParamsSafe(sunTargetDir);
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_MEKF):
case (acs::ControlModeStrategy::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_GYR):
case (acs::ControlModeStrategy::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_SUSMGM):
case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
fusedRotRateData.rotRateParallel.value,
fusedRotRateData.rotRateOrthogonal.value,
@ -268,29 +268,29 @@ void AcsController::performSafe() {
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING):
errAng = NAN;
safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0;
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1);
break;
default:
@ -335,24 +335,24 @@ void AcsController::performSafe() {
}
void AcsController::performDetumble() {
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
mgmDataProcessed.mgmVecTotDerivative.isValid(),
acsParameters.detumbleParameter.useFullDetumbleLaw);
double magMomMtq[3] = {0, 0, 0};
switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL):
detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainFull);
break;
case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
magMomMtq, acsParameters.detumbleParameter.gainBdot);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
safeCtrlFailure(1, 0);
break;
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
safeCtrlFailure(0, 1);
break;
default:
@ -668,7 +668,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
ctrlValData.errAng.value = errAng;
std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF;
ctrlValData.safeStrat.value = acs::ControlModeStrategy::SAFECTRL_OFF;
ctrlValData.setValidity(true, true);
}
}