Merge branch 'develop' into acs-ctrl-finite-check

mission/controller/AcsController.cpp

@@ -181,7 +181,18 @@ void AcsController::performSafe() {
         sunTargetDir, satRateSafe, &errAng, magMomMtq);
   }
   if (result == returnvalue::FAILED) {
-    // ToDo: this should never ever happen or we are dead. prob add an event at least
+    if (not safeCtrlFailureFlag) {
+      triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE);
+      safeCtrlFailureFlag = true;
+    }
+    safeCtrlFailureCounter++;
+    if (safeCtrlFailureCounter > 50) {
+      safeCtrlFailureFlag = false;
+      safeCtrlFailureCounter = 0;
+    }
+  } else {
+    safeCtrlFailureFlag = false;
+    safeCtrlFailureCounter = 0;
   }
 
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,

mission/controller/AcsController.h

@@ -62,6 +62,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = false;
   uint16_t mekfInvalidCounter = 0;
+  bool safeCtrlFailureFlag = false;
+  uint8_t safeCtrlFailureCounter = 0;
   uint8_t resetMekfCount = 0;
   bool mekfLost = false;
 

mission/controller/acs/AcsParameters.h

@@ -768,10 +768,10 @@ class AcsParameters : public HasParametersIF {
     double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
     double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
 
-    double gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004};
-    double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359};
-    double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
-    double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
+    double gyr0bias[3] = {0.0, 0.4, -0.1};
+    double gyr1bias[3] = {0.0956745283018868, 2.0854575471698116, 1.2505990566037737};
+    double gyr2bias[3] = {0.1, 0.7, -0.2};
+    double gyr3bias[3] = {-0.10721698113207549, -0.6111650943396226, 0.1716462264150944};
 
     /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
      * assumed to be equal for the same class of sensors */

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -1080,12 +1080,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
 
   MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
 
-  if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
-      not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
-    updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
-    return MEKF_NOT_FINITE;
-  }
-
   updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
   return MEKF_RUNNING;
 }
@@ -1095,7 +1089,6 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
   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(propagatedQuaternion, resetQuaternion, 4 * sizeof(double));
   std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
   updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
   return MEKF_UNINITIALIZED;

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -62,7 +62,6 @@ class MultiplicativeKalmanFilter {
     NO_MODEL_VECTORS = 2,
     NO_SUS_MGM_STR_DATA = 3,
     COVARIANCE_INVERSION_FAILED = 4,
-    NOT_FINITE = 5,
     INITIALIZED = 10,
     RUNNING = 11,
   };
@@ -75,9 +74,8 @@ class MultiplicativeKalmanFilter {
   static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
   static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
       returnvalue::makeCode(IF_MEKF_ID, 6);
-  static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
-  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
-  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
+  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
+  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
 
  private:
   /*Parameters*/

mission/controller/acs/util/MathOperations.h

@@ -404,26 +404,6 @@ class MathOperations {
     std::memcpy(inverse, identity, sizeof(identity));
     return 0;  // successful inversion
   }
-
-  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
-    for (uint8_t i = 0; i < size; i++) {
-      if (not isfinite(inputVector[i])) {
-        return false;
-      }
-    }
-    return true;
-  }
-
-  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
-    for (uint8_t col = 0; col < cols; col++) {
-      for (uint8_t row = 0; row < rows; row++) {
-        if (not isfinite(inputMatrix[row * cols + cols])) {
-          return false;
-        }
-      }
-    }
-    return true;
-  }
 };
 
 #endif /* ACS_MATH_MATHOPERATIONS_H_ */