Merge branch 'main' into psb-update

CHANGELOG.md

@@ -25,6 +25,13 @@ will consitute of a breaking change warranting a new major release:
 - Bugfix in PLOC MPSoC HK set: Set and variables were not set valid.
 - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode
   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
+- Changed PTG Strat priorities to favor STR before MEKF.
 
 ## 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 fusedRateFromStr = true;
-    uint8_t fusedRateFromQuest = false;
+    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,
+    const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed,
-    acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
+    acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
-    acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
+    ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData,
-    acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData,
+    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
+  if (not(mode == acs::AcsMode::SAFE) and (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
-      acsParameters->onBoardParams.fusedRateFromStr) {
+                                           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
+  } else if (not(mode == acs::AcsMode::SAFE) and
-             acsParameters->onBoardParams.fusedRateFromQuest) {
+             (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,

mission/controller/acs/control/PtgCtrl.cpp

@@ -15,10 +15,10 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
     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 (mekfEnabled and mekfValid) {
+    return acs::ControlModeStrategy::PTGCTRL_MEKF;
   } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
     return acs::ControlModeStrategy::PTGCTRL_QUEST;
   }
@@ -221,6 +221,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
             rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
           } else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
             rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
+          } else {
+            rwCmdSpeeds[i] = 0;
           }
         }
       }