eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 119 Wiki Activity

updated AcsParams, added class function to get quaternion for sun pointing (guidance)
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Marquardt 2022-10-28 18:18:28 +02:00
parent e87221a8a3
commit ba541300ca
9 changed files with 133 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
mission/controller/acs/AcsParameters.h
View File

@ -22,12 +22,22 @@ class AcsParameters /*: public HasParametersIF*/ {
const ParameterWrapper *newValues, uint16_t startAtIndex);
*/
struct OnBoardParams {
double sampleTime = 0.1; // [s]
double sampleTime = 0.4; // [s]
} onBoardParams;
struct InertiaEIVE {
double inertiaMatrix[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.5, 1.0}};
double inertiaMatrixInverse[3][3];
double inertiaMatrix[3][3] = {{0.1539829, -0.0001821456, -0.0050135},
{-0.0001821456, 0.1701302, 0.0004748963},
{-0.0050135, 0.0004748963, 0.08374296}}; //19.11.2021
double inertiaMatrixNoPanel[3][3] = {{0.122485, -0.0001798426, -0.005008},
{-0.0001798426, 0.162240, 0.000475596},
{-0.005008, 0.000475596, 0.060136}}; //19.11.2021
double inertiaMatrixPanel1[3][3] = {{0.13823347, -0.0001836122, -0.00501207},
{-0.0001836122, 0.16619787, 0.0083537},
{-0.00501207, 0.0083537, 0.07192588}}; //19.11.2021
double inertiaMatrixPanel3[3][3] = {{0.13823487, -0.000178376, -0.005009767},
{-0.000178376, 0.166172, -0.007403},
{-0.005009767, -0.007403, 0.07195314}}; //19.11.2021
} inertiaEIVE;
struct MgmHandlingParameters {
@ -768,41 +778,50 @@ class AcsParameters /*: public HasParametersIF*/ {
double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
{0.0000, -0.9205, 0.0000, 0.9205},
{0.3907, 0.3907, 0.3907, 0.3907}};
double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597},
{0.2136, -0.3317, 1.0123},
{-0.8672, -0.1406, 0.1778},
{0.6426, 0.4794, 1.3603}};
double without0[4][3];
double without1[4][3];
double without2[4][3];
double without3[4][3];
double nullspace[4] = {-0.7358, 0.5469, -0.3637, -0.1649};
// double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597},
// {0.2136, -0.3317, 1.0123},
// {-0.8672, -0.1406, 0.1778},
// {0.6426, 0.4794, 1.3603}};
// where does the first pseudo inverse come frome - matlab gives result below
double pseudoInverse[4][3] = {{0.5432, 0, 0.6398},
{0, -0.5432, 0.6398},
{-0.5432, 0, 0.6398},
{0, 0.5432, 0.6398}};
double without0[4][3] = {{0, 0, 0},
{0.5432, -0.5432, 1.2797},
{-1.0864, 0, 0},
{0.5432, 0.5432, 1.2797}};
double without1[4][3] = {{0.5432, -0.5432, 1.2797},
{0, 0, 0},
{-0.5432, -0.5432, 1.2797},
{0, 1.0864, 0}};
double without2[4][3] = {{1.0864, 0, 0},
{-0.5432, -0.5432, 1.2797},
{0, 0, 0},
{-0.5432, 0.5432, 1.2797}};
double without3[4][3] = {{0.5432, 0.5432, 1.2797},
{0, -1.0864, 0},
{-0.5432, 0.5432, 1.2797},
{0, 0, 0}};
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
} rwMatrices;
struct SafeModeControllerParameters {
double k_rate_mekf = 0.00059437;
double k_align_mekf = 0.000056875;
double k_rate_no_mekf;
double k_align_no_mekf;
double sunMagAngleMin;
double k_rate_no_mekf = 0.00059437;;
double k_align_no_mekf = 0.000056875;;
double sunMagAngleMin; // ???
double sunTargetDir[3] = {1, 0, 0}; // Body frame
double satRateRef[3]; // Body frame
double sunTargetDir[3] = {0, 0, 1}; // Geometry Frame
double satRateRef[3] = {0, 0, 0}; // Geometry Frame
} safeModeControllerParameters;
struct DetumbleCtrlParameters {
double gainD = pow(10.0, -3.3);
} detumbleCtrlParameters;
// ToDo: mutiple structs for different pointing mode controllers?
struct PointingModeControllerParameters {
double updtFlag;
double A_rw[3][12];
double refDirection[3] = {1, 0, 0};
double refDirection[3] = {1, 0, 0}; //Antenna
double refRotRate[3] = {0, 0, 0};
double quatRef[4] = {0, 0, 0, 1};
bool avoidBlindStr = true;
@ -811,9 +830,7 @@ class AcsParameters /*: public HasParametersIF*/ {
double blindRotRate = 1 * M_PI / 180;
double zeta = 0.3;
double zetaLow;
double om = 0.3;
double omLow;
double omMax = 1 * M_PI / 180;
double qiMin = 0.1;
double gainNullspace = 0.01;
@ -828,8 +845,7 @@ class AcsParameters /*: public HasParametersIF*/ {
struct StrParameters {
double exclusionAngle = 20 * M_PI / 180;
// double strOrientationMatrix[3][3];
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // in body/geometry frame
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame
} strParameters;
struct GpsParameters {
@ -840,13 +856,9 @@ class AcsParameters /*: public HasParametersIF*/ {
double latitudeGs = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeGs = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeGs = 500; // [m] Altitude
double earthRadiusEquat = 6378137; // [m]
double earthRadiusPolar = 6356752.314; // [m]
} groundStationParameters; // Stuttgart
struct SunModelParameters {
enum UseSunModel { NO = 0, YES = 3 };
uint8_t useSunModel;
float domega = 36000.771;
float omega_0 = 282.94 * M_PI / 180.; // RAAN plus argument of perigee
float m_0 = 357.5256; // coefficients for mean anomaly
@ -859,12 +871,6 @@ class AcsParameters /*: public HasParametersIF*/ {
} sunModelParameters;
struct KalmanFilterParameters {
uint8_t activateKalmanFilter;
uint8_t requestResetFlag;
double maxToleratedTimeBetweenKalmanFilterExecutionSteps;
double processNoiseOmega[3];
double processNoiseQuaternion[4];
double sensorNoiseSTR = 0.1 * M_PI / 180;
double sensorNoiseSS = 8 * M_PI / 180;
double sensorNoiseMAG = 4 * M_PI / 180;
@ -885,9 +891,10 @@ class AcsParameters /*: public HasParametersIF*/ {
} magnetorquesParameter;
struct DetumbleParameter {
uint8_t detumblecounter;
double omegaDetumbleStart;
double omegaDetumbleEnd;
uint8_t detumblecounter = 75;
double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.2 * M_PI / 180;
double gainD = pow(10.0, -3.3);
} detumbleParameter;
};

60
mission/controller/acs/Guidance.cpp
View File

@ -45,8 +45,9 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
// Position of the satellite in the earth/fixed frame via GPS
double posSatE[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gpsSet.latitude.value,
sensorValues->gpsSet.longitude.value,
double geodeticLatRad = (sensorValues->gpsSet.latitude.value)*PI/180;
double longitudeRad = (sensorValues->gpsSet.longitude.value)*PI/180;
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad,longitudeRad,
sensorValues->gpsSet.altitude.value, posSatE);
// Target direction in the ECEF frame
@ -190,6 +191,59 @@ void Guidance::targetQuatPtg(ACS::SensorValues *sensorValues, ACS::OutputValues
}
}
void Guidance::sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion to sun
//-------------------------------------------------------------------------------------
double quatBJ[4] = {0, 0, 0, 0};
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
quatBJ[0] = outputValues->quatMekfBJ[0];
quatBJ[1] = outputValues->quatMekfBJ[1];
quatBJ[2] = outputValues->quatMekfBJ[2];
quatBJ[3] = outputValues->quatMekfBJ[3];
QuaternionOperations::toDcm(quatBJ, dcmBJ);
double sunDirJ[3] = {0, 0, 0}, sunDir[3] = {0, 0, 0};
if (outputValues->sunDirModelValid) {
sunDirJ[0] = outputValues->sunDirModel[0];
sunDirJ[1] = outputValues->sunDirModel[1];
sunDirJ[2] = outputValues->sunDirModel[2];
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDir, 3, 3, 1);
}
else {
sunDir[0] = outputValues->sunDirEst[0];
sunDir[1] = outputValues->sunDirEst[1];
sunDir[2] = outputValues->sunDirEst[2];
}
double sunRef[3] = {0, 0, 0};
sunRef[0] = acsParameters.safeModeControllerParameters.sunTargetDir[0];
sunRef[1] = acsParameters.safeModeControllerParameters.sunTargetDir[1];
sunRef[2] = acsParameters.safeModeControllerParameters.sunTargetDir[2];
double sunCross[3] = {0, 0, 0};
VectorOperations<double>::cross(sunDir, sunRef, sunCross);
double normSunDir = VectorOperations<double>::norm(sunDir, 3);
double normSunRef = VectorOperations<double>::norm(sunRef, 3);
double dotSun = VectorOperations<double>::dot(sunDir, sunRef);
targetQuat[0] = sunCross[0];
targetQuat[1] = sunCross[1];
targetQuat[2] = sunCross[2];
targetQuat[3] = sqrt(pow(normSunDir,2) * pow(normSunRef,2) + dotSun);
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
//-------------------------------------------------------------------------------------
// Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
refSatRate[0] = 0;
refSatRate[1] = 0;
refSatRate[2] = 0;
}
void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues,
double refSatRate[3], double quatError[3], double deltaRate[3]) {
double quatRef[4] = {0, 0, 0, 0};
@ -310,7 +364,7 @@ void Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
// Does not make sense, but is implemented that way in MATLAB ?!
// Thought: It does not really play a role, because in case there are more then one
// reaction wheel the pointing control is destined to fail.
// reaction wheel invalid the pointing control is destined to fail.
rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];

4
mission/controller/acs/Guidance.h
View File

@ -26,6 +26,10 @@ public:
void targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, timeval now,
double targetQuat[4], double refSatRate[3]);
// Function to get the target quaternion and refence rotation rate for sun pointing after ground station
void sunQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
double targetQuat[4], double refSatRate[3]);
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and desired
void comparePtg( double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] );

2
mission/controller/acs/SensorProcessing.cpp
View File

@ -496,7 +496,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
sensorValues->gpsSet.isValid(), &acsParameters->gpsParameters,
&outputValues->gcLatitude, &outputValues->gdLongitude,
&outputValues->gpsVelocity);
outputValues->gpsVelocity);
outputValues->mgmUpdated = processMgm(
sensorValues->mgm0Lis3Set.fieldStrengths.value,

6
mission/controller/acs/control/Detumble.cpp
View File

@ -27,7 +27,7 @@ Detumble::~Detumble(){
void Detumble::loadAcsParameters(AcsParameters *acsParameters_){
detumbleCtrlParameters = &(acsParameters_->detumbleCtrlParameters);
detumbleParameter = &(acsParameters_->detumbleParameter);
magnetorquesParameter = &(acsParameters_->magnetorquesParameter);
}
@ -40,7 +40,7 @@ ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
if (!magRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleCtrlParameters->gainD;
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
return returnvalue::OK;
@ -71,7 +71,7 @@ ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateVa
if (!satRateValid || !magFieldValid) {
return DETUMBLE_NO_SENSORDATA;
}
double gain = detumbleCtrlParameters->gainD;
double gain = detumbleParameter->gainD;
double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
return returnvalue::OK;

2
mission/controller/acs/control/Detumble.h
View File

@ -43,7 +43,7 @@ public:
double *magMom);
private:
AcsParameters::DetumbleCtrlParameters* detumbleCtrlParameters;
AcsParameters::DetumbleParameter* detumbleParameter;
AcsParameters::MagnetorquesParameter* magnetorquesParameter;
};

1
mission/controller/acs/control/PtgCtrl.cpp
View File

@ -109,6 +109,7 @@ void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const do
double torque[3] = {0, 0, 0};
VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
}

2
mission/controller/acs/control/PtgCtrl.h
View File

@ -34,7 +34,7 @@ class PtgCtrl {
static const uint8_t INTERFACE_ID = CLASS_ID::PTG;
static const ReturnValue_t PTGCTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
/* @brief: Load AcsParameters für this class
/* @brief: Load AcsParameters for this class
* @param: acsParameters_ Pointer to object which defines the ACS configuration parameters
*/
void loadAcsParameters(AcsParameters *acsParameters_);

24
mission/controller/acs/util/MathOperations.h
View File

@ -104,9 +104,16 @@ public:
}
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){
double radiusPolar = 6378137;
double radiusEqua = 6356752.314;
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
* longitude and altitude
* @param: lat geodetic latitude [rad]
* longi longitude [rad]
* alt altitude [m]
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
* Landis Markley and John L. Crassidis*/
double radiusPolar = 6356752.314;
double radiusEqua = 6378137;
double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2));
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2));
@ -117,13 +124,12 @@ public:
}
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
* Landis Markley and John L. Crassidis*/
static void dcmEJ(timeval time, T1 * outputDcmEJ){
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
* Landis Markley and John L. Crassidis*/
double JD2000Floor = 0;
double JD2000 = convertUnixToJD2000(time);
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)