From 673826b1315dafa61d1334f2fa8745fd4bd55045 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 13 Apr 2023 15:02:07 +0200 Subject: [PATCH] removed even more deprecated code --- mission/controller/AcsController.cpp | 13 ++++++------- mission/controller/acs/Guidance.cpp | 4 +--- mission/controller/acs/Guidance.h | 2 +- mission/controller/acs/control/SafeCtrl.cpp | 21 +++++++++------------ mission/controller/acs/control/SafeCtrl.h | 19 +++++++++---------- 5 files changed, 26 insertions(+), 33 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b0baeddf..8a7ac78c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -165,9 +165,8 @@ void AcsController::performSafe() { } // get desired satellite rate, sun direction to align to and inertia - double satRateSafe = 0, sunTargetDir[3] = {0, 0, 0}, - inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - guidance.getTargetParamsSafe(sunTargetDir, satRateSafe, inertiaEive); + double sunTargetDir[3] = {0, 0, 0}, inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + guidance.getTargetParamsSafe(sunTargetDir, inertiaEive); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( @@ -179,20 +178,20 @@ void AcsController::performSafe() { case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, - satRateSafe, inertiaEive, magMomMtq, errAng); + inertiaEive, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF): safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - susDataProcessed.susVecTot.value, sunTargetDir, satRateSafe, inertiaEive, - magMomMtq, errAng); + susDataProcessed.susVecTot.value, sunTargetDir, inertiaEive, magMomMtq, + errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - satRateSafe, sunTargetDir, magMomMtq, errAng); + sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 81160efe..646bcd3f 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -539,8 +539,7 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, } } -void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double &satRateSafe, - double inertiaEive[3][3]) { +void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double inertiaEive[3][3]) { std::error_code e; if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) { @@ -554,7 +553,6 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double &satRateSafe, std::memcpy(inertiaEive, acsParameters->inertiaEIVE.inertiaMatrixUndeployed, 3 * 3 * sizeof(double)); } - satRateSafe = acsParameters->safeModeControllerParameters.satRateRef; } ReturnValue_t Guidance::solarArrayDeploymentComplete() { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 0335fca7..e0aaafab 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -12,7 +12,7 @@ class Guidance { Guidance(AcsParameters *acsParameters_); virtual ~Guidance(); - void getTargetParamsSafe(double sunTargetSafe[3], double &satRateRef, double inertiaEive[3][3]); + void getTargetParamsSafe(double sunTargetSafe[3], double inertiaEive[3][3]); ReturnValue_t solarArrayDeploymentComplete(); // Function to get the target quaternion and refence rotation rate from gps position and diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 940be1c3..1d2244ae 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -29,8 +29,7 @@ uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, - const double satRotRateRef, const double inertiaMatrix[3][3], - double *magMomB, double &errorAngle) { + const double inertiaMatrix[3][3], double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -43,7 +42,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_orthoMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, @@ -59,8 +58,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB, const double *sunDirRefB, - const double satRotRateRef, const double inertiaMatrix[3][3], - double *magMomB, double &errorAngle) { + const double inertiaMatrix[3][3], double *magMomB, double &errorAngle) { // convert magFieldB from uT to T double magFieldBT[3] = {0, 0, 0}; VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -70,7 +68,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, @@ -86,8 +84,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, } void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, - const double satRotRateRef, const double *sunDirRefB, - double *magMomB, double &errorAngle) { + const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -95,7 +92,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate errorAngle = NAN; splitRotationalRate(satRotRateB, sunDirRefB); - calculateRotationalRateTorque(satRotRateRef, sunDirRefB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf); @@ -115,9 +112,9 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD VectorOperations::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); } -void SafeCtrl::calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB, - const double *sunDirRefB, double &errorAngle, - const double gainParallel, const double gainOrtho) { +void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, + double &errorAngle, const double gainParallel, + const double gainOrtho) { // calculate torque for parallel rotational rate VectorOperations::mulScalar(cmdParallel, -gainParallel, cmdParallel, 3); diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 1d358904..108321df 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -17,22 +17,21 @@ class SafeCtrl { const uint8_t mekfEnabled, const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, - const double *quatBI, const double *sunDirRefB, const double satRotRateRef, - const double inertiaMatrix[3][3], double *magMomB, double &errorAngle); + const double *quatBI, const double *sunDirRefB, const double inertiaMatrix[3][3], + double *magMomB, double &errorAngle); void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB, - const double *sunDirRefB, const double satRotRateRef, - const double inertiaMatrix[3][3], double *magMomB, double &errorAngle); + const double *sunDirRefB, const double inertiaMatrix[3][3], double *magMomB, + double &errorAngle); - void safeRateDamping(const double *magFieldB, const double *satRotRateB, - const double satRotRateRef, const double *sunDirRefB, double *magMomB, - double &errorAngle); + void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, + double *magMomB, double &errorAngle); void splitRotationalRate(const double *satRotRateB, const double *sunDirB); - void calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB, - const double *sunDirRefB, double &errorAngle, - const double gainParallel, const double gainOrtho); + void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, + double &errorAngle, const double gainParallel, + const double gainOrtho); void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, const double gainAlign, const double inertiaMatrix[3][3]);