eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests Releases 117 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
Show Stats 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_ */

8
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -96,6 +96,7 @@ enum PoolIds : lp_id_t {
SAT_ROT_RATE_MEKF,
QUAT_MEKF,
MEKF_STATUS,
QUAT_QUEST,
// Ctrl Values
SAFE_STRAT,
TGT_QUAT,
@ -122,7 +123,7 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
@ -250,13 +251,14 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
private:
};
class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
class AttitudeEstimationData : public StaticLocalDataSet<ATTITUDE_ESTIMATION_SET_ENTRIES> {
public:
MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
AttitudeEstimationData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);
lp_var_t<uint8_t> mekfStatus = lp_var_t<uint8_t>(sid.objectId, MEKF_STATUS, this);
lp_vec_t<double, 4> quatQuest = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
private:
};