Merge pull request 'Improve Safe Strat State Machine' (#777) from safe-strat-improvement into main

Reviewed-on: #777

CHANGELOG.md

@@ -40,6 +40,8 @@ will consitute of a breaking change warranting a new major release:
   return the actual GPS data will be ignored once SPG4 is running. However, by setting the
   according parameter, the ACS Controller can be directed to ignore the SGP4 solution.
 - Skyview dataset for more GPS TM has been added
+- The MGM and SUS vectors being too close together does not prevent the usage of the safe
+  mode controller anymore.
 - Parameter to disable usage of MGM4, which is part of the MTQ and therefore cannot be
   disabled without disabling the MTQ itself.
 

mission/controller/AcsController.cpp

@@ -204,8 +204,7 @@ void AcsController::performSafe() {
   acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
-      fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(),
+      fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
-      acsParameters.safeModeControllerParameters.useMekf,
       acsParameters.safeModeControllerParameters.useGyr,
       acsParameters.safeModeControllerParameters.dampingDuringEclipse);
   switch (safeCtrlStrat) {
@@ -223,7 +222,8 @@ void AcsController::performSafe() {
       safeCtrlFailureCounter = 0;
       break;
     case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
-      safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value,
+      safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
+                          fusedRotRateData.rotRateParallel.value,
                           fusedRotRateData.rotRateOrthogonal.value,
                           susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;

mission/controller/acs/control/SafeCtrl.cpp

@@ -9,10 +9,12 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
 
 SafeCtrl::~SafeCtrl() {}
 
-acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
+acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
-    const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
+                                                 const bool satRotRateValid, const bool sunDirValid,
-    const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid,
+                                                 const bool fusedRateTotalValid,
-    const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
+                                                 const uint8_t mekfEnabled,
+                                                 const uint8_t gyrEnabled,
+                                                 const uint8_t dampingEnabled) {
   if (not magFieldValid) {
     return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (mekfEnabled and mekfValid) {
@@ -20,7 +22,7 @@ acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
   } else if (sunDirValid) {
     if (gyrEnabled and satRotRateValid) {
       return acs::SafeModeStrategy::SAFECTRL_GYR;
-    } else if (not gyrEnabled and fusedRateSplitValid) {
+    } else if (not gyrEnabled and fusedRateTotalValid) {
       return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
     } else {
       return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
@@ -95,9 +97,10 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const
   calculateMagneticMoment(magMomB);
 }
 
-void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
+void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
-                          const double *rotRateOrthogonalB, const double *sunDirB,
+                          const double *rotRateParallelB, const double *rotRateOrthogonalB,
-                          const double *sunDirRefB, double *magMomB, double &errorAngle) {
+                          const double *sunDirB, const double *sunDirRefB, double *magMomB,
+                          double &errorAngle) {
   // convert magFieldB from uT to T
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
@@ -105,8 +108,14 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel
   double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
   errorAngle = acos(dotSun);
 
+  if (VectorOperations<double>::norm(rotRateParallelB, 3) != 0 and
+      VectorOperations<double>::norm(rotRateOrthogonalB, 3) != 0) {
     std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
     std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
+  } else {
+    splitRotationalRate(rotRateTotalB, sunDirB);
+  }
+
   calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
                                 acsParameters->safeModeControllerParameters.k_orthoSusMgm);
   calculateAngleErrorTorque(sunDirB, sunDirRefB,

mission/controller/acs/control/SafeCtrl.h

@@ -14,7 +14,6 @@ class SafeCtrl {
 
   acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                          const bool satRotRateValid, const bool sunDirValid,
-                                         const bool fusedRateSplitValid,
                                          const bool fusedRateTotalValid, const uint8_t mekfEnabled,
                                          const uint8_t gyrEnabled, const uint8_t dampingEnabled);
 
@@ -25,9 +24,10 @@ class SafeCtrl {
   void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
                const double *sunDirRefB, double *magMomB, double &errorAngle);
 
-  void safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
+  void safeSusMgm(const double *magFieldB, const double *rotRateTotalB,
-                  const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB,
+                  const double *rotRateParallelB, const double *rotRateOrthogonalB,
-                  double *magMomB, double &errorAngle);
+                  const double *sunDirB, const double *sunDirRefB, double *magMomB,
+                  double &errorAngle);
 
   void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
                           const double *sunDirRefB, double *magMomB, double &errorAngle);

tmtc

@@ -1 +1 @@
-Subproject commit 4b054b7628e43975e2401c7db10c358824f7a2d3
+Subproject commit 403657110b1555b0e15702cb96d7fe43e815a036