From c48be8f37bc0644ce76c85f6c9005ae8309fdeff Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 15:30:54 +0200 Subject: [PATCH] stuff --- mission/controller/AcsController.cpp | 11 ++++--- mission/controller/acs/control/SafeCtrl.cpp | 35 ++++++++++++++++----- 2 files changed, 35 insertions(+), 11 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index e163fd67..3c584ba2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -146,6 +146,7 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && @@ -173,6 +174,7 @@ void AcsController::performSafe() { mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), acsParameters.safeModeControllerParameters.useMekf, + acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { case (acs::SafeModeStrategy::SAFECTRL_MEKF): @@ -184,19 +186,20 @@ void AcsController::performSafe() { break; case (acs::SafeModeStrategy::SAFECTRL_GYR): safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + 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, - sunTargetDir, magMomMtq, errAng); + safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value, + gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, + errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 8c06c976..e37d8c3d 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -73,10 +73,10 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const splitRotationalRate(satRotRateB, sunDirB); calculateRotationalRateTorque(sunDirB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelNonMekf, - acsParameters->safeModeControllerParameters.k_orthoNonMekf); + acsParameters->safeModeControllerParameters.k_parallelGyr, + acsParameters->safeModeControllerParameters.k_orthoGyr); calculateAngleErrorTorque(sunDirB, sunDirRefB, - acsParameters->safeModeControllerParameters.k_alignNonMekf); + acsParameters->safeModeControllerParameters.k_alignGyr); // sum of all torques for (uint8_t i = 0; i < 3; i++) { @@ -112,8 +112,8 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel calculateMagneticMoment(magMomB); } -void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, - const double *sunDirRefB, double *magMomB, double &errorAngle) { +void SafeCtrl::safeRateDampingGyr(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -122,8 +122,29 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate splitRotationalRate(satRotRateB, sunDirRefB); calculateRotationalRateTorque(sunDirRefB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelNonMekf, - acsParameters->safeModeControllerParameters.k_orthoNonMekf); + acsParameters->safeModeControllerParameters.k_parallelGyr, + acsParameters->safeModeControllerParameters.k_orthoGyr); + + // sum of all torques + VectorOperations::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::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 VectorOperations::add(cmdParallel, cmdOrtho, cmdTorque, 3);