Merge pull request 'Different Rot Rate Source for Detumble' (#902) from use-different-rot-rate-for-detumble into main

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

CHANGELOG.md

@@ -20,6 +20,10 @@ will consitute of a breaking change warranting a new major release:
 
 - STR quaternions are now used by the `MEKF` by default
 - Changed nominal `SUS Assembly` side to `B Side`.
+- Changed source for state machine of detumbling to SUS and MGM only.
+- Changed `FusedRotRateData` dataset to always display rotation rate from SUS and MGM.
+- Solution from `QUEST` will be set to invalid if sun vector and magnetic field vector are too close
+  to each other.
 - Changed collection intervals of dataset collection
   - `GPS Controller`: `GPS Set` to 60s
   - `MTQ Handler`: `HK with Torque`, `HK without Torque` to 60s

mission/controller/AcsController.cpp

@@ -232,7 +232,8 @@ void AcsController::performSafe() {
   acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
-      fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
+      fusedRotRateSourcesData.rotRateTotalSusMgm.isValid(),
+      acsParameters.safeModeControllerParameters.useMekf,
       acsParameters.safeModeControllerParameters.useGyr,
       acsParameters.safeModeControllerParameters.dampingDuringEclipse);
   switch (safeCtrlStrat) {
@@ -251,9 +252,10 @@ void AcsController::performSafe() {
       safeCtrlFailureCounter = 0;
       break;
     case (acs::ControlModeStrategy::SAFECTRL_SUSMGM):
-      safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
+      safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value,
-                          fusedRotRateData.rotRateParallel.value,
+                          fusedRotRateSourcesData.rotRateTotalSusMgm.value,
-                          fusedRotRateData.rotRateOrthogonal.value,
+                          fusedRotRateSourcesData.rotRateParallelSusMgm.value,
+                          fusedRotRateSourcesData.rotRateOrthogonalSusMgm.value,
                           susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
@@ -267,8 +269,8 @@ void AcsController::performSafe() {
       break;
     case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
       safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
-                                     fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
+                                     fusedRotRateSourcesData.rotRateTotalSusMgm.value, sunTargetDir,
-                                     errAng);
+                                     magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
@@ -355,7 +357,7 @@ void AcsController::performPointingCtrl() {
   }
   acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
-      attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
+      attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotalSource.isValid(),
       fusedRotRateData.rotRateSource.value, useMekf);
 
   if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
@@ -387,11 +389,11 @@ void AcsController::performPointingCtrl() {
       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));
+      std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.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));
+      std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB));
       break;
     default:
       sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
@@ -555,8 +557,8 @@ void AcsController::performPointingCtrl() {
 void AcsController::handleDetumbling() {
   switch (detumbleState) {
     case DetumbleState::NO_DETUMBLE:
-      if (fusedRotRateData.rotRateTotal.isValid() and
+      if (fusedRotRateData.rotRateTotalSusMgm.isValid() and
-          VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
+          VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) >
               acsParameters.detumbleParameter.omegaDetumbleStart) {
         detumbleCounter++;
       } else if (detumbleCounter > 0) {
@@ -599,8 +601,8 @@ void AcsController::handleDetumbling() {
       detumbleState = DetumbleState::NO_DETUMBLE;
       break;
     case DetumbleState::IN_DETUMBLE:
-      if (fusedRotRateData.rotRateTotal.isValid() and
+      if (fusedRotRateData.rotRateTotalSusMgm.isValid() and
-          VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
+          VectorOperations<double>::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) <
               acsParameters.detumbleParameter.omegaDetumbleEnd) {
         detumbleCounter++;
       } else if (detumbleCounter > 0) {
@@ -829,9 +831,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
   poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 30.0});
   // Fused Rot Rate
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SUSMGM, &rotRateTotSusMgm);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SOURCE, &rotRateTotSource);
-  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource);
   poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 30.0});
   // Fused Rot Rate Sources

mission/controller/AcsController.h

@@ -271,9 +271,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   // Fused Rot Rate
   acsctrl::FusedRotRateData fusedRotRateData;
-  PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
+  PoolEntry<double> rotRateTotSusMgm = PoolEntry<double>(3);
-  PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
+  PoolEntry<double> rotRateTotSource = PoolEntry<double>(3);
-  PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
   PoolEntry<uint8_t> rotRateSource = PoolEntry<uint8_t>();
 
   // Fused Rot Rate Sources

mission/controller/acs/AcsParameters.cpp

@@ -38,6 +38,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x5:
           parameterWrapper->set(onBoardParams.questFilterWeight);
           break;
+        case 0x6:
+          parameterWrapper->set(onBoardParams.questAngleLimit);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -26,6 +26,7 @@ class AcsParameters : public HasParametersIF {
     uint8_t fusedRateFromStr = true;
     uint8_t fusedRateFromQuest = true;
     double questFilterWeight = 0.9;
+    double questAngleLimit = 5 * DEG2RAD;
   } onBoardParams;
 
   struct InertiaEIVE {

mission/controller/acs/AttitudeEstimation.cpp

@@ -29,6 +29,20 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
   VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMgmB, 3);
   VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMgmI, 3);
 
+  if ((std::acos(VectorOperations<double>::dot(normSusB, normMgmB)) <
+       acsParameters->onBoardParams.questAngleLimit) or
+      (std::acos(VectorOperations<double>::dot(normSusI, normMgmI)) <
+       acsParameters->onBoardParams.questAngleLimit)) {
+    {
+      PoolReadGuard pg{attitudeEstimationData};
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(attitudeEstimationData->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
+        attitudeEstimationData->quatQuest.setValid(false);
+      }
+    }
+    return;
+  }
+
   // Create Helper Vectors
   double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
          helperSum[3] = {0, 0, 0};

mission/controller/acs/FusedRotationEstimation.cpp

@@ -19,13 +19,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
                                            acsParameters->onBoardParams.fusedRateFromStr)) {
     PoolReadGuard pg(fusedRotRateData);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateTotalSource.value,
-      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->rotRateTotalSource.setValid(true);
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
       fusedRotRateData->rotRateSource.setValid(true);
     }
@@ -34,41 +30,38 @@ void FusedRotationEstimation::estimateFusedRotationRate(
               acsParameters->onBoardParams.fusedRateFromQuest)) {
     PoolReadGuard pg(fusedRotRateData);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateTotalSource.value,
-      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->rotRateTotalSource.setValid(true);
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
       fusedRotRateData->rotRateSource.setValid(true);
     }
   } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
-    std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
+    std::memcpy(fusedRotRateData->rotRateTotalSource.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->rotRateTotalSource.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->rotRateTotalSource.value, ZERO_VEC3, 3 * sizeof(double));
-      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->rotRateTotalSource.setValid(false);
-      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);
     }
   }
+  if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
+    std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value,
+                fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
+    fusedRotRateData->rotRateTotalSusMgm.setValid(true);
+  } else {
+    PoolReadGuard pg(fusedRotRateData);
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value, ZERO_VEC3, 3 * sizeof(double));
+      fusedRotRateData->rotRateTotalSusMgm.setValid(false);
+    }
+  }
 }
 
 void FusedRotationEstimation::estimateFusedRotationRateStr(

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -129,9 +129,8 @@ enum PoolIds : lp_id_t {
   RW_TARGET_SPEED,
   MTQ_TARGET_DIPOLE,
   // Fused Rotation Rate
-  ROT_RATE_ORTHOGONAL,
+  ROT_RATE_TOT_SUSMGM,
-  ROT_RATE_PARALLEL,
+  ROT_RATE_TOT_SOURCE,
-  ROT_RATE_TOTAL,
   ROT_RATE_SOURCE,
   // Fused Rotation Rate Sources
   ROT_RATE_ORTHOGONAL_SUSMGM,
@@ -151,7 +150,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
 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 = 4;
+static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
 static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5;
 
 /**
@@ -318,10 +317,10 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
   FusedRotRateData(HasLocalDataPoolIF* hkOwner)
       : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {}
 
-  lp_vec_t<double, 3> rotRateOrthogonal =
+  lp_vec_t<double, 3> rotRateTotalSusMgm =
-      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_ORTHOGONAL, this);
+      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOT_SUSMGM, this);
-  lp_vec_t<double, 3> rotRateParallel = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_PARALLEL, this);
+  lp_vec_t<double, 3> rotRateTotalSource =
-  lp_vec_t<double, 3> rotRateTotal = lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOTAL, this);
+      lp_vec_t<double, 3>(sid.objectId, ROT_RATE_TOT_SOURCE, this);
   lp_var_t<uint8_t> rotRateSource = lp_var_t<uint8_t>(sid.objectId, ROT_RATE_SOURCE, this);
 
  private: