Merge branch 'main' into mekf-fix

CHANGELOG.md

@@ -18,9 +18,20 @@ will consitute of a breaking change warranting a new major release:
 
 ## Fixed
 
+- PLOC SUPV sets: Added missing `PoolReadGuard` instantiations when reading boot status report
+  and latchup status report.
 - PLOC SUPV latchup report could not be handled previously.
 - Bugfix in PLOC SUPV latchup report parsing.
 - 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.
 
 # [v7.6.1] 2024-02-05
 

linux/payload/FreshSupvHandler.cpp

@@ -900,7 +900,7 @@ ReturnValue_t FreshSupvHandler::parseTmPackets() {
         }
         break;
       }
-      case(Apid::LATCHUP_MON): {
+      case (Apid::LATCHUP_MON): {
         if (tmReader.getServiceId() ==
             static_cast<uint8_t>(supv::tm::LatchupMonId::LATCHUP_STATUS_REPORT)) {
           handleLatchupStatusReport(receivedData);
@@ -1399,15 +1399,17 @@ ReturnValue_t FreshSupvHandler::verifyPacket(const uint8_t* start, size_t foundL
 
 ReturnValue_t FreshSupvHandler::handleBootStatusReport(const uint8_t* data) {
   ReturnValue_t result = returnvalue::OK;
-
   result = verifyPacket(data, tmReader.getFullPacketLen());
-
   if (result == result::CRC_FAILURE) {
     sif::error << "PlocSupervisorHandler::handleBootStatusReport: Boot status report has invalid"
                   " crc"
                << std::endl;
     return result;
   }
+  PoolReadGuard pg(&bootStatusReport);
+  if (pg.getReadResult() != returnvalue::OK) {
+    return pg.getReadResult();
+  }
 
   const uint8_t* payloadStart = tmReader.getPayloadStart();
   uint16_t offset = 0;
@@ -1471,13 +1473,17 @@ ReturnValue_t FreshSupvHandler::handleLatchupStatusReport(const uint8_t* data) {
   ReturnValue_t result = returnvalue::OK;
 
   result = verifyPacket(data, tmReader.getFullPacketLen());
-
   if (result == result::CRC_FAILURE) {
     sif::error << "PlocSupervisorHandler::handleLatchupStatusReport: Latchup status report has "
                << "invalid crc" << std::endl;
     return result;
   }
 
+  PoolReadGuard pg(&latchupStatusReport);
+  if (pg.getReadResult() != returnvalue::OK) {
+    return pg.getReadResult();
+  }
+
   const uint8_t* payloadData = tmReader.getPayloadStart();
   uint16_t offset = 0;
   latchupStatusReport.id = *(payloadData + offset);

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

@@ -23,8 +23,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,

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;
           }
         }
       }

mission/system/acs/StrFdir.cpp

@@ -1,6 +1,8 @@
 #include "StrFdir.h"
 
-#include "mission/acs/defs.h"
+#include <eive/objects.h>
+#include <fsfw/events/EventManagerIF.h>
+#include <mission/acs/defs.h>
 
 StrFdir::StrFdir(object_id_t strObject)
     : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
@@ -12,3 +14,13 @@ ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
   }
   return DeviceHandlerFailureIsolation::eventReceived(event);
 }
+
+ReturnValue_t StrFdir::initialize() {
+  ReturnValue_t result = DeviceHandlerFailureIsolation::initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
+  return manager->subscribeToEvent(eventQueue->getId(),
+                                   event::getEventId(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION));
+}

mission/system/acs/StrFdir.h

@@ -6,6 +6,7 @@
 class StrFdir : public DeviceHandlerFailureIsolation {
  public:
   StrFdir(object_id_t strObject);
+  ReturnValue_t initialize() override;
   ReturnValue_t eventReceived(EventMessage* event) override;
 };