First Version of ACS Controller #329
128
mission/controller/acs/AcsParameters.cpp
Normal file
128
mission/controller/acs/AcsParameters.cpp
Normal file
@ -0,0 +1,128 @@
|
||||
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||
#include "AcsParameters.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <cmath>
|
||||
|
||||
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;
|
||||
|
||||
}*/
|
||||
201
mission/controller/acs/AcsParameters.h
Normal file
201
mission/controller/acs/AcsParameters.h
Normal file
@ -0,0 +1,201 @@
|
||||
/*******************************
|
||||
* EIVE Flight Software Framework (FSFW)
|
||||
* (c) 2022 IRS, Uni Stuttgart
|
||||
*******************************/
|
||||
|
||||
#ifndef ACSPARAMETERS_H_
|
||||
#define ACSPARAMETERS_H_
|
||||
|
||||
#include <fsfw/parameters/HasParametersIF.h>
|
||||
|
||||
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_ */
|
||||
Loading…
Reference in New Issue
Block a user