eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

cleaned up SafeCtrl
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-02-28 09:18:44 +01:00
parent 26369039da
commit d86b9d55da
2 changed files with 12 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -1,10 +1,3 @@
/*
* SafeCtrl.cpp
*
* Created on: 19 Apr 2022
* Author: Robin Marquardt
*/
#include "SafeCtrl.h" #include "SafeCtrl.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>
@ -15,19 +8,10 @@
#include "../util/MathOperations.h" #include "../util/MathOperations.h"
SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
loadAcsParameters(acsParameters_);
MatrixOperations<double>::multiplyScalar(*(inertiaEIVE->inertiaMatrix), 10, *gainMatrixInertia, 3,
3);
}
SafeCtrl::~SafeCtrl() {} SafeCtrl::~SafeCtrl() {}
void SafeCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
safeModeControllerParameters = &(acsParameters_->safeModeControllerParameters);
inertiaEIVE = &(acsParameters_->inertiaEIVE);
}
ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid, ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
double *magFieldModel, bool magFieldModelValid, double *magFieldModel, bool magFieldModelValid,
double *sunDirModel, bool sunDirModelValid, double *satRateMekf, 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; double kRate = 0, kAlign = 0;
kRate = safeModeControllerParameters->k_rate_mekf; kRate = acsParameters->safeModeControllerParameters.k_rate_mekf;
kAlign = safeModeControllerParameters->k_align_mekf; kAlign = acsParameters->safeModeControllerParameters.k_align_mekf;
// Calc sunDirB ,magFieldB with mekf output and model // Calc sunDirB ,magFieldB with mekf output and model
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; 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<double>::add(torqueRate, torqueAlign, torqueAll, 3); VectorOperations<double>::add(torqueRate, torqueAlign, torqueAll, 3);
// Adding factor of inertia for axes // Adding factor of inertia for axes
MatrixOperations<double>::multiplyScalar(*(acsParameters->inertiaEIVE.inertiaMatrix), 10,
*gainMatrixInertia, 3,
3); // why only for mekf one and not for no mekf
MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1); MatrixOperations<double>::multiply(*gainMatrixInertia, torqueAll, torqueCmd, 3, 3, 1);
// MagMom B (orthogonal torque) // 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 /* Only valid if angle between sun direction and magnetic field direction
* is sufficiently large */ * is sufficiently large */
double angleSunMag = acos(cosAngleSunMag); double angleSunMag = acos(cosAngleSunMag);
if (angleSunMag < safeModeControllerParameters->sunMagAngleMin) { if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) {
return; return;
} }
@ -139,8 +126,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
// Torque Align calculation // Torque Align calculation
double kRateNoMekf = 0, kAlignNoMekf = 0; double kRateNoMekf = 0, kAlignNoMekf = 0;
kRateNoMekf = safeModeControllerParameters->k_rate_no_mekf; kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf;
kAlignNoMekf = safeModeControllerParameters->k_align_no_mekf; kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf;
double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB); double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
double crossSusSunRef[3] = {0, 0, 0}; double crossSusSunRef[3] = {0, 0, 0};
@ -159,8 +146,8 @@ void SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, doubl
// Final torque // Final torque
double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0}; double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3); VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), torqueAlignRate, torqueB, 3, 3, MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate,
1); torqueB, 3, 3, 1);
// Magnetic moment // Magnetic moment
double magMomB[3] = {0, 0, 0}; double magMomB[3] = {0, 0, 0};

7
mission/controller/acs/control/SafeCtrl.h
View File

@ -17,8 +17,6 @@ class SafeCtrl {
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE; static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01); 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, ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid, bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
double *satRateMekf, bool rateMekfValid, double *sunDirRef, double *satRateMekf, bool rateMekfValid, double *sunDirRef,
@ -30,12 +28,9 @@ class SafeCtrl {
bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle, bool magRateBValid, double *sunDirRef, double *satRateRef, double *outputAngle,
double *outputMagMomB, bool *outputValid); double *outputMagMomB, bool *outputValid);
void idleSunPointing(); // with reaction wheels
protected: protected:
private: private:
AcsParameters::SafeModeControllerParameters *safeModeControllerParameters; AcsParameters *acsParameters;
AcsParameters::InertiaEIVE *inertiaEIVE;
double gainMatrixInertia[3][3]; double gainMatrixInertia[3][3];
double magFieldBState[3]; double magFieldBState[3];