reseting the MEKF now also resets its state in navigation

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -1088,13 +1088,14 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
   return KALMAN_RUNNING;
 }
 
-void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
+ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
   double resetQuaternion[4] = {0, 0, 0, 1};
   double resetCovarianceMatrix[6][6] = {{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}};
   std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
   std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
   updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
+  return KALMAN_UNINITIALIZED;
 }
 
 void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -29,7 +29,7 @@ class MultiplicativeKalmanFilter {
   MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
   virtual ~MultiplicativeKalmanFilter();
 
-  void reset(acsctrl::MekfData *mekfData);
+  ReturnValue_t reset(acsctrl::MekfData *mekfData);
 
   /* @brief: init() - This function initializes the Kalman Filter and will provide the first
    * quaternion through the QUEST algorithm

mission/controller/acs/Navigation.cpp

@@ -25,26 +25,25 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
                      sensorValues->strSet.caliQy.isValid() &&
                      sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
 
-  if (kalmanInit) {
-    return multiplicativeKalmanFilter.mekfEst(
+  if (mekfStatus == MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED) {
+    mekfStatus = multiplicativeKalmanFilter.init(
+        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
+        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
+        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
+        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
+    return mekfStatus;
+  } else {
+    mekfStatus = multiplicativeKalmanFilter.mekfEst(
         quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
         gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
         mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
         susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
         susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
         mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
-  } else {
-    ReturnValue_t result;
-    result = multiplicativeKalmanFilter.init(
-        mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
-        susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
-        susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
-        mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
-    kalmanInit = true;
-    return result;
+    return mekfStatus;
   }
 }
 
 void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
-  multiplicativeKalmanFilter.reset(mekfData);
+  mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
 }

mission/controller/acs/Navigation.h

@@ -22,7 +22,7 @@ class Navigation {
  private:
   MultiplicativeKalmanFilter multiplicativeKalmanFilter;
   AcsParameters acsParameters;
-  bool kalmanInit = false;
+  ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::KALMAN_UNINITIALIZED;
 };
 
 #endif /* ACS_NAVIGATION_H_ */