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

stuff
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-07-19 15:30:54 +02:00
parent 9dfc8a0e93
commit c48be8f37b
2 changed files with 35 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

@ -146,6 +146,7 @@ void AcsController::performSafe() {
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters); &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters); &susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
@ -173,6 +174,7 @@ void AcsController::performSafe() {
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useMekf,
acsParameters.safeModeControllerParameters.useGyr,
acsParameters.safeModeControllerParameters.dampingDuringEclipse); acsParameters.safeModeControllerParameters.dampingDuringEclipse);
switch (safeCtrlStrat) { switch (safeCtrlStrat) {
case (acs::SafeModeStrategy::SAFECTRL_MEKF): case (acs::SafeModeStrategy::SAFECTRL_MEKF):
@ -184,19 +186,20 @@ void AcsController::performSafe() {
break; break;
case (acs::SafeModeStrategy::SAFECTRL_GYR): case (acs::SafeModeStrategy::SAFECTRL_GYR):
safeCtrl.safeGyr(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_SUSMGM): case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
safeCtrl.safeGyr(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_GYR): case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
sunTargetDir, magMomMtq, errAng); gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
errAng);
safeCtrlFailureFlag = false; safeCtrlFailureFlag = false;
safeCtrlFailureCounter = 0; safeCtrlFailureCounter = 0;
break; break;

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

@ -73,10 +73,10 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const
splitRotationalRate(satRotRateB, sunDirB); splitRotationalRate(satRotRateB, sunDirB);
calculateRotationalRateTorque(sunDirB, errorAngle, calculateRotationalRateTorque(sunDirB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_orthoNonMekf); acsParameters->safeModeControllerParameters.k_orthoGyr);
calculateAngleErrorTorque(sunDirB, sunDirRefB, calculateAngleErrorTorque(sunDirB, sunDirRefB,
acsParameters->safeModeControllerParameters.k_alignNonMekf); acsParameters->safeModeControllerParameters.k_alignGyr);
// sum of all torques // sum of all torques
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
@ -112,8 +112,8 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel
calculateMagneticMoment(magMomB); calculateMagneticMoment(magMomB);
} }
void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, void SafeCtrl::safeRateDampingGyr(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
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3); VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@ -122,8 +122,29 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
splitRotationalRate(satRotRateB, sunDirRefB); splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, errorAngle, calculateRotationalRateTorque(sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_orthoNonMekf); acsParameters->safeModeControllerParameters.k_orthoGyr);
// sum of all torques
VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
// calculate magnetic moment to command
calculateMagneticMoment(magMomB);
}
void SafeCtrl::safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB,
const double *sunDirRefB, double *magMomB,
double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// no error angle available for eclipse
errorAngle = NAN;
splitRotationalRate(satRotRateB, sunDirRefB);
calculateRotationalRateTorque(sunDirRefB, errorAngle,
acsParameters->safeModeControllerParameters.k_parallelSusMgm,
acsParameters->safeModeControllerParameters.k_orthoSusMgm);
// sum of all torques // sum of all torques
VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3); VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);