Merge remote-tracking branch 'origin/main' into simplify-image-prot-code

CHANGELOG.md

@@ -24,6 +24,12 @@ will consitute of a breaking change warranting a new major release:
 - Re-ordered some functions of the core controller in the initialize function.
 - Rad sensor is now only polled every 30 minutes instead of every device cycle to reduce wear of
   the RADFET electronics.
+- The SD cards will still be switched OFF on a reboot, but this is done in a non-blocking manner
+  now with a timeout of 10 seconds where the reboot will be performed in any case.
+- ACS Controller now includes the safe mode from FLP, which will calculate its rotational rate
+  from SUS and MGM measurements. To accommodate these changes, low-pass filters for SUS
+  measurements and rates as well as MGM measurements and rates are included. Usage of the new
+  controller as well as settings of the low-pass filters can be handled via parameter commands.
 
 ## Added
 
@@ -41,6 +47,9 @@ will consitute of a breaking change warranting a new major release:
 
 ## Fixed
 
+- General bugs in the SD card state machine. This might fix some other known bugs for certain
+  combinations of switching ON and OFF SD cards and also makes the whole state machine a lot more
+  robust against hanging up.
 - SUS dummy handler went to `MODE_NORMAL` for ON commands.
 - PL PCDU dummy went to `MODE_NORMAL` for ON commands.
 

bsp_q7s/boardtest/Q7STestTask.cpp

@@ -218,25 +218,30 @@ void Q7STestTask::testProtHandler() {
   bool opPerformed = false;
   ReturnValue_t result = returnvalue::OK;
   // If any chips are unlocked, lock them here
-  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_0, xsc::Copy::COPY_0, false);
+  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_0, xsc::Copy::COPY_0,
+                                                              false);
   if (result != returnvalue::OK) {
     sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
   }
-  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_0, xsc::Copy::COPY_1, false);
+  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_0, xsc::Copy::COPY_1,
+                                                              false);
   if (result != returnvalue::OK) {
     sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
   }
-  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_0, false);
+  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_0,
+                                                              false);
   if (result != returnvalue::OK) {
     sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
   }
-  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, false);
+  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_1,
+                                                              false);
   if (result != returnvalue::OK) {
     sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
   }
 
   // unlock own copy
-  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, false);
+  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::SELF_CHIP,
+                                                              xsc::Copy::SELF_COPY, false);
   if (result != returnvalue::OK) {
     sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
   }
@@ -249,7 +254,8 @@ void Q7STestTask::testProtHandler() {
   }
 
   // lock own copy
-  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true);
+  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::SELF_CHIP,
+                                                              xsc::Copy::SELF_COPY, true);
   if (result != returnvalue::OK) {
     sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
   }
@@ -262,7 +268,8 @@ void Q7STestTask::testProtHandler() {
   }
 
   // unlock specific copy
-  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, false);
+  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_1,
+                                                              false);
   if (result != returnvalue::OK) {
     sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
   }
@@ -275,7 +282,8 @@ void Q7STestTask::testProtHandler() {
   }
 
   // lock specific copy
-  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, true);
+  result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_1,
+                                                              true);
   if (result != returnvalue::OK) {
     sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
   }

bsp_q7s/core/CoreController.cpp

@@ -554,21 +554,24 @@ ReturnValue_t CoreController::sdStateMachine() {
   }
 
   // This lambda checks the non-blocking operation of the SD card manager and assigns the new
-  // state on success. It returns true for an operation success and false otherwise
+  // state on success. It returns 0 for an operation success, -1 for failed operations, and 1
+  // for pending operations
   auto nonBlockingSdcOpChecking = [&](SdStates newStateOnSuccess, uint16_t maxCycleCount,
                                       std::string opPrintout) {
     SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation);
-    if (status == SdCardManager::OpStatus::SUCCESS) {
+    if (status == SdCardManager::OpStatus::SUCCESS or sdInfo.cycleCount > maxCycleCount) {
       sdFsmState = newStateOnSuccess;
       sdInfo.commandPending = false;
-      sdInfo.cycleCount = 0;
+      if (sdInfo.cycleCount > maxCycleCount) {
-      return true;
-    } else if (sdInfo.cycleCount > 4) {
         sif::warning << "CoreController::sdStateMachine: " << opPrintout << " takes too long"
                      << std::endl;
-      return false;
+        sdInfo.cycleCount = 0;
+        return -1;
       }
-    return false;
+      sdInfo.cycleCount = 0;
+      return 0;
+    };
+    return 1;
   };
 
   if (sdFsmState == SdStates::UPDATE_SD_INFO_START) {
@@ -644,7 +647,7 @@ ReturnValue_t CoreController::sdStateMachine() {
         sdFsmState = tgtState;
       }
     } else {
-      if (nonBlockingSdcOpChecking(SdStates::MOUNT_SELF, 10, "Setting SDC state")) {
+      if (nonBlockingSdcOpChecking(SdStates::MOUNT_SELF, 10, "Setting SDC state") <= 0) {
         sdInfo.activeState = sd::SdState::ON;
         currentStateSetter(sdInfo.active, sd::SdState::ON);
         // Skip the two cycles now.
@@ -672,7 +675,7 @@ ReturnValue_t CoreController::sdStateMachine() {
       result = sdCardSetup(sdInfo.active, sd::SdState::MOUNTED, sdInfo.activeChar);
       sdInfo.commandPending = true;
     } else {
-      if (nonBlockingSdcOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card")) {
+      if (nonBlockingSdcOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card") <= 0) {
         sdcMan->setActiveSdCard(sdInfo.active);
         currMntPrefix = sdcMan->getCurrentMountPrefix();
         sdInfo.activeState = sd::SdState::MOUNTED;
@@ -714,12 +717,7 @@ ReturnValue_t CoreController::sdStateMachine() {
         sdInfo.commandPending = true;
       } else {
         if (nonBlockingSdcOpChecking(SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE, 10,
-                                     "Switching off other SD card")) {
+                                     "Switching off other SD card") <= 0) {
-          sdInfo.otherState = sd::SdState::OFF;
-          currentStateSetter(sdInfo.other, sd::SdState::OFF);
-        } else {
-          // Continue.. avoid being stuck here..
-          sdFsmState = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
           sdInfo.otherState = sd::SdState::OFF;
           currentStateSetter(sdInfo.other, sd::SdState::OFF);
         }
@@ -730,12 +728,7 @@ ReturnValue_t CoreController::sdStateMachine() {
         sdInfo.commandPending = true;
       } else {
         if (nonBlockingSdcOpChecking(SdStates::MOUNT_UNMOUNT_OTHER, 10,
-                                     "Switching on other SD card")) {
+                                     "Switching on other SD card") <= 0) {
-          sdInfo.otherState = sd::SdState::ON;
-          currentStateSetter(sdInfo.other, sd::SdState::ON);
-        } else {
-          // Contnue.. avoid being stuck here.
-          sdFsmState = SdStates::MOUNT_UNMOUNT_OTHER;
           sdInfo.otherState = sd::SdState::ON;
           currentStateSetter(sdInfo.other, sd::SdState::ON);
         }
@@ -750,7 +743,8 @@ ReturnValue_t CoreController::sdStateMachine() {
         result = sdCardSetup(sdInfo.other, sd::SdState::ON, sdInfo.otherChar);
         sdInfo.commandPending = true;
       } else {
-        if (nonBlockingSdcOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card")) {
+        if (nonBlockingSdcOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card") <=
+            0) {
           sdInfo.otherState = sd::SdState::ON;
           currentStateSetter(sdInfo.other, sd::SdState::ON);
         } else {
@@ -764,7 +758,8 @@ ReturnValue_t CoreController::sdStateMachine() {
         result = sdCardSetup(sdInfo.other, sd::SdState::MOUNTED, sdInfo.otherChar);
         sdInfo.commandPending = true;
       } else {
-        if (nonBlockingSdcOpChecking(SdStates::UPDATE_SD_INFO_END, 4, "Mounting other SD card")) {
+        if (nonBlockingSdcOpChecking(SdStates::UPDATE_SD_INFO_END, 4, "Mounting other SD card") <=
+            0) {
           sdInfo.otherState = sd::SdState::MOUNTED;
           currentStateSetter(sdInfo.other, sd::SdState::MOUNTED);
         }
@@ -840,7 +835,7 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS
   if (state == sd::SdState::MOUNTED) {
     if (targetState == sd::SdState::OFF) {
       sif::info << "Switching off SD card " << sdChar << std::endl;
-      return sdcMan->switchOffSdCard(sdCard, true, &sdInfo.currentState);
+      return sdcMan->switchOffSdCard(sdCard, sdInfo.currentState, true);
     } else if (targetState == sd::SdState::ON) {
       sif::info << "Unmounting SD card " << sdChar << std::endl;
       return sdcMan->unmountSdCard(sdCard);
@@ -874,7 +869,7 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS
       return sdcMan->mountSdCard(sdCard);
     } else if (targetState == sd::SdState::OFF) {
       sif::info << "Switching off SD card " << sdChar << std::endl;
-      return sdcMan->switchOffSdCard(sdCard, false, &sdInfo.currentState);
+      return sdcMan->switchOffSdCard(sdCard, sdInfo.currentState, false);
     }
   } else {
     sif::warning << "CoreController::sdCardSetup: Invalid state for this call" << std::endl;
@@ -898,8 +893,7 @@ ReturnValue_t CoreController::sdColdRedundantBlockingInit() {
     sif::info << "Switching off secondary SD card " << sdInfo.otherChar << std::endl;
     // Switch off other SD card in cold redundant mode if setting up preferred one worked
     // without issues
-    ReturnValue_t result2 =
+    ReturnValue_t result2 = sdcMan->switchOffSdCard(sdInfo.other, sdInfo.currentState, true);
-        sdcMan->switchOffSdCard(sdInfo.other, sdInfo.otherState, &sdInfo.currentState);
     if (result2 != returnvalue::OK and result2 != SdCardManager::ALREADY_OFF) {
       sif::warning << "Switching off secondary SD card " << sdInfo.otherChar
                    << " in cold redundant mode failed" << std::endl;
@@ -1229,18 +1223,26 @@ ReturnValue_t CoreController::gracefulShutdownTasks(xsc::Chip chip, xsc::Copy co
   // Ensure that all writes/reads do finish.
   sync();
 
-  // Attempt graceful shutdown by unmounting and switching off SD cards
+  // Unmount and switch off SD cards. This could possibly fix issues with the SD card and is
-  sdcMan->switchOffSdCard(sd::SdCard::SLOT_0);
+  // the more graceful way to reboot the system. This function takes around 400 ms.
-  sdcMan->switchOffSdCard(sd::SdCard::SLOT_1);
+  ReturnValue_t result = handleSwitchingSdCardsOffNonBlocking();
+  if (result != returnvalue::OK) {
+    sif::error
+        << "CoreController::gracefulShutdownTasks: Issues unmounting or switching SD cards off"
+        << std::endl;
+  }
 
-  // Ensure tha the target chip is writeprotected in any case.
+  // Ensure that the target chip is writeprotected in any case.
   bool wasProtected = handleBootCopyProt(chip, copy, true);
   if (wasProtected) {
     // TODO: Would be nice to notify operator. But we can't use the filesystem anymore
     // and a reboot is imminent. Use scratch buffer?
     sif::info << "Running slot was writeprotected before reboot" << std::endl;
   }
-  return returnvalue::OK;
+  sif::info << "Graceful shutdown handling done" << std::endl;
+  // Ensure that all diagnostic prinouts arrive.
+  TaskFactory::delayTask(50);
+  return result;
 }
 
 void CoreController::updateInternalSdInfo() {
@@ -1273,9 +1275,10 @@ ReturnValue_t CoreController::generateChipStateFile() {
   return returnvalue::OK;
 }
 
-ReturnValue_t CoreController::setBootCopyProtectionAndUpdateFile(xsc::Chip targetChip, xsc::Copy targetCopy,
+ReturnValue_t CoreController::setBootCopyProtectionAndUpdateFile(xsc::Chip targetChip,
+                                                                 xsc::Copy targetCopy,
                                                                  bool protect) {
-  if(targetChip == xsc::Chip::ALL_CHIP or targetCopy == xsc::Copy::ALL_COPY) {
+  if (targetChip == xsc::Chip::ALL_CHIP or targetCopy == xsc::Copy::ALL_COPY) {
     return returnvalue::FAILED;
   }
 
@@ -1286,24 +1289,23 @@ ReturnValue_t CoreController::setBootCopyProtectionAndUpdateFile(xsc::Chip targe
   return returnvalue::OK;
 }
 
-bool CoreController::handleBootCopyProt(xsc::Chip targetChip, xsc::Copy targetCopy,
+bool CoreController::handleBootCopyProt(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect) {
-                                              bool protect) {
   std::ostringstream oss;
   oss << "writeprotect ";
-  if(targetChip == xsc::Chip::SELF_CHIP) {
+  if (targetChip == xsc::Chip::SELF_CHIP) {
     targetChip = CURRENT_CHIP;
   }
-  if(targetCopy == xsc::Copy::SELF_COPY) {
+  if (targetCopy == xsc::Copy::SELF_COPY) {
     targetCopy = CURRENT_COPY;
   }
   if (targetChip == xsc::Chip::CHIP_0) {
     oss << "0 ";
-  } else if(targetChip == xsc::Chip::SELF_CHIP) {
+  } else if (targetChip == xsc::Chip::SELF_CHIP) {
     oss << "1 ";
   }
-  if(targetCopy == xsc::Copy::COPY_0) {
+  if (targetCopy == xsc::Copy::COPY_0) {
     oss << "0 ";
-  } else if(targetCopy == xsc::Copy::COPY_1) {
+  } else if (targetCopy == xsc::Copy::COPY_1) {
     oss << "1 ";
   }
   if (protect) {
@@ -1313,7 +1315,7 @@ bool CoreController::handleBootCopyProt(xsc::Chip targetChip, xsc::Copy targetCo
   }
   sif::info << "Executing command: " << oss.str() << std::endl;
   int result = std::system(oss.str().c_str());
-  if(result == 0) {
+  if (result == 0) {
     return true;
   }
   return false;
@@ -2470,6 +2472,57 @@ void CoreController::announceSdInfo(SdCardManager::SdStatePair sdStates) {
   triggerEvent(core::ACTIVE_SD_INFO, p1, p2);
 }
 
+ReturnValue_t CoreController::handleSwitchingSdCardsOffNonBlocking() {
+  sdcMan->setBlocking(false);
+  SdCardManager::Operations op;
+  std::pair<sd::SdState, sd::SdState> sdStatus;
+  ReturnValue_t result = sdcMan->getSdCardsStatus(sdStatus);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  Countdown maxWaitTimeCd(10000);
+  // Stopwatch watch;
+  auto waitingForFinish = [&]() {
+    auto currentState = sdcMan->checkCurrentOp(op);
+    if (currentState == SdCardManager::OpStatus::IDLE) {
+      return returnvalue::OK;
+    }
+    while (currentState == SdCardManager::OpStatus::ONGOING) {
+      if (maxWaitTimeCd.hasTimedOut()) {
+        return returnvalue::FAILED;
+      }
+      TaskFactory::delayTask(50);
+      currentState = sdcMan->checkCurrentOp(op);
+    }
+    return returnvalue::OK;
+  };
+  if (sdStatus.first != sd::SdState::OFF) {
+    sdcMan->unmountSdCard(sd::SdCard::SLOT_0);
+    result = waitingForFinish();
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    sdcMan->switchOffSdCard(sd::SdCard::SLOT_0, sdStatus, false);
+    result = waitingForFinish();
+    if (result != returnvalue::OK) {
+      return result;
+    }
+  }
+  if (sdStatus.second != sd::SdState::OFF) {
+    sdcMan->unmountSdCard(sd::SdCard::SLOT_1);
+    result = waitingForFinish();
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    sdcMan->switchOffSdCard(sd::SdCard::SLOT_1, sdStatus, false);
+    result = waitingForFinish();
+    if (result != returnvalue::OK) {
+      return result;
+    }
+  }
+  return result;
+}
+
 bool CoreController::isNumber(const std::string &s) {
   return !s.empty() && std::find_if(s.begin(), s.end(),
                                     [](unsigned char c) { return !std::isdigit(c); }) == s.end();

bsp_q7s/core/CoreController.h

@@ -199,7 +199,8 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
    * @param updateProtFile Specify whether the protection info file is updated
    * @return
    */
-  ReturnValue_t setBootCopyProtectionAndUpdateFile(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect);
+  ReturnValue_t setBootCopyProtectionAndUpdateFile(xsc::Chip targetChip, xsc::Copy targetCopy,
+                                                   bool protect);
 
   bool sdInitFinished() const;
 
@@ -306,7 +307,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
    * First index: Chip.
    * Second index: Copy.
    */
-  bool protArray[2][2] {};
+  bool protArray[2][2]{};
   PeriodicOperationDivider opDivider5;
   PeriodicOperationDivider opDivider10;
 
@@ -371,6 +372,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
   ReturnValue_t gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, bool& protOpPerformed);
 
   ReturnValue_t handleProtInfoUpdateLine(std::string nextLine);
+  ReturnValue_t handleSwitchingSdCardsOffNonBlocking();
   bool handleBootCopyProt(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect);
   void rebootWatchdogAlgorithm(RebootWatchdogFile& rf, bool& needsReboot, xsc::Chip& tgtChip,
                                xsc::Copy& tgtCopy);

bsp_q7s/em/emObjectFactory.cpp

@@ -124,6 +124,8 @@ void ObjectFactory::produce(void* args) {
   if (core::FW_VERSION_MAJOR >= 4) {
     battAndImtqI2cDev = q7s::I2C_PS_EIVE;
   }
+  static_cast<void>(battAndImtqI2cDev);
+
 #if OBSW_ADD_MGT == 1
   createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev);
 #endif

bsp_q7s/fs/SdCardManager.cpp

@@ -125,13 +125,8 @@ ReturnValue_t SdCardManager::switchOnSdCard(sd::SdCard sdCard, bool doMountSdCar
   return mountSdCard(sdCard);
 }
 
-ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, bool doUnmountSdCard,
+ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, SdStatePair& sdStates,
-                                             SdStatePair* statusPair) {
+                                             bool doUnmountSdCard) {
-  std::pair<sd::SdState, sd::SdState> active;
-  ReturnValue_t result = getSdCardsStatus(active);
-  if (result != returnvalue::OK) {
-    return result;
-  }
   if (doUnmountSdCard) {
     if (not blocking) {
       sif::warning << "SdCardManager::switchOffSdCard: Two-step command but manager is"
@@ -147,17 +142,17 @@ ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, bool doUnmountSd
     return returnvalue::FAILED;
   }
   if (sdCard == sd::SdCard::SLOT_0) {
-    if (active.first == sd::SdState::OFF) {
+    if (sdStates.first == sd::SdState::OFF) {
       return ALREADY_OFF;
     }
   } else if (sdCard == sd::SdCard::SLOT_1) {
-    if (active.second == sd::SdState::OFF) {
+    if (sdStates.second == sd::SdState::OFF) {
       return ALREADY_OFF;
     }
   }
 
   if (doUnmountSdCard) {
-    result = unmountSdCard(sdCard);
+    ReturnValue_t result = unmountSdCard(sdCard);
     if (result != returnvalue::OK) {
       return result;
     }
@@ -189,7 +184,7 @@ ReturnValue_t SdCardManager::setSdCardState(sd::SdCard sdCard, bool on) {
   command << "q7hw sd set " << sdstring << " " << statestring;
   cmdExecutor.load(command.str(), blocking, printCmdOutput);
   ReturnValue_t result = cmdExecutor.execute();
-  if (blocking and result != returnvalue::OK) {
+  if (result != returnvalue::OK) {
     utility::handleSystemError(cmdExecutor.getLastError(), "SdCardManager::setSdCardState");
   }
   return result;
@@ -204,6 +199,7 @@ ReturnValue_t SdCardManager::getSdCardsStatus(SdStatePair& sdStates) {
 ReturnValue_t SdCardManager::mountSdCard(sd::SdCard sdCard) {
   using namespace std;
   if (cmdExecutor.getCurrentState() == CommandExecutor::States::PENDING) {
+    sif::warning << "SdCardManager::mountSdCard: Command still pending" << std::endl;
     return CommandExecutor::COMMAND_PENDING;
   }
   if (sdCard == sd::SdCard::BOTH) {

bsp_q7s/fs/SdCardManager.h

@@ -114,8 +114,7 @@ class SdCardManager : public SystemObject, public SdCardMountedIF {
    * @return - returnvalue::OK on success, ALREADY_ON if it is already on,
    *           SYSTEM_CALL_ERROR on system error
    */
-  ReturnValue_t switchOffSdCard(sd::SdCard sdCard, bool doUnmountSdCard = true,
+  ReturnValue_t switchOffSdCard(sd::SdCard sdCard, SdStatePair& sdStates, bool doUnmountSdCard);
-                                SdStatePair* statusPair = nullptr);
 
   /**
    * Get the state of the SD cards. If the state file does not exist, this function will

mission/acs/defs.h

@@ -26,10 +26,12 @@ enum SafeModeStrategy : uint8_t {
   SAFECTRL_OFF = 0,
   SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
   SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
-  SAFECTRL_ACTIVE_MEKF = 10,
+  SAFECTRL_MEKF = 10,
-  SAFECTRL_WITHOUT_MEKF = 11,
+  SAFECTRL_GYR = 11,
-  SAFECTRL_ECLIPSE_DAMPING = 12,
+  SAFECTRL_SUSMGM = 12,
-  SAFECTRL_ECLIPSE_IDELING = 13,
+  SAFECTRL_ECLIPSE_DAMPING_GYR = 13,
+  SAFECTRL_ECLIPSE_DAMPING_SUSMGM = 14,
+  SAFECTRL_ECLIPSE_IDELING = 15,
   SAFECTRL_DETUMBLE_FULL = 20,
   SAFECTRL_DETUMBLE_DETERIORATED = 21,
 };

mission/controller/AcsController.cpp

@@ -7,6 +7,7 @@
 AcsController::AcsController(object_id_t objectId, bool enableHkSets)
     : ExtendedControllerBase(objectId),
       enableHkSets(enableHkSets),
+      fusedRotationEstimation(&acsParameters),
       guidance(&acsParameters),
       safeCtrl(&acsParameters),
       ptgCtrl(&acsParameters),
@@ -20,7 +21,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
       gpsDataProcessed(this),
       mekfData(this),
       ctrlValData(this),
-      actuatorCmdData(this) {}
+      actuatorCmdData(this),
+      fusedRotRateData(this) {}
 
 ReturnValue_t AcsController::initialize() {
   ReturnValue_t result = parameterHelper.initialize();
@@ -146,6 +148,8 @@ void AcsController::performSafe() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
+  fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
+                                                        &gyrDataProcessed, &fusedRotRateData);
   ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
                                             &susDataProcessed, &mekfData, &acsParameters);
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
@@ -172,25 +176,42 @@ void AcsController::performSafe() {
   acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
       gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
+      fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(),
       acsParameters.safeModeControllerParameters.useMekf,
+      acsParameters.safeModeControllerParameters.useGyr,
       acsParameters.safeModeControllerParameters.dampingDuringEclipse);
   switch (safeCtrlStrat) {
-    case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF):
+    case (acs::SafeModeStrategy::SAFECTRL_MEKF):
       safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
                         susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
                         magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF):
+    case (acs::SafeModeStrategy::SAFECTRL_GYR):
-      safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
+      safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
                        susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
-    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING):
+    case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
-      safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
+      safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value,
-                               sunTargetDir, magMomMtq, errAng);
+                          fusedRotRateData.rotRateOrthogonal.value,
+                          susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
+      safeCtrlFailureFlag = false;
+      safeCtrlFailureCounter = 0;
+      break;
+    case (acs::SafeModeStrategy::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):
+      safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
+                                     fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
+                                     errAng);
       safeCtrlFailureFlag = false;
       safeCtrlFailureCounter = 0;
       break;
@@ -214,14 +235,22 @@ void AcsController::performSafe() {
                            acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
 
   // detumble check and switch
-  if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
+  if (acsParameters.safeModeControllerParameters.useMekf) {
+    if (mekfData.satRotRateMekf.isValid() and
         VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
             acsParameters.detumbleParameter.omegaDetumbleStart) {
       detumbleCounter++;
-  } else if (gyrDataProcessed.gyrVecTot.isValid() &&
+    }
+  } else if (acsParameters.safeModeControllerParameters.useGyr) {
+    if (gyrDataProcessed.gyrVecTot.isValid() and
         VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
             acsParameters.detumbleParameter.omegaDetumbleStart) {
       detumbleCounter++;
+    }
+  } else if (fusedRotRateData.rotRateTotal.isValid() and
+             VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
+                 acsParameters.detumbleParameter.omegaDetumbleStart) {
+    detumbleCounter++;
   } else if (detumbleCounter > 0) {
     detumbleCounter -= 1;
   }
@@ -289,17 +318,26 @@ void AcsController::performDetumble() {
   actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                            acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
 
-  if (mekfData.satRotRateMekf.isValid() &&
+  if (acsParameters.safeModeControllerParameters.useMekf) {
+    if (mekfData.satRotRateMekf.isValid() and
         VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
-          acsParameters.detumbleParameter.omegaDetumbleEnd) {
+            acsParameters.detumbleParameter.omegaDetumbleStart) {
       detumbleCounter++;
-  } else if (gyrDataProcessed.gyrVecTot.isValid() &&
+    }
+  } else if (acsParameters.safeModeControllerParameters.useGyr) {
+    if (gyrDataProcessed.gyrVecTot.isValid() and
         VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
-                 acsParameters.detumbleParameter.omegaDetumbleEnd) {
+            acsParameters.detumbleParameter.omegaDetumbleStart) {
+      detumbleCounter++;
+    }
+  } else if (fusedRotRateData.rotRateTotal.isValid() and
+             VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
+                 acsParameters.detumbleParameter.omegaDetumbleStart) {
     detumbleCounter++;
   } else if (detumbleCounter > 0) {
     detumbleCounter -= 1;
   }
+
   if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
     detumbleCounter = 0;
     // Triggers safe mode transition in subsystem
@@ -707,6 +745,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
   localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
   poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
+  // Fused Rot Rate
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
+  poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
   return returnvalue::OK;
 }
 
@@ -732,6 +775,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
       return &ctrlValData;
     case acsctrl::ACTUATOR_CMD_DATA:
       return &actuatorCmdData;
+    case acsctrl::FUSED_ROTATION_RATE_DATA:
+      return &fusedRotRateData;
     default:
       return nullptr;
   }

mission/controller/AcsController.h

@@ -13,6 +13,7 @@
 #include <mission/acs/rwHelpers.h>
 #include <mission/acs/susMax1227Helpers.h>
 #include <mission/controller/acs/ActuatorCmd.h>
+#include <mission/controller/acs/FusedRotationEstimation.h>
 #include <mission/controller/acs/Guidance.h>
 #include <mission/controller/acs/MultiplicativeKalmanFilter.h>
 #include <mission/controller/acs/Navigation.h>
@@ -49,6 +50,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   AcsParameters acsParameters;
   SensorProcessing sensorProcessing;
+  FusedRotationEstimation fusedRotationEstimation;
   Navigation navigation;
   ActuatorCmd actuatorCmd;
   Guidance guidance;
@@ -226,6 +228,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PoolEntry<int32_t> rwTargetSpeed = PoolEntry<int32_t>(4);
   PoolEntry<int16_t> mtqTargetDipole = PoolEntry<int16_t>(3);
 
+  // Fused Rot Rate
+  acsctrl::FusedRotRateData fusedRotRateData;
+  PoolEntry<double> rotRateOrthogonal = PoolEntry<double>(3);
+  PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
+  PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
+
   // Initial delay to make sure all pool variables have been initialized their owners
   Countdown initialCountdown = Countdown(INIT_DELAY);
 };

mission/controller/acs/AcsParameters.cpp

@@ -105,6 +105,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->setVector(mgmHandlingParameters.mgm4variance);
           break;
         case 0x12:
+          parameterWrapper->set(mgmHandlingParameters.mgmVectorFilterWeight);
+          break;
+        case 0x13:
           parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight);
           break;
         default:
@@ -224,6 +227,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x24:
           parameterWrapper->set(susHandlingParameters.susBrightnessThreshold);
           break;
+        case 0x25:
+          parameterWrapper->set(susHandlingParameters.susVectorFilterWeight);
+          break;
+        case 0x26:
+          parameterWrapper->set(susHandlingParameters.susRateFilterWeight);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -339,26 +348,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(safeModeControllerParameters.k_parallelMekf);
           break;
         case 0x3:
-          parameterWrapper->set(safeModeControllerParameters.k_orthoNonMekf);
+          parameterWrapper->set(safeModeControllerParameters.k_orthoGyr);
           break;
         case 0x4:
-          parameterWrapper->set(safeModeControllerParameters.k_alignNonMekf);
+          parameterWrapper->set(safeModeControllerParameters.k_alignGyr);
           break;
         case 0x5:
-          parameterWrapper->set(safeModeControllerParameters.k_parallelNonMekf);
+          parameterWrapper->set(safeModeControllerParameters.k_parallelGyr);
           break;
         case 0x6:
-          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
+          parameterWrapper->set(safeModeControllerParameters.k_orthoSusMgm);
           break;
         case 0x7:
-          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
+          parameterWrapper->set(safeModeControllerParameters.k_alignSusMgm);
           break;
         case 0x8:
-          parameterWrapper->set(safeModeControllerParameters.useMekf);
+          parameterWrapper->set(safeModeControllerParameters.k_parallelSusMgm);
           break;
         case 0x9:
+          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
+          break;
+        case 0xA:
+          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
+          break;
+        case 0xB:
+          parameterWrapper->set(safeModeControllerParameters.useMekf);
+          break;
+        case 0xC:
+          parameterWrapper->set(safeModeControllerParameters.useGyr);
+          break;
+        case 0xD:
           parameterWrapper->set(safeModeControllerParameters.dampingDuringEclipse);
           break;
+        case 0xE:
+          parameterWrapper->set(safeModeControllerParameters.sineLimitSunRotRate);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -77,7 +77,8 @@ class AcsParameters : public HasParametersIF {
     float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
     float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
     float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
-    float mgmDerivativeFilterWeight = 0.5;
+    float mgmVectorFilterWeight = 0.85;
+    float mgmDerivativeFilterWeight = 0.85;
   } mgmHandlingParameters;
 
   struct SusHandlingParameters {
@@ -767,6 +768,8 @@ class AcsParameters : public HasParametersIF {
          0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
          -0.000889232196185857, -0.00168429567131815}};
     float susBrightnessThreshold = 0.7;
+    float susVectorFilterWeight = .85;
+    float susRateFilterWeight = .85;
   } susHandlingParameters;
 
   struct GyrHandlingParameters {
@@ -825,15 +828,22 @@ class AcsParameters : public HasParametersIF {
     double k_alignMekf = 4.0e-5;
     double k_parallelMekf = 3.75e-4;
 
-    double k_orthoNonMekf = 4.4e-3;
+    double k_orthoGyr = 4.4e-3;
-    double k_alignNonMekf = 4.0e-5;
+    double k_alignGyr = 4.0e-5;
-    double k_parallelNonMekf = 3.75e-4;
+    double k_parallelGyr = 3.75e-4;
+
+    double k_orthoSusMgm = 1.1e-2;
+    double k_alignSusMgm = 2.0e-5;
+    double k_parallelSusMgm = 4.4e-4;
 
     double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
     double sunTargetDir[3] = {0, 0, 1};
 
     uint8_t useMekf = false;
+    uint8_t useGyr = true;
     uint8_t dampingDuringEclipse = true;
+
+    float sineLimitSunRotRate = 0.24;
   } safeModeControllerParameters;
 
   struct PointingLawParameters {

mission/controller/acs/CMakeLists.txt

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

mission/controller/acs/FusedRotationEstimation.cpp

@@ -0,0 +1,103 @@
+#include "FusedRotationEstimation.h"
+
+FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) {
+  acsParameters = acsParameters_;
+}
+
+void FusedRotationEstimation::estimateFusedRotationRateSafe(
+    acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
+    acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
+  if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
+       not fusedRotRateData->rotRateTotal.isValid()) or
+      (not susDataProcessed->susVecTotDerivative.isValid() and
+       not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
+    {
+      PoolReadGuard pg(fusedRotRateData);
+      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
+      fusedRotRateData->setValidity(false, true);
+    }
+    return;
+  }
+  if (not susDataProcessed->susVecTot.isValid()) {
+    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
+    return;
+  }
+
+  // calculate rotation around the sun
+  double magSunCross[3] = {0, 0, 0};
+
+  VectorOperations<double>::cross(mgmDataProcessed->mgmVecTot.value,
+                                  susDataProcessed->susVecTot.value, magSunCross);
+  double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3);
+  double magNorm = VectorOperations<double>::norm(mgmDataProcessed->mgmVecTot.value, 3);
+  double fusedRotRateParallel[3] = {0, 0, 0};
+  if (magSunCrossNorm >
+      (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
+    double omegaParallel =
+        VectorOperations<double>::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) *
+        pow(magSunCrossNorm, -2);
+    VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
+                                        fusedRotRateParallel, 3);
+  } else {
+    estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData);
+    return;
+  }
+
+  // calculate rotation orthogonal to the sun
+  double fusedRotRateOrthogonal[3] = {0, 0, 0};
+  VectorOperations<double>::cross(susDataProcessed->susVecTotDerivative.value,
+                                  susDataProcessed->susVecTot.value, fusedRotRateOrthogonal);
+  VectorOperations<double>::mulScalar(
+      fusedRotRateOrthogonal,
+      pow(VectorOperations<double>::norm(susDataProcessed->susVecTot.value, 3), -2),
+      fusedRotRateOrthogonal, 3);
+
+  // calculate total rotation rate
+  double fusedRotRateTotal[3] = {0, 0, 0};
+  VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
+
+  // store for calculation of angular acceleration
+  if (gyrDataProcessed->gyrVecTot.isValid()) {
+    std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
+  }
+
+  {
+    PoolReadGuard pg(fusedRotRateData);
+    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal,
+                3 * sizeof(double));
+    std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double));
+    std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
+    fusedRotRateData->setValidity(true, true);
+  }
+}
+
+void FusedRotationEstimation::estimateFusedRotationRateEclipse(
+    acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
+  if (not gyrDataProcessed->gyrVecTot.isValid() or
+      VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
+    {
+      PoolReadGuard pg(fusedRotRateData);
+      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
+      fusedRotRateData->setValidity(false, true);
+    }
+    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,
+                                3);
+  {
+    PoolReadGuard pg(fusedRotRateData);
+    std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
+    fusedRotRateData->rotRateOrthogonal.setValid(false);
+    std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
+    fusedRotRateData->rotRateParallel.setValid(false);
+    std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
+    fusedRotRateData->rotRateTotal.setValid(true);
+  }
+}

mission/controller/acs/FusedRotationEstimation.h

@@ -0,0 +1,29 @@
+#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
+#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
+
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
+
+class FusedRotationEstimation {
+ public:
+  FusedRotationEstimation(AcsParameters *acsParameters_);
+
+  void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed,
+                                     acsctrl::MgmDataProcessed *mgmDataProcessed,
+                                     acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                     acsctrl::FusedRotRateData *fusedRotRateData);
+
+ protected:
+ private:
+  static constexpr double ZERO_VEC[3] = {0, 0, 0};
+
+  AcsParameters *acsParameters;
+  double rotRateOldB[3] = {0, 0, 0};
+
+  void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                        acsctrl::FusedRotRateData *fusedRotRateData);
+};
+
+#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */

mission/controller/acs/SensorProcessing.cpp

@@ -132,6 +132,10 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   for (uint8_t i = 0; i < 3; i++) {
     mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i];
   }
+  if (VectorOperations<double>::norm(mgmVecTot, 3) != 0 and mgmDataProcessed->mgmVecTot.isValid()) {
+    lowPassFilter(mgmVecTot, mgmDataProcessed->mgmVecTot.value,
+                  mgmParameters->mgmVectorFilterWeight);
+  }
 
   //-----------------------Mgm Rate Computation ---------------------------------------------------
   double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
@@ -351,6 +355,11 @@ void SensorProcessing::processSus(
   double susVecTot[3] = {0.0, 0.0, 0.0};
   VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
 
+  if (VectorOperations<double>::norm(susVecTot, 3) != 0 and susDataProcessed->susVecTot.isValid()) {
+    lowPassFilter(susVecTot, susDataProcessed->susVecTot.value,
+                  susParameters->susVectorFilterWeight);
+  }
+
   /* -------- Sun Derivatiative --------------------- */
 
   double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
@@ -363,6 +372,11 @@ void SensorProcessing::processSus(
       susVecTotDerivativeValid = true;
     }
   }
+  if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
+      susDataProcessed->susVecTotDerivative.isValid()) {
+    lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
+                  susParameters->susRateFilterWeight);
+  }
   timeOfSavedSusDirEst = timeOfSusMeasurement;
   {
     PoolReadGuard pg(susDataProcessed);

mission/controller/acs/control/SafeCtrl.cpp

@@ -9,23 +9,39 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
 
 SafeCtrl::~SafeCtrl() {}
 
-acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
+acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
-                                                 const bool satRotRateValid, const bool sunDirValid,
+    const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
-                                                 const uint8_t mekfEnabled,
+    const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid,
-                                                 const uint8_t dampingEnabled) {
+    const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) {
   if (not magFieldValid) {
     return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
   } else if (mekfEnabled and mekfValid) {
-    return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF;
+    return acs::SafeModeStrategy::SAFECTRL_MEKF;
-  } else if (satRotRateValid and sunDirValid) {
+  } else if (sunDirValid) {
-    return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF;
+    if (gyrEnabled and satRotRateValid) {
-  } else if (dampingEnabled and satRotRateValid and not sunDirValid) {
+      return acs::SafeModeStrategy::SAFECTRL_GYR;
-    return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING;
+    } else if (not gyrEnabled and fusedRateSplitValid) {
-  } else if (not dampingEnabled and satRotRateValid and not sunDirValid) {
+      return acs::SafeModeStrategy::SAFECTRL_SUSMGM;
+    } else {
+      return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+    }
+  } else if (not sunDirValid) {
+    if (dampingEnabled) {
+      if (gyrEnabled and satRotRateValid) {
+        return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR;
+      } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) {
+        return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM;
+      } else {
+        return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+      }
+    } else if (not dampingEnabled and satRotRateValid) {
       return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
     } else {
       return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
     }
+  } else {
+    return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL;
+  }
 }
 
 void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
@@ -43,8 +59,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
   errorAngle = acos(dotSun);
 
   splitRotationalRate(satRotRateB, sunDirB);
-  calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
+  calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelMekf,
-                                acsParameters->safeModeControllerParameters.k_parallelMekf,
                                 acsParameters->safeModeControllerParameters.k_orthoMekf);
   calculateAngleErrorTorque(sunDirB, sunDirRefB,
                             acsParameters->safeModeControllerParameters.k_alignMekf);
@@ -57,9 +72,8 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
   calculateMagneticMoment(magMomB);
 }
 
-void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
+void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
-                           const double *sunDirB, const double *sunDirRefB, double *magMomB,
+                       const double *sunDirRefB, double *magMomB, double &errorAngle) {
-                           double &errorAngle) {
   // convert magFieldB from uT to T
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
@@ -68,11 +82,10 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
   errorAngle = acos(dotSun);
 
   splitRotationalRate(satRotRateB, sunDirB);
-  calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
+  calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
-                                acsParameters->safeModeControllerParameters.k_parallelNonMekf,
+                                acsParameters->safeModeControllerParameters.k_orthoGyr);
-                                acsParameters->safeModeControllerParameters.k_orthoNonMekf);
   calculateAngleErrorTorque(sunDirB, sunDirRefB,
-                            acsParameters->safeModeControllerParameters.k_alignNonMekf);
+                            acsParameters->safeModeControllerParameters.k_alignGyr);
 
   // sum of all torques
   for (uint8_t i = 0; i < 3; i++) {
@@ -82,7 +95,32 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
   calculateMagneticMoment(magMomB);
 }
 
-void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
+void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
+                          const double *rotRateOrthogonalB, const double *sunDirB,
+                          const double *sunDirRefB, double *magMomB, double &errorAngle) {
+  // convert magFieldB from uT to T
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
+
+  // calculate error angle between sunDirRef and sunDir
+  double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
+  errorAngle = acos(dotSun);
+
+  std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB));
+  std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB));
+  calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
+                                acsParameters->safeModeControllerParameters.k_orthoSusMgm);
+  calculateAngleErrorTorque(sunDirB, sunDirRefB,
+                            acsParameters->safeModeControllerParameters.k_alignSusMgm);
+
+  // sum of all torques
+  for (uint8_t i = 0; i < 3; i++) {
+    cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
+  }
+
+  calculateMagneticMoment(magMomB);
+}
+
+void SafeCtrl::safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
                                   const double *sunDirRefB, double *magMomB, double &errorAngle) {
   // convert magFieldB from uT to T
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
@@ -91,9 +129,28 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate
   errorAngle = NAN;
 
   splitRotationalRate(satRotRateB, sunDirRefB);
-  calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle,
+  calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
-                                acsParameters->safeModeControllerParameters.k_parallelNonMekf,
+                                acsParameters->safeModeControllerParameters.k_orthoGyr);
-                                acsParameters->safeModeControllerParameters.k_orthoNonMekf);
+
+  // sum of all torques
+  VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
+
+  // calculate magnetic moment to command
+  calculateMagneticMoment(magMomB);
+}
+
+void SafeCtrl::safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB,
+                                     const double *sunDirRefB, double *magMomB,
+                                     double &errorAngle) {
+  // convert magFieldB from uT to T
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
+
+  // no error angle available for eclipse
+  errorAngle = NAN;
+
+  splitRotationalRate(satRotRateB, sunDirRefB);
+  calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm,
+                                acsParameters->safeModeControllerParameters.k_orthoSusMgm);
 
   // sum of all torques
   VectorOperations<double>::add(cmdParallel, cmdOrtho, cmdTorque, 3);
@@ -110,9 +167,7 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
   VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
 }
 
-void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
+void SafeCtrl::calculateRotationalRateTorque(const double gainParallel, const double gainOrtho) {
-                                             double &errorAngle, const double gainParallel,
-                                             const double gainOrtho) {
   // calculate torque for parallel rotational rate
   VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);
 

mission/controller/acs/control/SafeCtrl.h

@@ -14,23 +14,34 @@ class SafeCtrl {
 
   acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
                                          const bool satRotRateValid, const bool sunDirValid,
-                                         const uint8_t mekfEnabled, const uint8_t dampingEnabled);
+                                         const bool fusedRateSplitValid,
+                                         const bool fusedRateTotalValid, const uint8_t mekfEnabled,
+                                         const uint8_t gyrEnabled, const uint8_t dampingEnabled);
 
   void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
                 const double *quatBI, const double *sunDirRefB, double *magMomB,
                 double &errorAngle);
 
-  void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
+  void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
                const double *sunDirRefB, double *magMomB, double &errorAngle);
 
-  void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB,
+  void safeSusMgm(const double *magFieldB, const double *rotRateParallelB,
+                  const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB,
                   double *magMomB, double &errorAngle);
 
+  void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB,
+                          const double *sunDirRefB, double *magMomB, double &errorAngle);
+
+  void safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB,
+                             const double *sunDirRefB, double *magMomB, double &errorAngle);
+
   void splitRotationalRate(const double *satRotRateB, const double *sunDirB);
 
-  void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
+  void calculateRotationalRates(const double *magFieldB, const double *magRateB,
-                                     double &errorAngle, const double gainParallel,
+                                const double *sunDirB, const double *sunRateB,
-                                     const double gainOrtho);
+                                double *fusedRotRate);
+
+  void calculateRotationalRateTorque(const double gainParallel, const double gainOrtho);
 
   void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
                                  const double gainAlign);

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -18,7 +18,8 @@ enum SetIds : uint32_t {
   GPS_PROCESSED_DATA,
   MEKF_DATA,
   CTRL_VAL_DATA,
-  ACTUATOR_CMD_DATA
+  ACTUATOR_CMD_DATA,
+  FUSED_ROTATION_RATE_DATA,
 };
 
 enum PoolIds : lp_id_t {
@@ -103,6 +104,10 @@ enum PoolIds : lp_id_t {
   RW_TARGET_TORQUE,
   RW_TARGET_SPEED,
   MTQ_TARGET_DIPOLE,
+  // Fused Rotation Rate
+  ROT_RATE_ORTHOGONAL,
+  ROT_RATE_PARALLEL,
+  ROT_RATE_TOTAL,
 };
 
 static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@@ -115,6 +120,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
 static constexpr uint8_t MEKF_SET_ENTRIES = 3;
 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;
 
 /**
  * @brief   Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@@ -273,6 +279,19 @@ class ActuatorCmdData : public StaticLocalDataSet<ACT_CMD_SET_ENTRIES> {
  private:
 };
 
+class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
+ public:
+  FusedRotRateData(HasLocalDataPoolIF* hkOwner)
+      : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {}
+
+  lp_vec_t<double, 3> rotRateOrthogonal =
+      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);
+
+ private:
+};
+
 }  // namespace acsctrl
 
 #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */

tmtc

@@ -1 +1 @@
-Subproject commit 95b69541751c9ea7146e0a342122da83bdb50f29
+Subproject commit 15716c988b6d26ae7f00e44b919d5ae7505d81ad