mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -14,7 +14,7 @@
 
 /*Initialisation of values for parameters in constructor*/
 MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
-    : initialQuaternion{0.5, 0.5, 0.5, 0.5},
+    : initialQuaternion{0, 0, 0, 1},
       initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                               {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} {
   loadAcsParameters(acsParameters_);
@@ -27,12 +27,10 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
   kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
 }
 
-void MultiplicativeKalmanFilter::reset() {}
-
 void MultiplicativeKalmanFilter::init(
     const double *magneticField_, const bool validMagField_, const double *sunDir_,
     const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-    const bool validMagModel) {  // valids for "model measurements"?
+    const bool validMagModel, acsctrl::MekfData *mekfData) {  // valids for "model measurements"?
   // check for valid mag/sun
   if (validMagField_ && validSS && validSSModel && validMagModel) {
     validInit = true;
@@ -190,9 +188,11 @@ void MultiplicativeKalmanFilter::init(
     initialCovarianceMatrix[5][3] = initGyroCov[2][0];
     initialCovarianceMatrix[5][4] = initGyroCov[2][1];
     initialCovarianceMatrix[5][5] = initGyroCov[2][2];
+    updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
   } else {
     // no initialisation possible, no valid measurements
     validInit = false;
+    updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
   }
 }
 
@@ -208,32 +208,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   // Check for GYR Measurements
   int MDF = 0;  // Matrix Dimension Factor
   if (!validGYRs_) {
-    {
+    updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
-      PoolReadGuard pg(mekfData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-        double zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
-        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
-        mekfData->setValidity(false, true);
-      }
-    }
-    validMekf = false;
     return KALMAN_NO_GYR_MEAS;
   }
   // Check for Model Calculations
   else if (!validSSModel || !validMagModel) {
-    {
+    updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
-      PoolReadGuard pg(mekfData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-        double zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
-        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
-        mekfData->setValidity(false, true);
-      }
-    }
-    validMekf = false;
     return KALMAN_NO_MODEL;
   }
   // Check Measurements available from SS, MAG, STR
@@ -260,17 +240,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
     MDF = 3;
   } else {
     sensorsAvail = 8;  // no measurements
-    validMekf = false;
+    updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA);
-    {
-      PoolReadGuard pg(mekfData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-        double zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
-        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
-        mekfData->setValidity(false, true);
-      }
-    }
     return returnvalue::FAILED;
   }
 
@@ -881,17 +851,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   double invResidualCov[MDF][MDF] = {{0}};
   int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
   if (inversionFailed) {
-    {
+    updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
-      PoolReadGuard pg(mekfData);
-      if (pg.getReadResult() == returnvalue::OK) {
-        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-        double zeroVec[3] = {0.0, 0.0, 0.0};
-        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
-        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
-        mekfData->setValidity(false, true);
-      }
-    }
-    validMekf = false;
     return KALMAN_INVERSION_FAILED;  // RETURN VALUE ? -- Like: Kalman Inversion Failed
   }
 
@@ -1121,20 +1081,40 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
 
   MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
-  validMekf = true;
 
-  // Discrete Time Step
+  updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
+  return returnvalue::OK;
+}
 
-  // Check for new data in measurement -> SensorProcessing ?
+void MultiplicativeKalmanFilter::reset() {}
 
+void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
+                                                         MekfStatus mekfStatus) {
   {
     PoolReadGuard pg(mekfData);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double));
+      double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
-      std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double));
+      double zeroVec[3] = {0.0, 0.0, 0.0};
-      mekfData->setValidity(true, true);
+      std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
+      mekfData->quatMekf.setValid(false);
+      std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
+      mekfData->satRotRateMekf.setValid(false);
+      mekfData->mekfStatus = mekfStatus;
+      mekfData->setValidity(true, false);
+    }
   }
 }
 
-  return returnvalue::OK;
+void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData,
+                                                             MekfStatus mekfStatus, double quat[4],
+                                                             double satRotRate[3]) {
+  {
+    PoolReadGuard pg(mekfData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double));
+      std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double));
+      mekfData->mekfStatus = mekfStatus;
+      mekfData->setValidity(true, true);
+    }
+  }
 }

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -41,7 +41,7 @@ class MultiplicativeKalmanFilter {
    */
   void init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
             const bool validSS, const double *sunDirJ, const bool validSSModel,
-            const double *magFieldJ, const bool validMagModel);
+            const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData);
 
   /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
    * for the current step after the initalization
@@ -63,6 +63,16 @@ class MultiplicativeKalmanFilter {
                         const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
                         const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
 
+  enum MekfStatus : uint8_t {
+    UNINITIALIZED = 0,
+    NO_GYR_DATA = 1,
+    NO_MODEL_VECTORS = 2,
+    NO_SUS_MGM_STR_DATA = 3,
+    COVARIANCE_INVERSION_FAILED = 4,
+    INITIALIZED = 10,
+    RUNNING = 11,
+  };
+
   // resetting Mekf
   static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
   static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1);
@@ -80,16 +90,17 @@ class MultiplicativeKalmanFilter {
   double initialQuaternion[4];          /*after reset?QUEST*/
   double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
   double propagatedQuaternion[4];       /*Filter Quaternion for next step*/
-  bool validMekf;
   uint8_t sensorsAvail;
 
   /*Outputs*/
   double quatBJ[4];  /* Output Quaternion */
   double biasGYR[3]; /*Between measured and estimated sat Rate*/
                      /*Parameter INIT*/
-  // double alpha, gamma, beta;
   /*Functions*/
   void loadAcsParameters(AcsParameters *acsParameters_);
+  void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
+  void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
+                     double satRotRate[3]);
 };
 
 #endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */

mission/controller/acs/Navigation.cpp

@@ -1,10 +1,3 @@
-/*
- * Navigation.cpp
- *
- *  Created on: 23 May 2022
- *      Author: Robin Marquardt
- */
-
 #include "Navigation.h"
 
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
@@ -46,7 +39,7 @@ void Navigation::useMekf(ACS::SensorValues *sensorValues,
         mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
         susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
         susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
-        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid());
+        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
     kalmanInit = true;
     *mekfValid = returnvalue::OK;