diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1a19ff87..cd9e7bdb 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -165,8 +165,8 @@ void AcsController::performSafe() { } // get desired satellite rate, sun direction to align to and inertia - double sunTargetDir[3] = {0, 0, 0}, inertiaEive[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - guidance.getTargetParamsSafe(sunTargetDir, inertiaEive); + double sunTargetDir[3] = {0, 0, 0}; + guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( @@ -178,20 +178,19 @@ void AcsController::performSafe() { case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, - inertiaEive, magMomMtq, errAng); + 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, inertiaEive, magMomMtq, - errAng); + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - sunTargetDir, inertiaEive, 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 646bcd3f..79c6b416 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -539,19 +539,15 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, } } -void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double inertiaEive[3][3]) { +void Guidance::getTargetParamsSafe(double sunTargetSafe[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)) { std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir, 3 * sizeof(double)); - std::memcpy(inertiaEive, acsParameters->inertiaEIVE.inertiaMatrixDeployed, - 3 * 3 * sizeof(double)); } else { std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop, 3 * sizeof(double)); - std::memcpy(inertiaEive, acsParameters->inertiaEIVE.inertiaMatrixUndeployed, - 3 * 3 * sizeof(double)); } } diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index e0aaafab..a52c476a 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 inertiaEive[3][3]); + void getTargetParamsSafe(double sunTargetSafe[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 95a27e92..d36dcec9 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -29,7 +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 inertiaMatrix[3][3], double *magMomB, double &errorAngle) { + double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -42,9 +42,9 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque( - sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelMekf, - acsParameters->safeModeControllerParameters.k_orthoMekf, inertiaMatrix); + calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, + acsParameters->safeModeControllerParameters.k_parallelMekf, + acsParameters->safeModeControllerParameters.k_orthoMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, acsParameters->safeModeControllerParameters.k_alignMekf); @@ -57,8 +57,8 @@ 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 inertiaMatrix[3][3], double *magMomB, double &errorAngle) { + const double *sunDirB, const double *sunDirRefB, double *magMomB, + double &errorAngle) { // convert magFieldB from uT to T double magFieldBT[3] = {0, 0, 0}; VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -70,8 +70,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, splitRotationalRate(satRotRateB, sunDirB); calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, - acsParameters->safeModeControllerParameters.k_orthoNonMekf, - inertiaMatrix); + acsParameters->safeModeControllerParameters.k_orthoNonMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, acsParameters->safeModeControllerParameters.k_alignNonMekf); @@ -84,8 +83,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, } void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, - const double *sunDirRefB, const double inertiaMatrix[3][3], - 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,8 +93,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate splitRotationalRate(satRotRateB, sunDirRefB); calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, - acsParameters->safeModeControllerParameters.k_orthoNonMekf, - inertiaMatrix); + acsParameters->safeModeControllerParameters.k_orthoNonMekf); // sum of all torques double cmdTorque[3] = {0, 0, 0}; @@ -116,15 +113,12 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, double &errorAngle, const double gainParallel, - const double gainOrtho, - const double inertiaMatrix[3][3]) { + const double gainOrtho) { // calculate torque for parallel rotational rate VectorOperations::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); // calculate torque for orthogonal rotational rate - double orthoFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MatrixOperations::multiplyScalar(*inertiaMatrix, -gainOrtho, *orthoFactor, 3, 3); - MatrixOperations::multiply(*orthoFactor, satRotRateOrthogonalB, cmdOrtho, 3, 3, 1); + VectorOperations::mulScalar(satRotRateOrthogonalB, -gainOrtho, cmdOrtho, 3); } void SafeCtrl::calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index f0ab8a3c..2df5d500 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -17,21 +17,20 @@ 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 inertiaMatrix[3][3], - double *magMomB, double &errorAngle); + const double *quatBI, const double *sunDirRefB, double *magMomB, + double &errorAngle); void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB, - const double *sunDirRefB, const double inertiaMatrix[3][3], double *magMomB, - double &errorAngle); + const double *sunDirRefB, double *magMomB, double &errorAngle); void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, - const double inertiaMatrix[3][3], double *magMomB, double &errorAngle); + double *magMomB, double &errorAngle); void splitRotationalRate(const double *satRotRateB, const double *sunDirB); void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, double &errorAngle, const double gainParallel, - const double gainOrtho, const double inertiaMatrix[3][3]); + const double gainOrtho); void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, const double gainAlign);