From 0aa09bd5169b907861f97dba4d8ae47dc22b45cf Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 22 Nov 2023 13:56:33 +0100 Subject: [PATCH] this is better --- mission/controller/acs/AttitudeEstimation.cpp | 21 +++++++++++-------- mission/controller/acs/AttitudeEstimation.h | 8 +++---- .../AcsCtrlDefinitions.h | 8 ++++--- 3 files changed, 20 insertions(+), 17 deletions(-) diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index 0e8be1b1..f8effbe9 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -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::normalize(mgmB, normMgmB, 3); - VectorOperations::normalize(mgmI, normMgmI, 3); - VectorOperations::normalize(susB, normSusB, 3); - VectorOperations::normalize(susI, normSusI, 3); + VectorOperations::normalize(susData->susVecTot.value, normMgmB, 3); + VectorOperations::normalize(susData->sunIjkModel.value, normMgmI, 3); + VectorOperations::normalize(mgmData->mgmVecTot.value, normSusB, 3); + VectorOperations::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::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::dot(normHelperB, normHelperI)); @@ -85,6 +87,7 @@ void AttitudeEstimation::Quest(const double susB[3], const bool susBvalid, const VectorOperations::mulScalar(qBI, constMinus, qBI, 3); QuaternionOperations::normalize(qBI, qBI); } + // ToDo: fill dataset and set to valid } else { // ToDo: fill dataset and set to invalid } diff --git a/mission/controller/acs/AttitudeEstimation.h b/mission/controller/acs/AttitudeEstimation.h index eee58f81..b485b7b1 100644 --- a/mission/controller/acs/AttitudeEstimation.h +++ b/mission/controller/acs/AttitudeEstimation.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -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_ */ diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index b843ca0c..26edf269 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -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 { private: }; -class MekfData : public StaticLocalDataSet { +class AttitudeEstimationData : public StaticLocalDataSet { public: - MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {} + AttitudeEstimationData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {} lp_vec_t quatMekf = lp_vec_t(sid.objectId, QUAT_MEKF, this); lp_vec_t satRotRateMekf = lp_vec_t(sid.objectId, SAT_ROT_RATE_MEKF, this); lp_var_t mekfStatus = lp_var_t(sid.objectId, MEKF_STATUS, this); + lp_vec_t quatQuest = lp_vec_t(sid.objectId, QUAT_MEKF, this); private: };