CHANGELOG.md

@@ -14,7 +14,7 @@ will consitute of a breaking change warranting a new major release:
 - The TMTC interface changes in any shape of form.
 - The behaviour of the OBSW changes in a major shape or form relevant for operations
 
-# [v7.5.0] 2023-12-XX
+# [v7.5.0] 2023-12-06
 
 ## Changed
 
@@ -27,6 +27,13 @@ will consitute of a breaking change warranting a new major release:
 - Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate
   the time difference.
  
+## Added
+
+- Higher ACS modes can now be entered without a running `MEKF`. Higher modes will collect their
+  quaternion and rotational rate depending on the available sources.
+- `QUEST` attitude estimation was added to the `AcsController`.
+- The fused rotational rate can now be estimated from `QUEST` and the `STR`.
+
 # [v7.4.1] 2023-12-06
 
 ## Fixed
@@ -123,6 +130,16 @@ will consitute of a breaking change warranting a new major release:
   during which the SUS was not working as well as the maximum amount of invalid messages.
 - Updated battery internal resistance to new value
 
+## Changed
+
+- `Power Controller` now uses monotonic clock for calculating time difference
+- `ACS Controller` now uses monotonic clock for calculating time difference and the normal clock
+  for model calculations. The `timeDelta` is now calculated in the controller instead of
+  everywhere where it is needed.
+- `ACS Controller` now has the function `performAttitudeControl` which is called prior to passing
+  on to the relevant mode functions. It handles all telemetry relevant functions, which were
+  always called, regardless of the mode.
+
 # [v7.1.0] 2023-10-11
 
 - Bumped `eive-tmtc` to v5.8.0.

fsfw

@@ -1 +1 @@
-Subproject commit 7187f2b5cdfe163bf7ed1a8fab48900d69f4c8bf
+Subproject commit 7105e199c650303ac1a48e75aebc44182630931e

mission/acs/defs.h

@@ -22,10 +22,10 @@ enum AcsMode : Mode_t {
 
 enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 };
 
-enum SafeModeStrategy : uint8_t {
+enum ControlModeStrategy : uint8_t {
-  SAFECTRL_OFF = 0,
+  CTRL_OFF = 0,
-  SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
+  CTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
-  SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
+  CTRL_NO_SENSORS_FOR_CONTROL = 2,
   // OBSW version <= v6.1.0
   LEGACY_SAFECTRL_ACTIVE_MEKF = 10,
   LEGACY_SAFECTRL_WITHOUT_MEKF = 11,
@@ -40,14 +40,28 @@ enum SafeModeStrategy : uint8_t {
   SAFECTRL_ECLIPSE_IDELING = 19,
   SAFECTRL_DETUMBLE_FULL = 20,
   SAFECTRL_DETUMBLE_DETERIORATED = 21,
+  // Added in vNext
+  PTGCTRL_MEKF = 100,
+  PTGCTRL_STR = 101,
+  PTGCTRL_QUEST = 102,
 };
-
+namespace gps {
-enum GpsSource : uint8_t {
+enum Source : uint8_t {
   NONE,
   GPS,
   GPS_EXTRAPOLATED,
   SPG4,
 };
+}
+
+namespace rotrate {
+enum Source : uint8_t {
+  NONE,
+  SUSMGM,
+  QUEST,
+  STR,
+};
+}
 
 static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
 //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
@@ -64,9 +78,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
+//! [EXPORT] : [COMMENT] For a prolonged time, no attitude information was available for the
-//! prolonged time.
+//! Pointing Controller. Falling back to Safe Mode.
-static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH);
+static constexpr Event PTG_CTRL_NO_ATTITUDE_INFORMATION = 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

@@ -4,6 +4,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
     : ExtendedControllerBase(objectId),
       enableHkSets(enableHkSets),
       sdcMan(sdcMan),
+      attitudeEstimation(&acsParameters),
       fusedRotationEstimation(&acsParameters),
       guidance(&acsParameters),
       safeCtrl(&acsParameters),
@@ -16,10 +17,11 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets, SdCardMoun
       gyrDataRaw(this),
       gyrDataProcessed(this),
       gpsDataProcessed(this),
-      mekfData(this),
+      attitudeEstimationData(this),
       ctrlValData(this),
       actuatorCmdData(this),
-      fusedRotRateData(this) {}
+      fusedRotRateData(this),
+      fusedRotRateSourcesData(this) {}
 
 ReturnValue_t AcsController::initialize() {
   ReturnValue_t result = parameterHelper.initialize();
@@ -52,7 +54,7 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
       return HasActionsIF::EXECUTION_FINISHED;
     }
     case RESET_MEKF: {
-      navigation.resetMekf(&mekfData);
+      navigation.resetMekf(&attitudeEstimationData);
       return HasActionsIF::EXECUTION_FINISHED;
     }
     case RESTORE_MEKF_NONFINITE_RECOVERY: {
@@ -169,28 +171,32 @@ void AcsController::performAttitudeControl() {
 
   sensorProcessing.process(timeAbsolute, timeDelta, &sensorValues, &mgmDataProcessed,
                            &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
+  attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
-                                                        &gyrDataProcessed, &fusedRotRateData);
+  fusedRotationEstimation.estimateFusedRotationRate(
+      &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
+      &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
   result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
-                              &susDataProcessed, &mekfData, &acsParameters);
+                              &susDataProcessed, &attitudeEstimationData, &acsParameters);
 
-  switch (mode) {
-    case acs::SAFE:
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
     if (not mekfInvalidFlag) {
-          triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
+      triggerEvent(acs::MEKF_INVALID_INFO,
+                   static_cast<uint32_t>(attitudeEstimationData.mekfStatus.value));
       mekfInvalidFlag = true;
     }
     if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
       triggerEvent(acs::MEKF_AUTOMATIC_RESET);
-          navigation.resetMekf(&mekfData);
+      navigation.resetMekf(&attitudeEstimationData);
       mekfLost = true;
     }
   } else if (mekfInvalidFlag) {
     triggerEvent(acs::MEKF_RECOVERY);
     mekfInvalidFlag = false;
   }
+
+  switch (mode) {
+    case acs::SAFE:
       switch (submode) {
         case SUBMODE_NONE:
           performSafe();
@@ -205,35 +211,6 @@ void AcsController::performAttitudeControl() {
     case acs::PTG_TARGET_GS:
     case acs::PTG_NADIR:
     case acs::PTG_INERTIAL:
-      if (result != MultiplicativeKalmanFilter::MEKF_RUNNING and
-          result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
-        mekfInvalidCounter++;
-        if (not mekfInvalidFlag) {
-          triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
-          mekfInvalidFlag = true;
-        }
-        if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE and not mekfLost) {
-          triggerEvent(acs::MEKF_AUTOMATIC_RESET);
-          navigation.resetMekf(&mekfData);
-          mekfLost = true;
-        }
-        if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
-          // Trigger this so STR FDIR can set the device faulty.
-          EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0,
-                                       0);
-          mekfInvalidCounter = 0;
-        }
-        commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration,
-                         cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
-                         acsParameters.rwHandlingParameters.rampTime);
-        return;
-      } else {
-        if (mekfInvalidFlag) {
-          triggerEvent(acs::MEKF_RECOVERY);
-          mekfInvalidFlag = false;
-        }
-        mekfInvalidCounter = 0;
-      }
       performPointingCtrl();
       break;
   }
@@ -245,27 +222,28 @@ void AcsController::performSafe() {
   guidance.getTargetParamsSafe(sunTargetDir);
 
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
-  acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
+  acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
       fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
       acsParameters.safeModeControllerParameters.useGyr,
       acsParameters.safeModeControllerParameters.dampingDuringEclipse);
   switch (safeCtrlStrat) {
-    case (acs::SafeModeStrategy::SAFECTRL_MEKF):
+    case (acs::ControlModeStrategy::SAFECTRL_MEKF):
-      safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
+      safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value,
-                        susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
+                        attitudeEstimationData.satRotRateMekf.value,
-                        magMomMtq, errAng);
+                        susDataProcessed.sunIjkModel.value, attitudeEstimationData.quatMekf.value,
+                        sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_GYR):
+    case (acs::ControlModeStrategy::SAFECTRL_GYR):
       safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
                        susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
+    case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
       safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
                           fusedRotRateData.rotRateParallel.value,
                           fusedRotRateData.rotRateOrthogonal.value,
@@ -273,29 +251,29 @@ void AcsController::performSafe() {
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
+    case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
       safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
                                   gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
                                   errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
+    case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
       safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
                                      fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
                                      errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
+    case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING):
       errAng = NAN;
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
+    case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
       safeCtrlFailure(1, 0);
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
+    case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
       safeCtrlFailure(0, 1);
       break;
     default:
@@ -308,8 +286,8 @@ void AcsController::performSafe() {
 
   // detumble check and switch
   if (acsParameters.safeModeControllerParameters.useMekf) {
-    if (mekfData.satRotRateMekf.isValid() and
+    if (attitudeEstimationData.satRotRateMekf.isValid() and
-        VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
+        VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) >
             acsParameters.detumbleParameter.omegaDetumbleStart) {
       detumbleCounter++;
     }
@@ -340,24 +318,24 @@ void AcsController::performSafe() {
 }
 
 void AcsController::performDetumble() {
-  acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
+  acs::ControlModeStrategy 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::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL):
       detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
                            magMomMtq, acsParameters.detumbleParameter.gainFull);
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
+    case (acs::ControlModeStrategy::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::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL):
       safeCtrlFailure(1, 0);
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
+    case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL):
       safeCtrlFailure(0, 1);
       break;
     default:
@@ -369,8 +347,8 @@ void AcsController::performDetumble() {
                            acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
 
   if (acsParameters.safeModeControllerParameters.useMekf) {
-    if (mekfData.satRotRateMekf.isValid() and
+    if (attitudeEstimationData.satRotRateMekf.isValid() and
-        VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
+        VectorOperations<double>::norm(attitudeEstimationData.satRotRateMekf.value, 3) <
             acsParameters.detumbleParameter.omegaDetumbleEnd) {
       detumbleCounter++;
     }
@@ -402,6 +380,68 @@ void AcsController::performDetumble() {
 }
 
 void AcsController::performPointingCtrl() {
+  bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and
+                   sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid());
+  uint8_t useMekf = false;
+  switch (mode) {
+    case acs::PTG_IDLE:
+      useMekf = acsParameters.idleModeControllerParameters.useMekf;
+      break;
+    case acs::PTG_TARGET:
+      useMekf = acsParameters.targetModeControllerParameters.useMekf;
+      break;
+    case acs::PTG_TARGET_GS:
+      useMekf = acsParameters.gsTargetModeControllerParameters.useMekf;
+      break;
+    case acs::PTG_NADIR:
+      useMekf = acsParameters.nadirModeControllerParameters.useMekf;
+      break;
+    case acs::PTG_INERTIAL:
+      useMekf = acsParameters.inertialModeControllerParameters.useMekf;
+      break;
+  }
+  acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
+      mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
+      attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
+      fusedRotRateData.rotRateSource.isValid(), useMekf);
+
+  if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
+    ptgCtrlLostCounter++;
+    if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
+      triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
+      ptgCtrlLostCounter = 0;
+    }
+    commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
+                     cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
+                     acsParameters.rwHandlingParameters.rampTime);
+    return;
+  } else {
+    ptgCtrlLostCounter = 0;
+  }
+
+  double quatBI[4] = {0, 0, 0, 0}, rotRateB[3] = {0, 0, 0};
+  switch (ptgCtrlStrat) {
+    case acs::ControlModeStrategy::PTGCTRL_MEKF:
+      std::memcpy(quatBI, attitudeEstimationData.quatMekf.value, sizeof(quatBI));
+      std::memcpy(rotRateB, attitudeEstimationData.satRotRateMekf.value, sizeof(rotRateB));
+      break;
+    case acs::ControlModeStrategy::PTGCTRL_STR:
+      quatBI[0] = sensorValues.strSet.caliQx.value;
+      quatBI[1] = sensorValues.strSet.caliQy.value;
+      quatBI[2] = sensorValues.strSet.caliQz.value;
+      quatBI[3] = sensorValues.strSet.caliQw.value;
+      std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
+      break;
+    case acs::ControlModeStrategy::PTGCTRL_QUEST:
+      std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI));
+      std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
+      break;
+    default:
+      sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
+                 << std::endl;
+      break;
+  }
+
   uint8_t enableAntiStiction = true;
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
@@ -428,8 +468,8 @@ void AcsController::performPointingCtrl() {
     case acs::PTG_IDLE:
       guidance.targetQuatPtgSun(timeDelta, susDataProcessed.sunIjkModel.value, targetQuat,
                                 targetSatRotRate);
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
-                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
+                          errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
@@ -440,9 +480,9 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
       break;
 
@@ -450,8 +490,8 @@ void AcsController::performPointingCtrl() {
       guidance.targetQuatPtgThreeAxes(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
                                       gpsDataProcessed.gpsVelocity.value, targetQuat,
                                       targetSatRotRate);
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
-                          targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
+                          acsParameters.targetModeControllerParameters.quatRef,
                           acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
@@ -464,17 +504,17 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_TARGET_GS:
       guidance.targetQuatPtgGs(timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
                                susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate, errorQuat,
-                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
+                          errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
                      *rwPseudoInv, torquePtgRws);
       ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
@@ -485,9 +525,9 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
       break;
 
@@ -495,8 +535,8 @@ void AcsController::performPointingCtrl() {
       guidance.targetQuatPtgNadirThreeAxes(
           timeAbsolute, timeDelta, gpsDataProcessed.gpsPosition.value,
           gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate);
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
-                          targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
+                          acsParameters.nadirModeControllerParameters.quatRef,
                           acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
@@ -509,17 +549,17 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
       break;
 
     case acs::PTG_INERTIAL:
       std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
                   sizeof(targetQuat));
-      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
+      guidance.comparePtg(quatBI, rotRateB, targetQuat, targetSatRotRate,
-                          targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
+                          acsParameters.inertialModeControllerParameters.quatRef,
                           acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
                           errorSatRotRate, errorAngle);
       ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
@@ -532,9 +572,9 @@ void AcsController::performPointingCtrl() {
       actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
       ptgCtrl.ptgDesaturation(
           &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
-          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
+          mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
-          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
+          sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
-          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
+          sensorValues.rw4Set.currSpeed.value, mgtDpDes);
       enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
       break;
     default:
@@ -673,7 +713,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 = acs::SafeModeStrategy::SAFECTRL_OFF;
+    ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
     ctrlValData.setValidity(true, true);
   }
 }
@@ -750,11 +790,12 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
   localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
   poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
-  // MEKF
+  // Attitude Estimation
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
   localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
-  poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
+  localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_QUEST, &quatQuest);
+  poolManager.subscribeForDiagPeriodicPacket({attitudeEstimationData.getSid(), enableHkSets, 10.0});
   // Ctrl Values
   localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
   localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
@@ -771,7 +812,15 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
   poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
+  // Fused Rot Rate Sources
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL_SUSMGM, &rotRateOrthogonalSusMgm);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL_SUSMGM, &rotRateParallelSusMgm);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_QUEST, &rotRateTotalQuest);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_STR, &rotRateTotalStr);
+  poolManager.subscribeForRegularPeriodicPacket({fusedRotRateSourcesData.getSid(), false, 10.0});
   return returnvalue::OK;
 }
 
@@ -792,13 +841,15 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
     case acsctrl::GPS_PROCESSED_DATA:
       return &gpsDataProcessed;
     case acsctrl::MEKF_DATA:
-      return &mekfData;
+      return &attitudeEstimationData;
     case acsctrl::CTRL_VAL_DATA:
       return &ctrlValData;
     case acsctrl::ACTUATOR_CMD_DATA:
       return &actuatorCmdData;
     case acsctrl::FUSED_ROTATION_RATE_DATA:
       return &fusedRotRateData;
+    case acsctrl::FUSED_ROTATION_RATE_SOURCES_DATA:
+      return &fusedRotRateSourcesData;
     default:
       return nullptr;
   }

mission/controller/AcsController.h

@@ -17,6 +17,7 @@
 #include <mission/acs/susMax1227Helpers.h>
 #include <mission/config/torquer.h>
 #include <mission/controller/acs/ActuatorCmd.h>
+#include <mission/controller/acs/AttitudeEstimation.h>
 #include <mission/controller/acs/FusedRotationEstimation.h>
 #include <mission/controller/acs/Guidance.h>
 #include <mission/controller/acs/MultiplicativeKalmanFilter.h>
@@ -67,6 +68,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   AcsParameters acsParameters;
   SensorProcessing sensorProcessing;
+  AttitudeEstimation attitudeEstimation;
   FusedRotationEstimation fusedRotationEstimation;
   Navigation navigation;
   ActuatorCmd actuatorCmd;
@@ -82,7 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   uint8_t detumbleCounter = 0;
   uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = false;
-  uint16_t mekfInvalidCounter = 0;
+  uint16_t ptgCtrlLostCounter = 0;
   bool safeCtrlFailureFlag = false;
   uint8_t safeCtrlFailureCounter = 0;
   uint8_t resetMekfCount = 0;
@@ -239,11 +241,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
   PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
 
-  // MEKF
+  // Attitude Estimation
-  acsctrl::MekfData mekfData;
+  acsctrl::AttitudeEstimationData attitudeEstimationData;
   PoolEntry<double> quatMekf = PoolEntry<double>(4);
   PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
   PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
+  PoolEntry<double> quatQuest = PoolEntry<double>(4);
 
   // Ctrl Values
   acsctrl::CtrlValData ctrlValData;
@@ -264,6 +267,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
   PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
   PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
+  PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
+
+  // Fused Rot Rate Sources
+  acsctrl::FusedRotRateSourcesData fusedRotRateSourcesData;
+  PoolEntry<double> rotRateOrthogonalSusMgm = PoolEntry<double>(3);
+  PoolEntry<double> rotRateParallelSusMgm = PoolEntry<double>(3);
+  PoolEntry<double> rotRateTotalSusMgm = PoolEntry<double>(3);
+  PoolEntry<double> rotRateTotalQuest = PoolEntry<double>(3);
+  PoolEntry<double> rotRateTotalStr = PoolEntry<double>(3);
 
   // Initial delay to make sure all pool variables have been initialized their owners
   Countdown initialCountdown = Countdown(INIT_DELAY);

mission/controller/PowerController.cpp

@@ -216,6 +216,7 @@ void PowerController::calculateStateOfCharge() {
         pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
       }
     }
+    return;
   }
 
   // commit to dataset

mission/controller/acs/AcsParameters.cpp

@@ -24,11 +24,20 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(onBoardParams.sampleTime);
           break;
         case 0x1:
-          parameterWrapper->set(onBoardParams.mekfViolationTimer);
+          parameterWrapper->set(onBoardParams.ptgCtrlLostTimer);
           break;
         case 0x2:
           parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse);
           break;
+        case 0x3:
+          parameterWrapper->set(onBoardParams.fusedRateFromStr);
+          break;
+        case 0x4:
+          parameterWrapper->set(onBoardParams.fusedRateFromQuest);
+          break;
+        case 0x5:
+          parameterWrapper->set(onBoardParams.questFilterWeight);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -425,6 +434,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;
       }
@@ -462,36 +474,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:
@@ -531,18 +546,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:
@@ -582,15 +600,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:
@@ -630,12 +651,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

@@ -18,8 +18,11 @@ class AcsParameters : public HasParametersIF {
 
   struct OnBoardParams {
     double sampleTime = 0.4;  // [s]
-    uint16_t mekfViolationTimer = 750;
+    uint16_t ptgCtrlLostTimer = 750;
     uint8_t fusedRateSafeDuringEclipse = true;
+    uint8_t fusedRateFromStr = false;
+    uint8_t fusedRateFromQuest = false;
+    double questFilterWeight = 0.0;
   } onBoardParams;
 
   struct InertiaEIVE {
@@ -861,6 +864,7 @@ class AcsParameters : public HasParametersIF {
     double deSatGainFactor = 1000;
     uint8_t desatOn = true;
     uint8_t enableAntiStiction = true;
+    uint8_t useMekf = false;
   } pointingLawParameters;
 
   struct IdleModeControllerParameters : PointingLawParameters {

mission/controller/acs/AttitudeEstimation.cpp

@@ -0,0 +1,111 @@
+#include "AttitudeEstimation.h"
+
+AttitudeEstimation::AttitudeEstimation(AcsParameters *acsParameters_) {
+  acsParameters = acsParameters_;
+}
+
+AttitudeEstimation::~AttitudeEstimation() {}
+
+void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
+                               acsctrl::MgmDataProcessed *mgmData,
+                               acsctrl::AttitudeEstimationData *attitudeEstimation) {
+  if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
+          mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid())) {
+    {
+      PoolReadGuard pg{attitudeEstimation};
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
+        attitudeEstimation->quatQuest.setValid(false);
+      }
+    }
+    return;
+  }
+
+  // Normalize Data
+  double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
+         normSusI[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(susData->susVecTot.value, normMgmB, 3);
+  VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
+  VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
+  VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normSusI, 3);
+
+  // Create Helper Vectors
+  double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
+         helperSum[3] = {0, 0, 0};
+  VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
+  VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
+  VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
+  VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
+  VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
+  VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
+
+  // Sensor Weights
+  double kSus = 0, kMgm = 0;
+  kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
+  kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
+
+  // Weighted Vectors
+  double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
+         kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
+  VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
+  VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
+  VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
+  VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
+
+  // Some weird Angles
+  double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
+                     (VectorOperations<double>::dot(weightedSusB, normSusI) +
+                      VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
+                 VectorOperations<double>::dot(helperCross, kSumVec);
+  double beta = VectorOperations<double>::dot(helperSum, kSumVec);
+  double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
+
+  // I don't even know what this is supposed to be
+  double constPlus =
+      1. / (2 * std::sqrt(gamma * (gamma + alpha) *
+                          (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
+  double constMinus =
+      1. / (2 * std::sqrt(gamma * (gamma - alpha) *
+                          (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
+
+  // Calculate Quaternion
+  double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
+         qRotVecPt1[3] = {0, 0, 0};
+  if (alpha >= 0) {
+    // Scalar Part
+    qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
+    // Rotational Vector Part
+    VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
+    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
+    VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
+    std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
+
+    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
+    QuaternionOperations::normalize(qBI, qBI);
+  } else {
+    // Scalar Part
+    qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
+    // Rotational Vector Part
+    VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
+    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
+    VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
+    std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
+
+    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
+    QuaternionOperations::normalize(qBI, qBI);
+  }
+  // Low Pass
+  if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
+    QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
+  }
+  {
+    PoolReadGuard pg{attitudeEstimation};
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double));
+      attitudeEstimation->quatQuest.setValid(true);
+    }
+  }
+}

mission/controller/acs/AttitudeEstimation.h

@@ -0,0 +1,31 @@
+#ifndef MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
+#define MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_
+
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
+
+#include <cmath>
+#include <iostream>
+
+class AttitudeEstimation {
+ public:
+  AttitudeEstimation(AcsParameters *acsParameters_);
+  virtual ~AttitudeEstimation();
+  ;
+
+  void quest(acsctrl::SusDataProcessed *susData, acsctrl::MgmDataProcessed *mgmData,
+             acsctrl::AttitudeEstimationData *attitudeEstimation);
+
+ protected:
+ private:
+  AcsParameters *acsParameters;
+
+  double qOld[4] = {0, 0, 0, 0};
+
+  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
+};
+
+#endif /* MISSION_CONTROLLER_ACS_ATTITUDEESTIMATION_H_ */

mission/controller/acs/CMakeLists.txt

@@ -2,6 +2,7 @@ target_sources(
   ${LIB_EIVE_MISSION}
   PRIVATE AcsParameters.cpp
           ActuatorCmd.cpp
+          AttitudeEstimation.cpp
           FusedRotationEstimation.cpp
           Guidance.cpp
           Igrf13Model.cpp

mission/controller/acs/FusedRotationEstimation.cpp

@@ -4,19 +4,220 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
   acsParameters = acsParameters_;
 }
 
-void FusedRotationEstimation::estimateFusedRotationRateSafe(
+void FusedRotationEstimation::estimateFusedRotationRate(
     acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
-    acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
+    acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
+    acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
+    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
+    acsctrl::FusedRotRateData *fusedRotRateData) {
+  estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData);
+  estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData);
+  estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
+                                  fusedRotRateSourcesData);
+
+  if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
+      acsParameters->onBoardParams.fusedRateFromStr) {
+    PoolReadGuard pg(fusedRotRateData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->rotRateOrthogonal.setValid(false);
+      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->rotRateParallel.setValid(false);
+      std::memcpy(fusedRotRateData->rotRateTotal.value,
+                  fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
+      fusedRotRateData->rotRateTotal.setValid(true);
+      fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
+      fusedRotRateData->rotRateSource.setValid(true);
+    }
+  } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
+             acsParameters->onBoardParams.fusedRateFromQuest) {
+    PoolReadGuard pg(fusedRotRateData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->rotRateOrthogonal.setValid(false);
+      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->rotRateParallel.setValid(false);
+      std::memcpy(fusedRotRateData->rotRateTotal.value,
+                  fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
+      fusedRotRateData->rotRateTotal.setValid(true);
+      fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
+      fusedRotRateData->rotRateSource.setValid(true);
+    }
+  } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
+    std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
+                fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double));
+    fusedRotRateData->rotRateOrthogonal.setValid(
+        fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid());
+    std::memcpy(fusedRotRateData->rotRateParallel.value,
+                fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double));
+    fusedRotRateData->rotRateParallel.setValid(
+        fusedRotRateSourcesData->rotRateParallelSusMgm.isValid());
+    std::memcpy(fusedRotRateData->rotRateTotal.value,
+                fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
+    fusedRotRateData->rotRateTotal.setValid(true);
+    fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
+    fusedRotRateData->rotRateSource.setValid(true);
+  } else {
+    PoolReadGuard pg(fusedRotRateData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->setValidity(false, true);
+      fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
+      fusedRotRateData->rotRateSource.setValid(true);
+    }
+  }
+}
+
+void FusedRotationEstimation::estimateFusedRotationRateStr(
+    ACS::SensorValues *sensorValues, const double timeDelta,
+    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
+  if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
+          sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
+    {
+      PoolReadGuard pg(fusedRotRateSourcesData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
+      }
+    }
+    std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
+    return;
+  }
+
+  double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
+                       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
+  if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
+    double quatOldInv[4] = {0, 0, 0, 0};
+    double quatDelta[4] = {0, 0, 0, 0};
+
+    QuaternionOperations::inverse(quatOldStr, quatOldInv);
+    QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
+    if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
+      QuaternionOperations::normalize(quatDelta);
+    }
+
+    double rotVec[3] = {0, 0, 0};
+    double angle = QuaternionOperations::getAngle(quatDelta);
+    if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
+      {
+        PoolReadGuard pg(fusedRotRateSourcesData);
+        if (pg.getReadResult() == returnvalue::OK) {
+          std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
+                      3 * sizeof(double));
+          fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
+        }
+      }
+      std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
+      return;
+    }
+    VectorOperations<double>::normalize(quatDelta, rotVec, 3);
+    VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
+    {
+      PoolReadGuard pg(fusedRotRateSourcesData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
+      }
+    }
+    std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
+    return;
+  }
+  {
+    PoolReadGuard pg(fusedRotRateSourcesData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
+    }
+  }
+  std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
+  return;
+}
+
+void FusedRotationEstimation::estimateFusedRotationRateQuest(
+    acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
+    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
+  if (not attitudeEstimationData->quatQuest.isValid()) {
+    {
+      PoolReadGuard pg(fusedRotRateSourcesData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
+                    3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
+      }
+    }
+    std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
+  }
+
+  if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
+    double quatOldInv[4] = {0, 0, 0, 0};
+    double quatDelta[4] = {0, 0, 0, 0};
+
+    QuaternionOperations::inverse(quatOldQuest, quatOldInv);
+    QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta);
+    if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
+      QuaternionOperations::normalize(quatDelta);
+    }
+
+    double rotVec[3] = {0, 0, 0};
+    double angle = QuaternionOperations::getAngle(quatDelta);
+    if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
+      {
+        PoolReadGuard pg(fusedRotRateSourcesData);
+        if (pg.getReadResult() == returnvalue::OK) {
+          std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
+                      3 * sizeof(double));
+          fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
+        }
+      }
+      std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
+      return;
+    }
+    VectorOperations<double>::normalize(quatDelta, rotVec, 3);
+    VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
+    {
+      PoolReadGuard pg(fusedRotRateSourcesData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
+      }
+    }
+    std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
+    return;
+  }
+  {
+    PoolReadGuard pg(fusedRotRateSourcesData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
+    }
+  }
+  std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
+  return;
+}
+
+void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
+    acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
+    acsctrl::GyrDataProcessed *gyrDataProcessed,
+    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
   if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
-       not fusedRotRateData->rotRateTotal.isValid()) or
+       not fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) or
       (not susDataProcessed->susVecTotDerivative.isValid() and
        not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
     {
-      PoolReadGuard pg(fusedRotRateData);
+      PoolReadGuard pg(fusedRotRateSourcesData);
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+      if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+        std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
-      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
+                    3 * sizeof(double));
-      fusedRotRateData->setValidity(false, true);
+        fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
+        std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
+                    3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
+                    3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
+      }
     }
     // store for calculation of angular acceleration
     if (gyrDataProcessed->gyrVecTot.isValid()) {
@@ -25,7 +226,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
     return;
   }
   if (not susDataProcessed->susVecTot.isValid()) {
-    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
+    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
     // store for calculation of angular acceleration
     if (gyrDataProcessed->gyrVecTot.isValid()) {
       std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@@ -49,7 +250,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
     VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
                                         fusedRotRateParallel, 3);
   } else {
-    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
+    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
     // store for calculation of angular acceleration
     if (gyrDataProcessed->gyrVecTot.isValid()) {
       std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@@ -71,12 +272,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
   VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
 
   {
-    PoolReadGuard pg(fusedRotRateData);
+    PoolReadGuard pg(fusedRotRateSourcesData);
-    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, fusedRotRateOrthogonal,
                   3 * sizeof(double));
-    std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
+      fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(true);
-    std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
+      std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, fusedRotRateParallel,
-    fusedRotRateData->setValidity(true, true);
+                  3 * sizeof(double));
+      fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(true);
+      std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
+                  3 * sizeof(double));
+      fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
+    }
   }
 
   // store for calculation of angular acceleration
@@ -86,31 +293,44 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
 }
 
 void FusedRotationEstimation::estimateFusedRotationRateEclipse(
-    acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
+    acsctrl::GyrDataProcessed *gyrDataProcessed,
+    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
   if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or
       not gyrDataProcessed->gyrVecTot.isValid() or
-      VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
+      VectorOperations<double>::norm(fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3) == 0) {
     {
-      PoolReadGuard pg(fusedRotRateData);
+      PoolReadGuard pg(fusedRotRateSourcesData);
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+      if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+        std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
-      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
+                    3 * sizeof(double));
-      fusedRotRateData->setValidity(false, true);
+        fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
+        std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
+                    3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
+                    3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
+      }
     }
     return;
   }
   double angAccelB[3] = {0, 0, 0};
   VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
   double fusedRotRateTotal[3] = {0, 0, 0};
-  VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal,
+  VectorOperations<double>::add(fusedRotRateSourcesData->rotRateTotalSusMgm.value, angAccelB,
-                                3);
+                                fusedRotRateTotal, 3);
   {
-    PoolReadGuard pg(fusedRotRateData);
+    PoolReadGuard pg(fusedRotRateSourcesData);
-    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+    if (pg.getReadResult() == returnvalue::OK) {
-    fusedRotRateData->rotRateOrthogonal.setValid(false);
+      std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
-    std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+                  3 * sizeof(double));
-    fusedRotRateData->rotRateParallel.setValid(false);
+      fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
-    std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
+      std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
-    fusedRotRateData->rotRateTotal.setValid(true);
+                  3 * sizeof(double));
+      fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
+      std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
+                  3 * sizeof(double));
+      fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
+    }
   }
 }

mission/controller/acs/FusedRotationEstimation.h

@@ -2,28 +2,46 @@
 #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
 
 #include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/SensorValues.h>
 #include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class FusedRotationEstimation {
  public:
   FusedRotationEstimation(AcsParameters *acsParameters_);
 
-  void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
+  void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                 ACS::SensorValues *sensorValues,
+                                 acsctrl::AttitudeEstimationData *attitudeEstimationData,
+                                 const double timeDelta,
+                                 acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
                                  acsctrl::FusedRotRateData *fusedRotRateData);
 
  protected:
  private:
-  static constexpr double ZERO_VEC[3] = {0, 0, 0};
+  static constexpr double ZERO_VEC3[3] = {0, 0, 0};
+  static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
 
   AcsParameters *acsParameters;
+  double quatOldQuest[4] = {0, 0, 0, 0};
+  double quatOldStr[4] = {0, 0, 0, 0};
   double rotRateOldB[3] = {0, 0, 0};
 
+  void estimateFusedRotationRateSusMgm(acsctrl::SusDataProcessed *susDataProcessed,
+                                       acsctrl::MgmDataProcessed *mgmDataProcessed,
+                                       acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                       acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
   void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
-                                        acsctrl::FusedRotRateData *fusedRotRateData);
+                                        acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
+  void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData,
+                                      const double timeDelta,
+                                      acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
+  void estimateFusedRotationRateStr(ACS::SensorValues *sensorValues, const double timeDelta,
+                                    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
 };
 
 #endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */

mission/controller/acs/Guidance.cpp

@@ -495,24 +495,24 @@ void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double time
 
 ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
                                                 double *rwPseudoInv) {
-  bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
+  bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
-  bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid());
+  bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
-  bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid());
+  bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
-  bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid());
+  bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
 
-  if (rw1valid && rw2valid && rw3valid && rw4valid) {
+  if (rw1valid and rw2valid and rw3valid and rw4valid) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
     return returnvalue::OK;
-  } else if (!rw1valid && rw2valid && rw3valid && rw4valid) {
+  } else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double));
     return returnvalue::OK;
-  } else if (rw1valid && !rw2valid && rw3valid && rw4valid) {
+  } else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double));
     return returnvalue::OK;
-  } else if (rw1valid && rw2valid && !rw3valid && rw4valid) {
+  } else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double));
     return returnvalue::OK;
-  } else if (rw1valid && rw2valid && rw3valid && !rw4valid) {
+  } else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
     std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
     return returnvalue::OK;
   } else {

mission/controller/acs/MultiplicativeKalmanFilter.cpp

@@ -19,7 +19,7 @@ MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
 ReturnValue_t MultiplicativeKalmanFilter::init(
     const double *magneticField_, const bool validMagField_, const double *sunDir_,
     const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-    const bool validMagModel, acsctrl::MekfData *mekfData,
+    const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
     AcsParameters *acsParameters) {  // valids for "model measurements"?
   // check for valid mag/sun
   if (validMagField_ && validSS && validSSModel && validMagModel) {
@@ -191,7 +191,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
     const double *quaternionSTR, const bool validSTR_, const double *rateGYRs_,
     const bool validGYRs_, const double *magneticField_, const bool validMagField_,
     const double *sunDir_, const bool validSS, const double *sunDirJ, const bool validSSModel,
-    const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
+    const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
     AcsParameters *acsParameters) {
   // Check for GYR Measurements
   int MDF = 0;  // Matrix Dimension Factor
@@ -1090,7 +1090,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   return MEKF_RUNNING;
 }
 
-ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
+ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::AttitudeEstimationData *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}};
@@ -1100,7 +1100,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
   return MEKF_UNINITIALIZED;
 }
 
-void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
+void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData,
                                                           MekfStatus mekfStatus) {
   {
     PoolReadGuard pg(mekfData);
@@ -1115,7 +1115,7 @@ void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mek
   }
 }
 
-void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus,
+void MultiplicativeKalmanFilter::updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus,
                                                double quat[4], double satRotRate[3]) {
   {
     PoolReadGuard pg(mekfData);

mission/controller/acs/MultiplicativeKalmanFilter.h

@@ -21,7 +21,7 @@ class MultiplicativeKalmanFilter {
   MultiplicativeKalmanFilter();
   virtual ~MultiplicativeKalmanFilter();
 
-  ReturnValue_t reset(acsctrl::MekfData *mekfData);
+  ReturnValue_t reset(acsctrl::AttitudeEstimationData *mekfData);
 
   /* @brief: init() - This function initializes the Kalman Filter and will provide the first
    * quaternion through the QUEST algorithm
@@ -32,7 +32,7 @@ class MultiplicativeKalmanFilter {
    */
   ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
                      const bool validSS, const double *sunDirJ, const bool validSSModel,
-                     const double *magFieldJ, const bool validMagModel, acsctrl::MekfData *mekfData,
+                     const double *magFieldJ, const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
                      AcsParameters *acsParameters);
 
   /* @brief:  mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
@@ -53,7 +53,7 @@ class MultiplicativeKalmanFilter {
                         const bool validGYRs_, const double *magneticField_,
                         const bool validMagField_, const double *sunDir_, const bool validSS,
                         const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
-                        const bool validMagModel, acsctrl::MekfData *mekfData,
+                        const bool validMagModel, acsctrl::AttitudeEstimationData *mekfData,
                         AcsParameters *acsParameters);
 
   enum MekfStatus : uint8_t {
@@ -99,8 +99,8 @@ class MultiplicativeKalmanFilter {
   double biasGYR[3]; /*Between measured and estimated sat Rate*/
                      /*Parameter INIT*/
   /*Functions*/
-  void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
+  void updateDataSetWithoutData(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus);
-  void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
+  void updateDataSet(acsctrl::AttitudeEstimationData *mekfData, MekfStatus mekfStatus, double quat[4],
                      double satRotRate[3]);
 };
 

mission/controller/acs/Navigation.cpp

@@ -16,7 +16,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
                                   acsctrl::GyrDataProcessed *gyrDataProcessed,
                                   acsctrl::MgmDataProcessed *mgmDataProcessed,
                                   acsctrl::SusDataProcessed *susDataProcessed,
-                                  acsctrl::MekfData *mekfData, AcsParameters *acsParameters) {
+                                  acsctrl::AttitudeEstimationData *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.isTrustWorthy.value;
@@ -41,7 +42,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
   }
 }
 
-void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
+void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
   mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
 }
 
@@ -54,7 +55,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
     {
       PoolReadGuard pg(gpsDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        gpsDataProcessed->source = acs::GpsSource::SPG4;
+        gpsDataProcessed->source = acs::gps::Source::SPG4;
         gpsDataProcessed->source.setValid(true);
         std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
         gpsDataProcessed->gpsPosition.setValid(true);
@@ -66,7 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat
     {
       PoolReadGuard pg(gpsDataProcessed);
       if (pg.getReadResult() == returnvalue::OK) {
-        gpsDataProcessed->source = acs::GpsSource::NONE;
+        gpsDataProcessed->source = acs::gps::Source::NONE;
         gpsDataProcessed->source.setValid(true);
         std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
         gpsDataProcessed->gpsPosition.setValid(false);

mission/controller/acs/Navigation.h

@@ -17,9 +17,9 @@ class Navigation {
   ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
                         acsctrl::GyrDataProcessed *gyrDataProcessed,
                         acsctrl::MgmDataProcessed *mgmDataProcessed,
-                        acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
+                        acsctrl::SusDataProcessed *susDataProcessed, acsctrl::AttitudeEstimationData *mekfData,
                         AcsParameters *acsParameters);
-  void resetMekf(acsctrl::MekfData *mekfData);
+  void resetMekf(acsctrl::AttitudeEstimationData *mekfData);
 
   ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
   ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);

mission/controller/acs/SensorProcessing.cpp

@@ -1,7 +1,5 @@
 #include "SensorProcessing.h"
 
-#include <fsfw/timemanager/Stopwatch.h>
-
 SensorProcessing::SensorProcessing() {}
 
 SensorProcessing::~SensorProcessing() {}
@@ -17,7 +15,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   // ------------------------------------------------
   double magIgrfModel[3] = {0.0, 0.0, 0.0};
   bool gpsValid = false;
-  if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
+  if (gpsDataProcessed->source.value != acs::gps::Source::NONE) {
     // There seems to be a bug here, which causes the model vector to drift until infinity, if the
     // model class is not initialized new every time. Works for now, but should be investigated.
     Igrf13Model igrf13;
@@ -527,9 +525,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
   // init variables
   double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
          gpsVelocityE[3] = {0, 0, 0};
-  uint8_t gpsSource = acs::GpsSource::NONE;
+  uint8_t gpsSource = acs::gps::Source::NONE;
   // We do not trust the GPS and therefore it shall die here if SPG4 is running
-  if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) {
+  if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
     MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
                                                     gdLongitude, altitude);
     double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
@@ -572,7 +570,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
 
     validSavedPosSatE = true;
 
-    gpsSource = acs::GpsSource::GPS;
+    gpsSource = acs::gps::Source::GPS;
   }
   {
     PoolReadGuard pg(gpsDataProcessed);

mission/controller/acs/control/Detumble.cpp

@@ -7,18 +7,18 @@ Detumble::Detumble() {}
 
 Detumble::~Detumble() {}
 
-acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
+acs::ControlModeStrategy 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::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (satRotRateValid and useFullDetumbleLaw) {
-    return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
+    return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
   } else if (magFieldRateValid) {
-    return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
+    return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
   } else {
-    return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+    return acs::ControlModeStrategy::CTRL_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::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
                                          const bool magFieldRateValid,
                                          const bool useFullDetumbleLaw);
 

mission/controller/acs/control/PtgCtrl.cpp

@@ -10,6 +10,22 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_
 
 PtgCtrl::~PtgCtrl() {}
 
+acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
+    const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid,
+    const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) {
+  if (not magFieldValid) {
+    return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
+  } else if (mekfEnabled and mekfValid) {
+    return acs::ControlModeStrategy::PTGCTRL_MEKF;
+  } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
+    return acs::ControlModeStrategy::PTGCTRL_STR;
+  } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
+    return acs::ControlModeStrategy::PTGCTRL_QUEST;
+  } else {
+    return acs::ControlModeStrategy::CTRL_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

@@ -2,6 +2,7 @@
 #define PTGCTRL_H_
 
 #include <math.h>
+#include <mission/acs/defs.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/SensorValues.h>
 #include <stdio.h>
@@ -9,7 +10,7 @@
 class PtgCtrl {
   /*
    * @brief: This class handles the pointing control mechanism. Calculation of an commanded
-   * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation
+   * torque for the reaction wheels, and magnetic Field strength for magnetorquer for desaturation
    *
    * @note: A description of the used algorithms can be found in
    * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110
@@ -21,6 +22,12 @@ class PtgCtrl {
   PtgCtrl(AcsParameters *acsParameters_);
   virtual ~PtgCtrl();
 
+  acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+                                                const bool strValid, const bool questValid,
+                                                const bool fusedRateValid,
+                                                const uint8_t rotRateSource,
+                                                const uint8_t mekfEnabled);
+
   /* @brief: Calculates the needed torque for the pointing control mechanism
    */
   void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
@@ -36,7 +43,7 @@ class PtgCtrl {
                        const int32_t speedRw3, double *mgtDpDes);
 
   /* @brief: Commands the stiction torque in case wheel speed is to low
-   * 		 torqueCommand  modified torque after antistiction
+   * 		 torqueCommand  modified torque after anti-stiction
    */
   void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed);
 

mission/controller/acs/control/SafeCtrl.cpp

@@ -9,40 +9,40 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
 
 SafeCtrl::~SafeCtrl() {}
 
-acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                                  const bool satRotRateValid, const bool sunDirValid,
                                                  const bool fusedRateTotalValid,
                                                  const uint8_t mekfEnabled,
                                                  const uint8_t gyrEnabled,
                                                  const uint8_t dampingEnabled) {
   if (not magFieldValid) {
-    return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
+    return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (mekfEnabled and mekfValid) {
-    return acs::SafeModeStrategy::SAFECTRL_MEKF;
+    return acs::ControlModeStrategy::SAFECTRL_MEKF;
   } else if (sunDirValid) {
     if (gyrEnabled and satRotRateValid) {
-      return acs::SafeModeStrategy::SAFECTRL_GYR;
+      return acs::ControlModeStrategy::SAFECTRL_GYR;
     } else if (not gyrEnabled and fusedRateTotalValid) {
-      return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
+      return acs::ControlModeStrategy::SAFECTRL_SUSMGM;
     } else {
-      return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+      return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
     }
   } else if (not sunDirValid) {
     if (dampingEnabled) {
       if (gyrEnabled and satRotRateValid) {
-        return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
+        return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
       } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
-        return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
+        return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
       } else {
-        return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+        return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
       }
     } else if (not dampingEnabled and satRotRateValid) {
-      return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
+      return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING;
     } else {
-      return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+      return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
     }
   } else {
-    return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+    return acs::ControlModeStrategy::CTRL_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::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                          const bool satRotRateValid, const bool sunDirValid,
                                          const bool fusedRateTotalValid, const uint8_t mekfEnabled,
                                          const uint8_t gyrEnabled, const uint8_t dampingEnabled);

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -20,6 +20,7 @@ enum SetIds : uint32_t {
   CTRL_VAL_DATA,
   ACTUATOR_CMD_DATA,
   FUSED_ROTATION_RATE_DATA,
+  FUSED_ROTATION_RATE_SOURCES_DATA,
   TLE_SET,
 };
 
@@ -96,6 +97,7 @@ enum PoolIds : lp_id_t {
   SAT_ROT_RATE_MEKF,
   QUAT_MEKF,
   MEKF_STATUS,
+  QUAT_QUEST,
   // Ctrl Values
   SAFE_STRAT,
   TGT_QUAT,
@@ -110,6 +112,13 @@ enum PoolIds : lp_id_t {
   ROT_RATE_ORTHOGONAL,
   ROT_RATE_PARALLEL,
   ROT_RATE_TOTAL,
+  ROT_RATE_SOURCE,
+  // Fused Rotation Rate Sources
+  ROT_RATE_ORTHOGONAL_SUSMGM,
+  ROT_RATE_PARALLEL_SUSMGM,
+  ROT_RATE_TOTAL_SUSMGM,
+  ROT_RATE_TOTAL_QUEST,
+  ROT_RATE_TOTAL_STR,
 };
 
 static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@@ -119,10 +128,11 @@ static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
 static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
 static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
 static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
-static constexpr uint8_t MEKF_SET_ENTRIES = 3;
+static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4;
 static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
-static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
+static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 4;
+static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
 
 /**
  * @brief   Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@@ -246,13 +256,14 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
  private:
 };
 
-class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
+class AttitudeEstimationData : public StaticLocalDataSet<ATTITUDE_ESTIMATION_SET_ENTRIES> {
  public:
-  MekfData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
+  AttitudeEstimationData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, MEKF_DATA) {}
 
   lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
   lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);
   lp_var_t<uint8_t> mekfStatus = lp_var_t<uint8_t>(sid.objectId, MEKF_STATUS, this);
+  lp_vec_t<double, 4> quatQuest = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
 
  private:
 };
@@ -291,6 +302,25 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
       lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
   lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
   lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
+  lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
+
+ private:
+};
+
+class FusedRotRateSourcesData : public StaticLocalDataSet<FUSED_ROT_RATE_SOURCES_SET_ENTRIES> {
+ public:
+  FusedRotRateSourcesData(HasLocalDataPoolIF* hkOwner)
+      : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_SOURCES_DATA) {}
+
+  lp_vec_t<double, 3> rotRateOrthogonalSusMgm =
+      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL_SUSMGM, this);
+  lp_vec_t<double, 3> rotRateParallelSusMgm =
+      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL_SUSMGM, this);
+  lp_vec_t<double, 3> rotRateTotalSusMgm =
+      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this);
+  lp_vec_t<double, 3> rotRateTotalQuest =
+      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_QUEST, this);
+  lp_vec_t<double, 3> rotRateTotalStr = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL_STR, this);
 
  private:
 };

mission/system/acs/StrFdir.cpp

@@ -6,7 +6,7 @@ StrFdir::StrFdir(object_id_t strObject)
     : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
 
 ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
-  if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
+  if (event->getEvent() == acs::PTG_CTRL_NO_ATTITUDE_INFORMATION) {
     setFaulty(event->getEvent());
     return returnvalue::OK;
   }