From d9427c61125ad770a2a60ab9776a2a626adffe85 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Fri, 16 Sep 2022 14:13:41 +0200 Subject: [PATCH] added AcsParameters --- mission/controller/acs/AcsParameters.cpp | 128 +++++++++++++++ mission/controller/acs/AcsParameters.h | 201 +++++++++++++++++++++++ 2 files changed, 329 insertions(+) create mode 100644 mission/controller/acs/AcsParameters.cpp create mode 100644 mission/controller/acs/AcsParameters.h diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp new file mode 100644 index 00000000..91090028 --- /dev/null +++ b/mission/controller/acs/AcsParameters.cpp @@ -0,0 +1,128 @@ +#include +#include "AcsParameters.h" + +#include +#include + +AcsParameters::AcsParameters() { + + onBoardParams.sampleTime = 0.1; + + inertiaEIVE.inertiaMatrix = {{ 1, 0, 0}, + { 0, 1, 0}, + { 0,0.5, 1}}; + + mgmHandlingParameters.mgm0orientationMatrix = {{ 0, 0,-1}, + { 0, 1, 0}, + { 1, 0, 0}}; + /*mgmHandlingParameters.mgm1orientationMatrix = {{ 0, 0, 1}, + { 0,-1, 0}, + { 1, 0, 0}}; + mgmHandlingParameters.mgm2orientationMatrix = {{ 0, 0,-1}, + { 0, 1, 0}, + { 1 ,0, 0}}; + mgmHandlingParameters.mgm3orientationMatrix = {{ 0, 0, 1}, + { 0,-1, 0}, + { 1, 0, 0}}; + mgmHandlingParameters.mgm4orientationMatrix = {{ 0, 0,-1}, + {-1, 0, 0}, + { 0, 1, 0}};*/ + + rwHandlingParameters.inertiaWheel = 0.000028198; + rwHandlingParameters.maxTrq = 0.0032; //3.2 [mNm] + + //Geometry frame + rwMatrices.alignmentMatrix = {{ 0.9205, 0,-0.9205, 0}, + { 0,-0.9205, 0, 0.9205}, + { 0.3907, 0.3907, 0.3907, 0.3907}}; + + rwMatrices.pseudoInverse = {{ 0.4434,-0.2845, 0.3597}, + { 0.2136,-0.3317, 1.0123}, + {-0.8672,-0.1406, 0.1778}, + { 0.6426, 0.4794, 1.3603}}; + + rwMatrices.nullspace = {-0.7358, 0.5469,-0.3637,-0.1649}; + + kalmanFilterParameters.sensorNoiseSS = 8 * Math::PI / 180; + kalmanFilterParameters.sensorNoiseMAG = 4 * Math::PI / 180; + kalmanFilterParameters.sensorNoiseSTR = 0.1 * Math::PI / 180; + + sunModelParameters.domega = 36000.771; + sunModelParameters.omega_0 = 282.94 * Math::PI / 180.; + sunModelParameters.dm = 35999.049; + sunModelParameters.m_0 = 357.5256; + sunModelParameters.e = 23.4392911 * Math::PI / 180.; + sunModelParameters.e = 0.74508 * Math::PI / 180.; + sunModelParameters.p1 = 6892. / 3600. * Math::PI / 180.; + sunModelParameters.p2 = 72. / 3600. * Math::PI / 180.; + + safeModeControllerParameters.k_rate_mekf = 0.00059437; + safeModeControllerParameters.k_align_mekf = 0.000056875; + + safeModeControllerParameters.sunTargetDir = {1,0,0}; + + detumbleCtrlParameters.gainD = pow(10.0,-3.3); + + // Stuttgart + groundStationParameters.latitudeGs = 48.7495 * Math::PI / 180.; + groundStationParameters.longitudeGs = 9.10384 * Math::PI / 180.; + groundStationParameters.altitudeGs = 500; + + groundStationParameters.earthRadiusEquat = 6378137; + groundStationParameters.earthRadiusPolar = 6356752.314; + +// Geometry frame + targetModeControllerParameters.refDirection = {1,0,0}; + targetModeControllerParameters.refRotRate = {0,0,0}; + targetModeControllerParameters.quatRef = {0,0,0,1}; + + targetModeControllerParameters.avoidBlindStr = true; + targetModeControllerParameters.blindAvoidStart = 1.5; + targetModeControllerParameters.blindAvoidStop = 2.5; + targetModeControllerParameters.blindRotRate = 1 * Math::PI /180; + + targetModeControllerParameters.zeta = 0.3; + targetModeControllerParameters.om = 0.3; + targetModeControllerParameters.qiMin = 0.1; + targetModeControllerParameters.omMax = 1 * Math::PI / 180; + targetModeControllerParameters.gainNullspace = 0.01; + + targetModeControllerParameters.desatMomentumRef = {0,0,0}; + targetModeControllerParameters.deSatGainFactor = 1000; + targetModeControllerParameters.desatOn = true; + + targetModeControllerParameters.omegaEarth = 0.000072921158553; + + strParameters.boresightAxis = { 0.7593, 0,-0.6508}; + strParameters.exclusionAngle = 20 * Math::PI /180; + + magnetorquesParameter.mtq0orientationMatrix = {{ 1, 0, 0}, + { 0, 0, 1}, + { 0,-1, 0}}; + magnetorquesParameter.mtq1orientationMatrix = {{ 1, 0, 0}, + { 0, 1, 0}, + { 0, 0, 1}}; + magnetorquesParameter.mtq2orientationMatrix = {{ 0, 0, 1}, + { 0, 1, 0}, + {-1, 0, 0}}; + magnetorquesParameter.alignmentMatrixMtq = {{ 0, 0,-1}, + {-1, 0, 0}, + { 0, 1, 0}}; + magnetorquesParameter.inverseAlignment = {{ 0,-1, 0}, + { 0, 0, 1}, + {-1, 0, 0}}; + magnetorquesParameter.DipolMax = 0.2; + +} + +AcsParameters::~AcsParameters() { +} + +/* +ReturnValue_t AcsParameters::getParameter(uint8_t domainId, + uint16_t parameterId, ParameterWrapper* parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex) { + + return 0; + +}*/ diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h new file mode 100644 index 00000000..b4e32d65 --- /dev/null +++ b/mission/controller/acs/AcsParameters.h @@ -0,0 +1,201 @@ +/******************************* + * EIVE Flight Software Framework (FSFW) + * (c) 2022 IRS, Uni Stuttgart + *******************************/ + +#ifndef ACSPARAMETERS_H_ +#define ACSPARAMETERS_H_ + +#include + +typedef unsigned char uint8_t; + +class AcsParameters:public HasParametersIF{ +public: + + AcsParameters(); + virtual ~AcsParameters(); + + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex); + + struct OnBoardParams { + double sampleTime; // [s] + } onBoardParams; + + struct InertiaEIVE { + double inertiaMatrix[3][3]; + double inertiaMatrixInverse[3][3]; + } inertiaEIVE; + + struct MgmHandlingParameters { + float mgm0orientationMatrix[3][3]; + float mgm1orientationMatrix[3][3]; + float mgm2orientationMatrix[3][3]; + float mgm3orientationMatrix[3][3]; + float mgm4orientationMatrix[3][3]; + } mgmHandlingParameters; + + struct SusHandlingParameters { + float sus0orientationMatrix[3][3]; + float sus1orientationMatrix[3][3]; + float sus2orientationMatrix[3][3]; + float sus3orientationMatrix[3][3]; + float sus4orientationMatrix[3][3]; + float sus5orientationMatrix[3][3]; + float sus6orientationMatrix[3][3]; + float sus7orientationMatrix[3][3]; + float sus8orientationMatrix[3][3]; + float sus9orientationMatrix[3][3]; + float sus10orientationMatrix[3][3]; + float sus11orientationMatrix[3][3]; + + float filterAlpha; + float sunThresh; + } susHandlingParameters; + + struct RmuHandlingParameters { + double rmu0orientationMatrix[3][3]; + double rmu1orientationMatrix[3][3]; + double rmu2orientationMatrix[3][3]; + } rmuHandlingParameters; + + struct RwHandlingParameters { + double rw0orientationMatrix[3][3]; + double rw1orientationMatrix[3][3]; + double rw2orientationMatrix[3][3]; + double rw3orientationMatrix[3][3]; + double inertiaWheel; + double maxTrq; + } rwHandlingParameters; + + struct SafeModeControllerParameters { + double k_rate_mekf; + double k_align_mekf; + + double k_rate_no_mekf; + double k_align_no_mekf; + double sunMagAngleMin; + + double sunTargetDir[3]; //Body frame + double satRateRef[3]; //Body frame + + } safeModeControllerParameters; + + struct DetumbleCtrlParameters { + + double gainD; + + } detumbleCtrlParameters; + + + struct PointingModeControllerParameters { + double updtFlag; + double A_rw[3][12]; + + double refDirection[3]; + double refRotRate[3]; + double quatRef[4]; + bool avoidBlindStr; + double blindAvoidStart; + double blindAvoidStop; + double blindRotRate; + + double zeta; + double zetaLow; + double om; + double omLow; + double qiMin; + double omMax; + double gainNullspace; + + double desatMomentumRef[3]; + double deSatGainFactor; + bool desatOn; + + double omegaEarth; + + + } inertialModeControllerParameters, nadirModeControllerParameters, + targetModeControllerParameters; + + + struct RWMatrices { + double alignmentMatrix[3][4]; + double pseudoInverse[4][3]; + double without0[4][3]; + double without1[4][3]; + double without2[4][3]; + double without3[4][3]; + double nullspace[4]; + } rwMatrices; + + struct StrParameters { + double exclusionAngle; +// double strOrientationMatrix[3][3]; + double boresightAxis[3]; //in body/geometry frame + } strParameters; + + struct GpsParameters { + } gpsParameters; + + struct GroundStationParameters { + double latitudeGs; // [rad] Latitude + double longitudeGs; // [rad] Longitude + double altitudeGs; // [m] Altitude + double earthRadiusEquat; // [m] + double earthRadiusPolar; // [m] + } groundStationParameters; + + struct SunModelParameters { + enum UseSunModel { + NO = 0, YES = 3 + }; + uint8_t useSunModel; + float domega; + float omega_0; //Rektaszension des Aufsteigenden Knotens plus Argument des Perigäums + float m_0; //coefficients for mean anomaly + float dm; //coefficients for mean anomaly + float e; //angle of earth's rotation axis + float e1; + float p1; //some parameter + float p2; //some parameter + } sunModelParameters; + + struct KalmanFilterParameters { + uint8_t activateKalmanFilter; + uint8_t requestResetFlag; + double maxToleratedTimeBetweenKalmanFilterExecutionSteps; + double processNoiseOmega[3]; + double processNoiseQuaternion[4]; + + double sensorNoiseSTR; + double sensorNoiseSS; + double sensorNoiseMAG; + double sensorNoiseRMU[3]; + + double sensorNoiseArwRmu; //Angular Random Walk + double sensorNoiseBsRMU; // Bias Stability + } kalmanFilterParameters; + + struct MagnetorquesParameter { + + double mtq0orientationMatrix[3][3]; + double mtq1orientationMatrix[3][3]; + double mtq2orientationMatrix[3][3]; + double alignmentMatrixMtq[3][3]; + double inverseAlignment[3][3]; + double DipolMax; // [Am^2] + + } magnetorquesParameter; + + struct DetumbleParameter { + + uint8_t detumblecounter; + double omegaDetumbleStart; + double omegaDetumbleEnd; + } detumbleParameter; +}; + +#endif /* ACSPARAMETERS_H_ */