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],
+void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
-                               const bool susIvalid, const double mgmB[3], const bool mgmBvalid,
+                               acsctrl::MgmDataProcessed *mgmData,
-                               const double mgmI[3], const bool mgmIvalid, double qBI[4]) {
+                               acsctrl::AttitudeEstimationData *attitudeEstimation) {
-  if (susBvalid and susIvalid and mgmBvalid and mgmIvalid) {
+  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(susData->susVecTot.value, normMgmB, 3);
-    VectorOperations<double>::normalize(mgmI, normMgmI, 3);
+    VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
-    VectorOperations<double>::normalize(susB, normSusB, 3);
+    VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
-    VectorOperations<double>::normalize(susI, normSusI, 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
   }

mission/controller/acs/AttitudeEstimation.h

@@ -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,
+  void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData,
-             const double mgmB[3], const bool mgmBvalid, const double mgmI[3], const bool mgmIvalid,
+             acsctrl::AttitudeEstimationData *attitudeEstimation);
-             double qBI[4]);
 
  protected:
  private:
   AcsParameters *acsParameters;
-
-  void Triad(double susB[3], double susI[3], double mgmB[3], double mgmI[3]);
 };
 
 #endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */

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<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:
 };