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