diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index aa04cbb6..734c6a0c 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -1,10 +1,3 @@ -/* - * SafeCtrl.cpp - * - * Created on: 19 Apr 2022 - * Author: Robin Marquardt - */ - #include "SafeCtrl.h" #include @@ -15,19 +8,10 @@ #include "../util/MathOperations.h" -SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { - loadAcsParameters(acsParameters_); - MatrixOperations::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3, - 3); -} +SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } SafeCtrl::~SafeCtrl() {} -void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) { - safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters); - inertiaEIVE = &(acsParameters_->inertiaEIVE); -} - ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, @@ -39,8 +23,8 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, } double kRate = 0, kAlign = 0; - kRate = safeModeControllerParameters->k_rate_mekf; - kAlign = safeModeControllerParameters->k_align_mekf; + kRate = acsParameters->safeModeControllerParameters.k_rate_mekf; + kAlign = acsParameters->safeModeControllerParameters.k_align_mekf; // Calc sunDirB ,magFieldB with mekf output and model double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; @@ -73,6 +57,9 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, VectorOperations::add(torqueRate, torqueAlign, torqueAll, 3); // Adding factor of inertia for axes + MatrixOperations::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10, + *gainMatrixInertia, 3, + 3); // why only for mekf one and not for no mekf MatrixOperations::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); // MagMom B (orthogonal torque) @@ -129,7 +116,7 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl /* Only valid if angle between sun direction and magnetic field direction * is sufficiently large */ double angleSunMag = acos(cosAngleSunMag); - if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { + if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) { return; } @@ -139,8 +126,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl // Torque Align calculation double kRateNoMekf = 0, kAlignNoMekf = 0; - kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; - kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; + kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf; + kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf; double cosAngleAlignErr = VectorOperations::dot(sunDirRef, susDirB); double crossSusSunRef[3] = {0, 0, 0}; @@ -159,8 +146,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl // Final torque double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; VectorOperations::add(torqueRate, torqueAlign, torqueAlignRate, 3); - MatrixOperations::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, - 1); + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate, + torqueB, 3, 3, 1); // Magnetic moment double magMomB[3] = {0, 0, 0}; diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 1784f9ca..f14bc464 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -17,8 +17,6 @@ class SafeCtrl { static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE; static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); - void loadAcsParameters(AcsParameters *acsParameters_); - ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, double *satRateMekf, bool rateMekfValid, double *sunDirRef, @@ -30,12 +28,9 @@ class SafeCtrl { bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle, double *outputMagMomB, bool *outputValid); - void idleSunPointing(); // with reaction wheels - protected: private: - AcsParameters::SafeModeControllerParameters *safeModeControllerParameters; - AcsParameters::InertiaEIVE *inertiaEIVE; + AcsParameters *acsParameters; double gainMatrixInertia[3][3]; double magFieldBState[3];