cleanup and detumble switch

mission/controller/AcsController.cpp

@@ -174,47 +174,31 @@ void AcsController::performSafe() {
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid());
   switch (safeCtrlStrat) {
-    case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_MEKF):
+    case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF):
       safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
                         susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
                         satRateSafe, inertiaEive, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_NONMEKF):
+    case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF):
       safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
                            susDataProcessed.susVecTot.value, sunTargetDir, satRateSafe, inertiaEive,
                            magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (SafeCtrl::SafeModeStrategy::SAFECTRL_USE_DAMPING):
+    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING):
       safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
                                satRateSafe, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (SafeCtrl::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
+    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
-      if (not safeCtrlFailureFlag) {
+      safeCtrlFailure(1, 0);
-        triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 1, 0);
-        safeCtrlFailureFlag = true;
-      }
-      safeCtrlFailureCounter++;
-      if (safeCtrlFailureCounter > 150) {
-        safeCtrlFailureFlag = false;
-        safeCtrlFailureCounter = 0;
-      }
       break;
-    case (SafeCtrl::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
+    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
-      if (not safeCtrlFailureFlag) {
+      safeCtrlFailure(0, 1);
-        triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, 0, 1);
-        safeCtrlFailureFlag = true;
-      }
-      safeCtrlFailureCounter++;
-      if (safeCtrlFailureCounter > 150) {
-        safeCtrlFailureFlag = false;
-        safeCtrlFailureCounter = 0;
-      }
       break;
   }
 
@@ -270,11 +254,27 @@ void AcsController::performDetumble() {
     triggerEvent(acs::MEKF_RECOVERY);
     mekfInvalidFlag = false;
   }
+  uint8_t safeCtrlStrat = detumble.detumbleStrategy(mgmDataProcessed.mgmVecTot.isValid(),
+                                                    gyrDataProcessed.gyrVecTot.isValid(),
+                                                    mgmDataProcessed.mgmVecTotDerivative.isValid());
   double magMomMtq[3] = {0, 0, 0};
-  detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
+  switch (safeCtrlStrat) {
-                   mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
+    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
-                   mgmDataProcessed.mgmVecTot.isValid(), magMomMtq,
+      detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
-                   acsParameters.detumbleParameter.gainD);
+                           magMomMtq, acsParameters.detumbleParameter.gainD);
+      break;
+    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
+      detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
+                       magMomMtq, acsParameters.detumbleParameter.gainD);
+      break;
+    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
+      safeCtrlFailure(1, 0);
+      break;
+    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
+      safeCtrlFailure(0, 1);
+      break;
+  }
+
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
                           *acsParameters.magnetorquerParameter.inverseAlignment,
                           acsParameters.magnetorquerParameter.dipolMax);
@@ -498,6 +498,18 @@ void AcsController::performPointingCtrl() {
   //                   acsParameters.rwHandlingParameters.rampTime);
 }
 
+void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
+  if (not safeCtrlFailureFlag) {
+    triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
+    safeCtrlFailureFlag = true;
+  }
+  safeCtrlFailureCounter++;
+  if (safeCtrlFailureCounter > 150) {
+    safeCtrlFailureFlag = false;
+    safeCtrlFailureCounter = 0;
+  }
+}
+
 ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
                                               uint16_t dipoleTorqueDuration) {
   {
@@ -581,7 +593,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu
     std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
     ctrlValData.errAng.value = errAng;
     std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
-    ctrlValData.safeStrat.value = SafeCtrl::SafeModeStrategy::SAFECTRL_OFF;
+    ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF;
     ctrlValData.setValidity(true, true);
   }
 }

mission/controller/AcsController.h

@@ -12,18 +12,17 @@
 #include <mission/acs/imtqHelpers.h>
 #include <mission/acs/rwHelpers.h>
 #include <mission/acs/susMax1227Helpers.h>
+#include <mission/controller/acs/ActuatorCmd.h>
+#include <mission/controller/acs/Guidance.h>
+#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
+#include <mission/controller/acs/Navigation.h>
+#include <mission/controller/acs/SensorProcessing.h>
+#include <mission/controller/acs/control/Detumble.h>
+#include <mission/controller/acs/control/PtgCtrl.h>
+#include <mission/controller/acs/control/SafeCtrl.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 #include <mission/utility/trace.h>
 
-#include "acs/ActuatorCmd.h"
-#include "acs/Guidance.h"
-#include "acs/MultiplicativeKalmanFilter.h"
-#include "acs/Navigation.h"
-#include "acs/SensorProcessing.h"
-#include "acs/control/Detumble.h"
-#include "acs/control/PtgCtrl.h"
-#include "acs/control/SafeCtrl.h"
-#include "controllerdefinitions/AcsCtrlDefinitions.h"
-
 class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
  public:
   static constexpr dur_millis_t INIT_DELAY = 500;
@@ -106,6 +105,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void modeChanged(Mode_t mode, Submode_t submode);
   void announceMode(bool recursive);
 
+  void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
+
   ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
                                  uint16_t dipoleTorqueDuration);
   ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,