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

this is better
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-11-22 13:56:33 +01:00
parent a45e96b772
commit 0aa09bd516
3 changed files with 20 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
mission/controller/acs/AttitudeEstimation.cpp
View File

@ -6,17 +6,18 @@ AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) {
AttitudeEstimation::~AttitudeEstimation() {}
void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const double susI[3],
const bool susIvalid, const double mgmB[3], const bool mgmBvalid,
const double mgmI[3], const bool mgmIvalid, double qBI[4]) {
if (susBvalid and susIvalid and mgmBvalid and mgmIvalid) {
void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
acsctrl::MgmDataProcessed *mgmData,
acsctrl::AttitudeEstimationData *attitudeEstimation) {
if (susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid()) {
// Normalize Data
double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
normSusI[3] = {0, 0, 0};
VectorOperations<double>::normalize(mgmB, normMgmB, 3);
VectorOperations<double>::normalize(mgmI, normMgmI, 3);
VectorOperations<double>::normalize(susB, normSusB, 3);
VectorOperations<double>::normalize(susI, normSusI, 3);
VectorOperations<double>::normalize(susData->susVecTot.value, normMgmB, 3);
VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normSusI, 3);
// Create Helper Vectors
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
@ -59,7 +60,8 @@ void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
// Calculate Quaternion
double qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0}, qRotVecPt1[3] = {0, 0, 0};
double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
qRotVecPt1[3] = {0, 0, 0};
if (alpha >= 0) {
// Scalar Part
qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
@ -85,6 +87,7 @@ void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
}
// ToDo: fill dataset and set to valid
} else {
// ToDo: fill dataset and set to invalid
}

8
mission/controller/acs/AttitudeEstimation.h
View File

@ -4,6 +4,7 @@
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <cmath>
#include <iostream>
@ -14,15 +15,12 @@ class AttitudeEstimation {
virtual ~AttitudeEstimation();
;
void Quest(const double susB[3], const bool susBvalid, const double susI[3], const bool susIvalid,
const double mgmB[3], const bool mgmBvalid, const double mgmI[3], const bool mgmIvalid,
double qBI[4]);
void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData,
acsctrl::AttitudeEstimationData *attitudeEstimation);
protected:
private:
AcsParameters *acsParameters;
void Triad(double susB[3], double susI[3], double mgmB[3], double mgmI[3]);
};
#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */