Merge pull request 'Fused Rotation Rate for Safe Mode' (#867) from fused-rot-rate-safe into main

Reviewed-on: #867
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -27,6 +27,11 @@ will consitute of a breaking change warranting a new major release:
   and is triggered by the `AcsController` now.
 - Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`.
 
+## Changed
+
+- `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes
+- QUEST and STR rates are now allowed per default
+
 # [v7.6.1] 2024-02-05
 
 ## Changed

mission/controller/AcsController.cpp

@@ -173,7 +173,7 @@ void AcsController::performAttitudeControl() {
                            &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData);
   fusedRotationEstimation.estimateFusedRotationRate(
-      &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
+      mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues,
       &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData);
   result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
                               &susDataProcessed, &attitudeEstimationData, &acsParameters);

mission/controller/acs/AcsParameters.h

@@ -20,8 +20,8 @@ class AcsParameters : public HasParametersIF {
     double sampleTime = 0.4;  // [s]
     uint16_t ptgCtrlLostTimer = 750;
     uint8_t fusedRateSafeDuringEclipse = true;
-    uint8_t fusedRateFromStr = false;
-    uint8_t fusedRateFromQuest = false;
+    uint8_t fusedRateFromStr = true;
+    uint8_t fusedRateFromQuest = true;
     double questFilterWeight = 0.0;
   } onBoardParams;
 

mission/controller/acs/FusedRotationEstimation.cpp

@@ -5,18 +5,18 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
 }
 
 void FusedRotationEstimation::estimateFusedRotationRate(
-    acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
-    acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
-    acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
-    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
+    const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
+    acsctrl::MgmDataProcessed *mgmDataProcessed, 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) {
+  if (not(mode == acs::AcsMode::SAFE) and (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));
@@ -29,8 +29,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
       fusedRotRateData->rotRateSource.setValid(true);
     }
-  } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
-             acsParameters->onBoardParams.fusedRateFromQuest) {
+  } else if (not(mode == acs::AcsMode::SAFE) and
+             (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));

mission/controller/acs/FusedRotationEstimation.h

@@ -12,7 +12,7 @@ class FusedRotationEstimation {
  public:
   FusedRotationEstimation(AcsParameters *acsParameters_);
 
-  void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed,
+  void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
                                  acsctrl::MgmDataProcessed *mgmDataProcessed,
                                  acsctrl::GyrDataProcessed *gyrDataProcessed,
                                  ACS::SensorValues *sensorValues,