From 39b2a3420cb6d1e0eaf228f4690672f9c7ca4011 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 5 Apr 2023 16:46:57 +0200 Subject: [PATCH] changed satRotRateRef to scalar --- mission/controller/AcsController.cpp | 2 +- mission/controller/acs/AcsParameters.cpp | 2 +- mission/controller/acs/AcsParameters.h | 2 +- mission/controller/acs/Guidance.cpp | 5 ++--- mission/controller/acs/Guidance.h | 2 +- mission/controller/acs/control/SafeCtrl.cpp | 21 ++++++++++++--------- mission/controller/acs/control/SafeCtrl.h | 8 ++++---- 7 files changed, 22 insertions(+), 20 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ba2fcdda..2de8e46b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -165,7 +165,7 @@ void AcsController::performSafe() { } // get desired satellite rate, sun direction to align to and inertia - double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}, + 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); diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 4d963bec..3ace33d9 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -351,7 +351,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); break; case 0xA: - parameterWrapper->setVector(safeModeControllerParameters.satRateRef); + parameterWrapper->set(safeModeControllerParameters.satRateRef); break; case 0xB: parameterWrapper->set(safeModeControllerParameters.angleStartSpin); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index bc78e34d..77229961 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -828,7 +828,7 @@ class AcsParameters : public HasParametersIF { double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)}; double sunTargetDir[3] = {0, 0, 1}; - double satRateRef[3] = {0, 0, 0}; + double satRateRef = 0; float angleStartSpin = 20 * M_PI / 180; } safeModeControllerParameters; diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4368e014..d62225a6 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -539,7 +539,7 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, } } -void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3], +void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double &satRateSafe, double inertiaEive[3][3]) { std::error_code e; if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or @@ -554,8 +554,7 @@ void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3 std::memcpy(inertiaEive, acsParameters->inertiaEIVE.inertiaMatrixUndeployed, sizeof(&inertiaEive)); } - std::memcpy(satRateSafe, acsParameters->safeModeControllerParameters.satRateRef, - sizeof(&satRateSafe)); + satRateSafe = acsParameters->safeModeControllerParameters.satRateRef; } ReturnValue_t Guidance::solarArrayDeploymentComplete() { diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 08183534..0335fca7 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[3], double inertiaEive[3][3]); + void getTargetParamsSafe(double sunTargetSafe[3], double &satRateRef, 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 b16faf99..681ee97a 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -30,7 +30,7 @@ ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnV void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, - const double *satRotRateRefB, const double inertiaMatrix[3][3], + const double satRotRateRef, const double inertiaMatrix[3][3], double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -44,7 +44,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_orthoMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, @@ -60,7 +60,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 *satRotRateRefB, const double inertiaMatrix[3][3], + const double satRotRateRef, const double inertiaMatrix[3][3], double *magMomB, double &errorAngle) { // convert magFieldB from uT to T double magFieldBT[3] = {0, 0, 0}; @@ -71,7 +71,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(satRotRateRefB, sunDirB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(satRotRateRef, sunDirB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, @@ -87,7 +87,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, } void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, - const double *satRotRateRefB, const double *sunDirRefB, + const double satRotRateRef, const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -96,7 +96,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate errorAngle = NAN; splitRotationalRate(satRotRateB, sunDirRefB); - calculateRotationalRateTorque(satRotRateRefB, sunDirRefB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(satRotRateRef, sunDirRefB, sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf); @@ -116,14 +116,17 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD VectorOperations::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); } -void SafeCtrl::calculateRotationalRateTorque(const double *satRotRateRefB, const double *sunDirB, +void SafeCtrl::calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB, const double *sunDirRefB, double &errorAngle, const double gainParallel, const double gainOrtho) { // calculate torque for parallel rotational rate if ((isfinite(errorAngle)) and (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin)) { - VectorOperations::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3); - VectorOperations::mulScalar(cmdParallel, gainParallel, cmdParallel, 3); + double satRotRateNorm = VectorOperations::norm(satRotRateParallelB, 3); + double satRotRateUnitVec[3] = {0, 0, 0}; + VectorOperations::normalize(satRotRateParallelB, satRotRateUnitVec, 3); + VectorOperations::mulScalar(satRotRateUnitVec, satRotRateRef - satRotRateNorm, + cmdParallel, 3); } else { VectorOperations::mulScalar(cmdParallel, -gainParallel, cmdParallel, 3); } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 35ffa1a5..f33b55ad 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -26,20 +26,20 @@ class SafeCtrl { const bool satRotRateValid, const bool sunDirValid); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, - const double *quatBI, const double *sunDirRefB, const double *satRotRateRefB, + const double *quatBI, const double *sunDirRefB, const double satRotRateRef, 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 *satRotRateRefB, + const double *sunDirRefB, const double satRotRateRef, const double inertiaMatrix[3][3], double *magMomB, double &errorAngle); void safeRateDamping(const double *magFieldB, const double *satRotRateB, - const double *satRotRateRefB, const double *sunDirRefB, double *magMomB, + const double satRotRateRef, const double *sunDirRefB, double *magMomB, double &errorAngle); void splitRotationalRate(const double *satRotRateB, const double *sunDirB); - void calculateRotationalRateTorque(const double *satRotRateRefB, const double *sunDirB, + void calculateRotationalRateTorque(const double satRotRateRef, const double *sunDirB, const double *sunDirRefB, double &errorAngle, const double gainParallel, const double gainOrtho);