blub

mission/acs/defs.h

@@ -22,16 +22,22 @@ enum AcsMode : Mode_t {
 
 enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
 
-enum SafeModeStrategy : uint8_t {
+enum ctrlStrategy : uint8_t {
   SAFECTRL_OFF = 0,
   SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
   SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
+  PTGCTRL_NO_RW_FOR_CONTROL = 3,
+  PTGCTRL_NO_SENSORS_FOR_CONTROL = 4,
   SAFECTRL_ACTIVE_MEKF = 10,
   SAFECTRL_WITHOUT_MEKF = 11,
   SAFECTRL_ECLIPSE_DAMPING = 12,
   SAFECTRL_ECLIPSE_IDELING = 13,
   SAFECTRL_DETUMBLE_FULL = 20,
   SAFECTRL_DETUMBLE_DETERIORATED = 21,
+  PTGCTRL_ACTIVE_MEKF = 30,
+  PTGCTRL_WITHOUT_MEKF = 31,
+  PTGCTRL_ACTIVE_MEKF_WO_DESAT = 32,
+  PTGCTRL_WITHOUT_MEKF_WO_DESAT = 33,
 };
 
 static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
@@ -49,9 +55,9 @@ static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
 static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO);
 //! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values.
 static constexpr Event MEKF_AUTOMATIC_RESET = MAKE_EVENT(5, severity::INFO);
-//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution during any pointing ACS mode for a
-//! prolonged time.
-static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH);
+//! [EXPORT] : [COMMENT] No attitude information was available for a prolonged time and therefore
+//! the pointing mode controller has failed.
+static constexpr Event PTG_CCONTROLLER_FAILURE = MAKE_EVENT(6, severity::HIGH);
 //! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has
 //! failed.
 //! P1: Missing information about magnetic field, P2: Missing information about rotational rate

mission/controller/AcsController.cpp

@@ -169,40 +169,40 @@ void AcsController::performSafe() {
   guidance.getTargetParamsSafe(sunTargetDir);
 
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
-  acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
+  acs::ctrlStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
       acsParameters.safeModeControllerParameters.useMekf,
       acsParameters.safeModeControllerParameters.dampingDuringEclipse);
   switch (safeCtrlStrat) {
-    case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF):
+    case (acs::ctrlStrategy::SAFECTRL_ACTIVE_MEKF):
       safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
                         susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
                         magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF):
+    case (acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF):
       safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
                            susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING):
+    case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING):
       safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
                                sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
+    case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING):
       errAng = NAN;
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
+    case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
       safeCtrlFailure(1, 0);
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
+    case (acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
       safeCtrlFailure(0, 1);
       break;
     default:
@@ -261,24 +261,24 @@ void AcsController::performDetumble() {
     triggerEvent(acs::MEKF_RECOVERY);
     mekfInvalidFlag = false;
   }
-  acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
+  acs::ctrlStrategy safeCtrlStrat = detumble.detumbleStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
       mgmDataProcessed.mgmVecTotDerivative.isValid(),
       acsParameters.detumbleParameter.useFullDetumbleLaw);
   double magMomMtq[3] = {0, 0, 0};
   switch (safeCtrlStrat) {
-    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
+    case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL):
       detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
                            magMomMtq, acsParameters.detumbleParameter.gainFull);
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
+    case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
       detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
                        magMomMtq, acsParameters.detumbleParameter.gainBdot);
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
+    case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
       safeCtrlFailure(1, 0);
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
+    case (acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
       safeCtrlFailure(0, 1);
       break;
     default:
@@ -353,17 +353,22 @@ void AcsController::performPointingCtrl() {
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
-    if (multipleRwUnavailableCounter == 5) {
+    if (multipleRwUnavailableCounter >=
+        acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
       triggerEvent(acs::MULTIPLE_RW_INVALID);
     }
     multipleRwUnavailableCounter++;
-    return;
+    rwInvalidFlag = true;
   } else {
     multipleRwUnavailableCounter = 0;
+    rwInvalidFlag = false;
   }
 
+  // Variables required for ptgStrat
+  acs::ctrlStrategy ptgStrat;
+  double quat[4] = {0, 0, 0, 0}, satRotRate[3] = {0, 0, 0};
   // Variables required for guidance
-  double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
+  double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 0},
          errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
   // Variables required for setting actuators
   double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
@@ -371,10 +376,16 @@ void AcsController::performPointingCtrl() {
 
   switch (mode) {
     case acs::PTG_IDLE:
+      ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.idleModeControllerParameters, quat,
+                                    satRotRate);
+      if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
+          ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
+        break;
+      }
       guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
                                 targetSatRotRate);
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
-                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
+      guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
@@ -385,18 +396,24 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET:
+      ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.targetModeControllerParameters, quat,
+                                    satRotRate);
+      if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
+          ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
+        break;
+      }
       guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
                                       gpsDataProcessed.gpsVelocity.value, targetQuat,
                                       targetSatRotRate);
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
-                          targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
+      guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
+                          acsParameters.targetModeControllerParameters.quatRef,
                           acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
@@ -409,17 +426,23 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET_GS:
+      ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.gsTargetModeControllerParameters, quat,
+                                    satRotRate);
+      if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
+          ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
+        break;
+      }
       guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
                                susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
-                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
+      guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, errorQuat,
+                          errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
@@ -430,18 +453,24 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_NADIR:
+      ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.nadirModeControllerParameters, quat,
+                                    satRotRate);
+      if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
+          ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
+        break;
+      }
       guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
                                            gpsDataProcessed.gpsVelocity.value, targetQuat,
                                            targetSatRotRate);
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
-                          targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
+      guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
+                          acsParameters.nadirModeControllerParameters.quatRef,
                           acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
@@ -454,17 +483,23 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_INERTIAL:
+      ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.inertialModeControllerParameters, quat,
+                                    satRotRate);
+      if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or
+          ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
+        break;
+      }
       std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
                   sizeof(targetQuat));
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
-                          targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
+      guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate,
+                          acsParameters.inertialModeControllerParameters.quatRef,
                           acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
@@ -477,9 +512,9 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
       break;
     default:
@@ -487,18 +522,22 @@ void AcsController::performPointingCtrl() {
       break;
   }
 
-  actuatorCmd.cmdSpeedToRws(
-      sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
-      acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
-      acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
-  if (enableAntiStiction) {
-    ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
+  if (ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL and
+      ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) {
+    actuatorCmd.cmdSpeedToRws(
+        sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+        sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
+        acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
+        acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
+    if (enableAntiStiction) {
+      ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
+    }
+    actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
+                             acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes,
+                             cmdDipoleMtqs);
   }
-  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
-                           acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
 
-  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
+  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgStrat);
   updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
   commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                    acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
@@ -506,6 +545,31 @@ void AcsController::performPointingCtrl() {
                    acsParameters.rwHandlingParameters.rampTime);
 }
 
+void AcsController::handlePtgCtrlStrat(acs::ctrlStrategy &ptgStrat,
+                                       AcsParameters::PointingLawParameters ptgLawParameters,
+                                       double *quat, double *satRotRate) {
+  ptgStrat = ptgCtrl.ptgCtrlStrategy(
+      not rwInvalidFlag, ptgLawParameters.useMekf, not mekfInvalidFlag,
+      mgmDataProcessed.mgmVecTot.isValid(), ptgLawParameters.desatOn,
+      sensorValues.strSet.isTrustWorthy.value, gyrDataProcessed.gyrVecTot.isValid());
+  switch (ptgStrat) {
+    case acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF:
+    case acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF_WO_DESAT:
+      std::memcpy(quat, mekfData.quatMekf.value, 4 * sizeof(double));
+      std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double));
+      break;
+    case acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF:
+    case acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF_WO_DESAT:
+      quat[0] = sensorValues.strSet.caliQx.value;
+      quat[1] = sensorValues.strSet.caliQy.value;
+      quat[2] = sensorValues.strSet.caliQz.value;
+      quat[3] = sensorValues.strSet.caliQw.value;
+      std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double));
+      break;
+    default:;
+  }
+}
+
 void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
   if (not safeCtrlFailureFlag) {
     triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
@@ -576,7 +640,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
   }
 }
 
-void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
+void AcsController::updateCtrlValData(double errAng, acs::ctrlStrategy safeModeStrat) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
     std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@@ -587,21 +651,21 @@ void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
     ctrlValData.errAng.setValid(true);
     std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
     ctrlValData.tgtRotRate.setValid(false);
-    ctrlValData.safeStrat.value = safeModeStrat;
-    ctrlValData.safeStrat.setValid(true);
+    ctrlValData.ctrlStrat.value = safeModeStrat;
+    ctrlValData.ctrlStrat.setValid(true);
     ctrlValData.setValidity(true, false);
   }
 }
 
 void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
-                                      const double *tgtRotRate) {
+                                      const double *tgtRotRate, acs::ctrlStrategy ptgModeStrat) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
     std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
     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 = acs::SafeModeStrategy::SAFECTRL_OFF;
+    ctrlValData.ctrlStrat.value = ptgModeStrat;
     ctrlValData.setValidity(true, true);
   }
 }
@@ -694,7 +758,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
   poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
   // Ctrl Values
-  localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
+  localDataPoolMap.emplace(acsctrl::PoolIds::CTRL_STRAT, &ctrlStrat);
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
   localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);

mission/controller/AcsController.h

@@ -60,6 +60,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   ParameterHelper parameterHelper;
 
   uint8_t detumbleCounter = 0;
+  bool rwInvalidFlag = false;
   uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = false;
   uint16_t mekfInvalidCounter = 0;
@@ -107,6 +108,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure);
 
+  void handlePtgCtrlStrat(acs::ctrlStrategy& ptgStrat,
+                          AcsParameters::PointingLawParameters ptgLawParameters, double* quat,
+                          double* satRotRate);
+
   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,
@@ -115,9 +120,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void updateActuatorCmdData(const int16_t* mtqTargetDipole);
   void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
                              const int16_t* mtqTargetDipole);
-  void updateCtrlValData(double errAng, uint8_t safeModeStrat);
+  void updateCtrlValData(double errAng, acs::ctrlStrategy safeModeStrat);
   void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
-                         const double* tgtRotRate);
+                         const double* tgtRotRate, acs::ctrlStrategy ptgModeStrat);
   void disableCtrlValData();
 
   /* ACS Sensor Values */
@@ -214,7 +219,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   // Ctrl Values
   acsctrl::CtrlValData ctrlValData;
-  PoolEntry<uint8_t> safeStrat = PoolEntry<uint8_t>();
+  PoolEntry<uint8_t> ctrlStrat = PoolEntry<uint8_t>();
   PoolEntry<double> tgtQuat = PoolEntry<double>(4);
   PoolEntry<double> errQuat = PoolEntry<double>(4);
   PoolEntry<double> errAng = PoolEntry<double>();

mission/controller/acs/AcsParameters.cpp

@@ -290,6 +290,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x6:
           parameterWrapper->set(rwHandlingParameters.rampTime);
           break;
+        case 0x7:
+          parameterWrapper->set(rwHandlingParameters.multipleRwInvalidTimeout);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -389,6 +392,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x9:
           parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
           break;
+        case 0xA:
+          parameterWrapper->set(idleModeControllerParameters.useMekf);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -426,36 +432,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
+          parameterWrapper->set(targetModeControllerParameters.useMekf);
           break;
         case 0xB:
-          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(targetModeControllerParameters.refDirection);
           break;
         case 0xC:
-          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
+          parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
           break;
         case 0xD:
-          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->setVector(targetModeControllerParameters.quatRef);
           break;
         case 0xE:
-          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
           break;
         case 0xF:
-          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
           break;
         case 0x10:
-          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
+          parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
           break;
         case 0x11:
-          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
+          parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
           break;
         case 0x12:
-          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
+          parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
           break;
         case 0x13:
-          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
           break;
         case 0x14:
+          parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
+          break;
+        case 0x15:
           parameterWrapper->set(targetModeControllerParameters.blindRotRate);
           break;
         default:
@@ -495,18 +504,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
+          parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
           break;
         case 0xB:
-          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
+          parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
           break;
         case 0xC:
-          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
           break;
         case 0xD:
-          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
+          parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
           break;
         case 0xE:
+          parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
+          break;
+        case 0xF:
           parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
           break;
         default:
@@ -546,15 +558,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
+          parameterWrapper->set(nadirModeControllerParameters.useMekf);
           break;
         case 0xB:
-          parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
+          parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
           break;
         case 0xC:
-          parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
           break;
         case 0xD:
+          parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
+          break;
+        case 0xE:
           parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
           break;
         default:
@@ -594,12 +609,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
           break;
         case 0xA:
-          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
+          parameterWrapper->set(inertialModeControllerParameters.useMekf);
           break;
         case 0xB:
-          parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
+          parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
           break;
         case 0xC:
+          parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
+          break;
+        case 0xD:
           parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
           break;
         default:

mission/controller/acs/AcsParameters.h

@@ -798,6 +798,8 @@ class AcsParameters : public HasParametersIF {
     double stictionTorque = 0.0006;
 
     uint16_t rampTime = 10;
+
+    uint32_t multipleRwInvalidTimeout = 25;
   } rwHandlingParameters;
 
   struct RwMatrices {
@@ -844,6 +846,7 @@ class AcsParameters : public HasParametersIF {
 
     double desatMomentumRef[3] = {0, 0, 0};
     double deSatGainFactor = 1000;
+    uint8_t useMekf = true;
     uint8_t desatOn = true;
     uint8_t enableAntiStiction = true;
   } pointingLawParameters;

mission/controller/acs/Navigation.cpp

@@ -19,9 +19,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
                                   acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
   double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
-  bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
-                     sensorValues->strSet.caliQy.isValid() &&
-                     sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
+  bool quatIBValid = sensorValues->strSet.isTrustWorthy.value;
 
   if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
     mekfStatus = multiplicativeKalmanFilter.init(

mission/controller/acs/control/Detumble.cpp

@@ -7,18 +7,18 @@ Detumble::Detumble() {}
 
 Detumble::~Detumble() {}
 
-acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
+acs::ctrlStrategy Detumble::detumbleStrategy(const bool magFieldValid,
                                                  const bool satRotRateValid,
                                                  const bool magFieldRateValid,
                                                  const bool useFullDetumbleLaw) {
   if (not magFieldValid) {
-    return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
+    return acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (satRotRateValid and useFullDetumbleLaw) {
-    return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
+    return acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL;
   } else if (magFieldRateValid) {
-    return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
+    return acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
   } else {
-    return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+    return acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
   }
 }
 

mission/controller/acs/control/Detumble.h

@@ -11,7 +11,7 @@ class Detumble {
   Detumble();
   virtual ~Detumble();
 
-  acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
+  acs::ctrlStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
                                          const bool magFieldRateValid,
                                          const bool useFullDetumbleLaw);
 

mission/controller/acs/control/PtgCtrl.cpp

@@ -10,6 +10,27 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
 
 PtgCtrl::~PtgCtrl() {}
 
+acs::ctrlStrategy PtgCtrl::ptgCtrlStrategy(const bool rwAvail, const uint8_t mekfEnabled,
+                                           const bool mekfValid, const bool magFieldValid,
+                                           const uint8_t desatEnabled, const uint8_t quatValid,
+                                           const bool satRotRateValid) {
+  if (not rwAvail) {
+    return acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL;
+  } else if ((mekfEnabled and mekfValid) and (desatEnabled and magFieldValid)) {
+    return acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF;
+  } else if ((mekfEnabled and mekfValid) and not(desatEnabled or magFieldValid)) {
+    return acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF_WO_DESAT;
+  } else if (not(mekfEnabled or mekfValid) and (quatValid and satRotRateValid) and
+             (desatEnabled and magFieldValid)) {
+    return acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF;
+  } else if (not(mekfEnabled or mekfValid) and (quatValid and satRotRateValid) and
+             not(desatEnabled and magFieldValid)) {
+    return acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF_WO_DESAT;
+  } else {
+    return acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL;
+  }
+}
+
 void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
                      const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
                      double *torqueRws) {

mission/controller/acs/control/PtgCtrl.h

@@ -21,6 +21,11 @@ class PtgCtrl {
   PtgCtrl(AcsParameters *acsParameters_);
   virtual ~PtgCtrl();
 
+  acs::ctrlStrategy ptgCtrlStrategy(const bool rwAvail, const uint8_t mekfEnabled,
+                                    const bool mekfValid, const bool magFieldValid,
+                                    const uint8_t desatEnabled, const uint8_t quatValid,
+                                    const bool satRotRateValid);
+
   /* @brief: Calculates the needed torque for the pointing control mechanism
    */
   void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,

mission/controller/acs/control/SafeCtrl.cpp

@@ -9,22 +9,22 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
 
 SafeCtrl::~SafeCtrl() {}
 
-acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+acs::ctrlStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                                  const bool satRotRateValid, const bool sunDirValid,
                                                  const uint8_t mekfEnabled,
                                                  const uint8_t dampingEnabled) {
   if (not magFieldValid) {
-    return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
+    return acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (mekfEnabled and mekfValid) {
-    return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF;
+    return acs::ctrlStrategy::SAFECTRL_ACTIVE_MEKF;
   } else if (satRotRateValid and sunDirValid) {
-    return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF;
+    return acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF;
   } else if (dampingEnabled and satRotRateValid and not sunDirValid) {
-    return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING;
+    return acs::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING;
   } else if (not dampingEnabled and satRotRateValid and not sunDirValid) {
-    return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
+    return acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING;
   } else {
-    return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+    return acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
   }
 }
 

mission/controller/acs/control/SafeCtrl.h

@@ -12,7 +12,7 @@ class SafeCtrl {
   SafeCtrl(AcsParameters *acsParameters_);
   virtual ~SafeCtrl();
 
-  acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+  acs::ctrlStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                          const bool satRotRateValid, const bool sunDirValid,
                                          const uint8_t mekfEnabled, const uint8_t dampingEnabled);
 

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -94,7 +94,7 @@ enum PoolIds : lp_id_t {
   QUAT_MEKF,
   MEKF_STATUS,
   // Ctrl Values
-  SAFE_STRAT,
+  CTRL_STRAT,
   TGT_QUAT,
   ERROR_QUAT,
   ERROR_ANG,
@@ -252,7 +252,7 @@ class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
  public:
   CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {}
 
-  lp_var_t<uint8_t> safeStrat = lp_var_t<uint8_t>(sid.objectId, SAFE_STRAT, this);
+  lp_var_t<uint8_t> ctrlStrat = lp_var_t<uint8_t>(sid.objectId, CTRL_STRAT, this);
   lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
   lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
   lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);