diff --git a/.idea/cmake.xml b/.idea/cmake.xml
index 3be3a2b0..eff02c30 100644
--- a/.idea/cmake.xml
+++ b/.idea/cmake.xml
@@ -2,7 +2,7 @@
-
+
@@ -10,6 +10,7 @@
+
\ No newline at end of file
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 045b7121..de576ea6 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release:
# [unreleased]
+# [v6.2.0] 2023-07-26
+
+- `eive-tmtc`: v5.3.1
+
## Changed
- STR missed reply handling is now moved to DHB rather than the COM interface. The COM IF will
@@ -24,6 +28,15 @@ 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.
+- Simplify and fix the chip and copy protection functions in the core controller. This mechanism
+ now is always performed for the target chip and target copy in the reboot handlers.
+- Improvement in FSFW: HK generation is now countdown based.
## Added
@@ -41,6 +54,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.
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a7b5013c..6ffaf32f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,7 @@
cmake_minimum_required(VERSION 3.13)
set(OBSW_VERSION_MAJOR 6)
-set(OBSW_VERSION_MINOR 1)
+set(OBSW_VERSION_MINOR 2)
set(OBSW_VERSION_REVISION 0)
# set(CMAKE_VERBOSE TRUE)
diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
index 65b11c49..2899592d 100644
--- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp
+++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
/**
* @brief Auto-generated event translation file. Contains 301 translations.
* @details
- * Generated on: 2023-07-21 11:04:23
+ * Generated on: 2023-07-26 12:51:20
*/
#include "translateEvents.h"
diff --git a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
index ff7f24d8..a0002c24 100644
--- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
+++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
@@ -2,7 +2,7 @@
* @brief Auto-generated object translation file.
* @details
* Contains 171 translations.
- * Generated on: 2023-07-21 11:04:23
+ * Generated on: 2023-07-26 12:51:20
*/
#include "translateObjects.h"
diff --git a/bsp_q7s/boardtest/Q7STestTask.cpp b/bsp_q7s/boardtest/Q7STestTask.cpp
index 50a34284..03805fde 100644
--- a/bsp_q7s/boardtest/Q7STestTask.cpp
+++ b/bsp_q7s/boardtest/Q7STestTask.cpp
@@ -218,15 +218,30 @@ void Q7STestTask::testProtHandler() {
bool opPerformed = false;
ReturnValue_t result = returnvalue::OK;
// If any chips are unlocked, lock them here
- result = coreController->setBootCopyProtection(xsc::Chip::ALL_CHIP, xsc::Copy::ALL_COPY, true,
- opPerformed, true);
+ result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_0, xsc::Copy::COPY_0,
+ true);
+ if (result != returnvalue::OK) {
+ sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
+ }
+ result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_0, xsc::Copy::COPY_1,
+ true);
+ if (result != returnvalue::OK) {
+ sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
+ }
+ result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_0,
+ true);
+ if (result != returnvalue::OK) {
+ sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
+ }
+ result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_1,
+ true);
if (result != returnvalue::OK) {
sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
}
// unlock own copy
- result = coreController->setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, false,
- opPerformed, true);
+ result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::SELF_CHIP,
+ xsc::Copy::SELF_COPY, false);
if (result != returnvalue::OK) {
sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
}
@@ -239,8 +254,8 @@ void Q7STestTask::testProtHandler() {
}
// lock own copy
- result = coreController->setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true,
- opPerformed, 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;
}
@@ -253,8 +268,8 @@ void Q7STestTask::testProtHandler() {
}
// unlock specific copy
- result = coreController->setBootCopyProtection(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, false,
- opPerformed, true);
+ result = coreController->setBootCopyProtectionAndUpdateFile(xsc::Chip::CHIP_1, xsc::Copy::COPY_1,
+ false);
if (result != returnvalue::OK) {
sif::warning << "Q7STestTask::testProtHandler: Op failed" << std::endl;
}
@@ -267,8 +282,8 @@ void Q7STestTask::testProtHandler() {
}
// lock specific copy
- result = coreController->setBootCopyProtection(xsc::Chip::CHIP_1, xsc::Copy::COPY_1, true,
- opPerformed, 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;
}
diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp
index 6d8f60c5..e1b48fd2 100644
--- a/bsp_q7s/core/CoreController.cpp
+++ b/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;
+ if (sdInfo.cycleCount > maxCycleCount) {
+ sif::warning << "CoreController::sdStateMachine: " << opPrintout << " takes too long"
+ << std::endl;
+ sdInfo.cycleCount = 0;
+ return -1;
+ }
sdInfo.cycleCount = 0;
- return true;
- } else if (sdInfo.cycleCount > 4) {
- sif::warning << "CoreController::sdStateMachine: " << opPrintout << " takes too long"
- << std::endl;
- return false;
- }
- return false;
+ 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")) {
- sdInfo.otherState = sd::SdState::OFF;
- currentStateSetter(sdInfo.other, sd::SdState::OFF);
- } else {
- // Continue.. avoid being stuck here..
- sdFsmState = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE;
+ "Switching off other SD card") <= 0) {
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")) {
- sdInfo.otherState = sd::SdState::ON;
- currentStateSetter(sdInfo.other, sd::SdState::ON);
- } else {
- // Contnue.. avoid being stuck here.
- sdFsmState = SdStates::MOUNT_UNMOUNT_OTHER;
+ "Switching on other SD card") <= 0) {
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 =
- sdcMan->switchOffSdCard(sdInfo.other, sdInfo.otherState, &sdInfo.currentState);
+ ReturnValue_t result2 = sdcMan->switchOffSdCard(sdInfo.other, sdInfo.currentState, true);
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,25 @@ 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
- sdcMan->switchOffSdCard(sd::SdCard::SLOT_0);
- sdcMan->switchOffSdCard(sd::SdCard::SLOT_1);
- // If any boot copies are unprotected.
- // Actually this function only ensures that reboots to the own image are protected..
- ReturnValue_t result = setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true,
- protOpPerformed, false);
- if (result == returnvalue::OK and protOpPerformed) {
+ // Unmount and switch off SD cards. This could possibly fix issues with the SD card and is
+ // the more graceful way to reboot the system. This function takes around 400 ms.
+ ReturnValue_t result = handleSwitchingSdCardsOffNonBlocking();
+ if (result != returnvalue::OK) {
+ sif::error
+ << "CoreController::gracefulShutdownTasks: Issues unmounting or switching SD cards off"
+ << std::endl;
+ }
+
+ // 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;
}
+ sif::info << "Graceful shutdown handling done" << std::endl;
+ // Ensure that all diagnostic prinouts arrive.
+ TaskFactory::delayTask(50);
return result;
}
@@ -1274,144 +1275,50 @@ ReturnValue_t CoreController::generateChipStateFile() {
return returnvalue::OK;
}
-ReturnValue_t CoreController::setBootCopyProtection(xsc::Chip targetChip, xsc::Copy targetCopy,
- bool protect, bool &protOperationPerformed,
- bool updateProtFile) {
- bool allChips = false;
- bool allCopies = false;
- bool selfChip = false;
- bool selfCopy = false;
- protOperationPerformed = false;
-
- switch (targetChip) {
- case (xsc::Chip::ALL_CHIP): {
- allChips = true;
- break;
- }
- case (xsc::Chip::NO_CHIP): {
- return returnvalue::OK;
- }
- case (xsc::Chip::SELF_CHIP): {
- selfChip = true;
- targetChip = CURRENT_CHIP;
- break;
- }
- default: {
- break;
- }
- }
- switch (targetCopy) {
- case (xsc::Copy::ALL_COPY): {
- allCopies = true;
- break;
- }
- case (xsc::Copy::NO_COPY): {
- return returnvalue::OK;
- }
- case (xsc::Copy::SELF_COPY): {
- selfCopy = true;
- targetCopy = CURRENT_COPY;
- break;
- }
- default: {
- break;
- }
+ReturnValue_t CoreController::setBootCopyProtectionAndUpdateFile(xsc::Chip targetChip,
+ xsc::Copy targetCopy,
+ bool protect) {
+ if (targetChip == xsc::Chip::ALL_CHIP or targetCopy == xsc::Copy::ALL_COPY) {
+ return returnvalue::FAILED;
}
- for (uint8_t arrIdx = 0; arrIdx < protArray.size(); arrIdx++) {
- int result = handleBootCopyProtAtIndex(targetChip, targetCopy, protect, protOperationPerformed,
- selfChip, selfCopy, allChips, allCopies, arrIdx);
- if (result != 0) {
- break;
- }
- }
- if (protOperationPerformed and updateProtFile) {
+ bool protOperationPerformed = handleBootCopyProt(targetChip, targetCopy, protect);
+ if (protOperationPerformed) {
updateProtInfo();
}
return returnvalue::OK;
}
-int CoreController::handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy,
- bool protect, bool &protOperationPerformed,
- bool selfChip, bool selfCopy, bool allChips,
- bool allCopies, uint8_t arrIdx) {
- bool currentProt = protArray[arrIdx];
+bool CoreController::handleBootCopyProt(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect) {
std::ostringstream oss;
- bool performOp = false;
- if (protect == currentProt) {
- return 0;
- }
- if (protOperationPerformed) {
- if ((selfChip and selfCopy) or (not allCopies and not allChips)) {
- // No need to continue, only one operation was requested
- return 1;
- }
- }
- xsc::Chip currentChip;
- xsc::Copy currentCopy;
oss << "writeprotect ";
- if (arrIdx == 0 or arrIdx == 1) {
- oss << "0 ";
- currentChip = xsc::Chip::CHIP_0;
- } else {
- oss << "1 ";
- currentChip = xsc::Chip::CHIP_1;
+ if (targetChip == xsc::Chip::SELF_CHIP) {
+ targetChip = CURRENT_CHIP;
}
- if (arrIdx == 0 or arrIdx == 2) {
+ if (targetCopy == xsc::Copy::SELF_COPY) {
+ targetCopy = CURRENT_COPY;
+ }
+ if (targetChip == xsc::Chip::CHIP_0) {
oss << "0 ";
- currentCopy = xsc::Copy::COPY_0;
- } else {
+ } else if (targetChip == xsc::Chip::CHIP_1) {
+ oss << "1 ";
+ }
+ if (targetCopy == xsc::Copy::COPY_0) {
+ oss << "0 ";
+ } else if (targetCopy == xsc::Copy::COPY_1) {
oss << "1 ";
- currentCopy = xsc::Copy::COPY_1;
}
if (protect) {
oss << "1";
} else {
oss << "0";
}
-
- int result = 0;
- if (allChips and allCopies) {
- performOp = true;
- } else if (allChips) {
- if ((selfCopy and CURRENT_COPY == targetCopy) or (currentCopy == targetCopy)) {
- performOp = true;
- }
- } else if (allCopies) {
- if ((selfChip and CURRENT_COPY == targetCopy) or (currentChip == targetChip)) {
- performOp = true;
- }
- } else if (selfChip and (currentChip == targetChip)) {
- if (selfCopy) {
- if (currentCopy == targetCopy) {
- performOp = true;
- }
- } else {
- performOp = true;
- }
-
- } else if (selfCopy and (currentCopy == targetCopy)) {
- if (selfChip) {
- if (currentChip == targetChip) {
- performOp = true;
- }
- } else {
- performOp = true;
- }
- } else if ((targetChip == currentChip) and (targetCopy == currentCopy)) {
- performOp = true;
+ sif::info << "Executing command: " << oss.str() << std::endl;
+ int result = std::system(oss.str().c_str());
+ if (result == 0) {
+ return true;
}
- if (result != 0) {
- utility::handleSystemError(result, "CoreController::checkAndSetBootCopyProtection");
- }
- if (performOp) {
- // TODO: Lock operation take a long time. Use command executor? That would require a
- // new state machine..
- protOperationPerformed = true;
- sif::info << "Executing command: " << oss.str() << std::endl;
- result = std::system(oss.str().c_str());
- }
- return 0;
+ return false;
}
ReturnValue_t CoreController::updateProtInfo(bool regenerateChipStateFile) {
@@ -1456,7 +1363,6 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
using namespace std;
string word;
uint8_t wordIdx = 0;
- uint8_t arrayIdx = 0;
istringstream iss(nextLine);
xsc::Chip currentChip = xsc::Chip::CHIP_0;
xsc::Copy currentCopy = xsc::Copy::COPY_0;
@@ -1468,28 +1374,11 @@ ReturnValue_t CoreController::handleProtInfoUpdateLine(std::string nextLine) {
currentCopy = static_cast(stoi(word));
}
- if (wordIdx == 3) {
- if (currentChip == xsc::Chip::CHIP_0) {
- if (currentCopy == xsc::Copy::COPY_0) {
- arrayIdx = 0;
- } else if (currentCopy == xsc::Copy::COPY_1) {
- arrayIdx = 1;
- }
- }
-
- else if (currentChip == xsc::Chip::CHIP_1) {
- if (currentCopy == xsc::Copy::COPY_0) {
- arrayIdx = 2;
- } else if (currentCopy == xsc::Copy::COPY_1) {
- arrayIdx = 3;
- }
- }
- }
if (wordIdx == 5) {
if (word == "unlocked.") {
- protArray[arrayIdx] = false;
+ protArray[currentChip][currentCopy] = false;
} else {
- protArray[arrayIdx] = true;
+ protArray[currentChip][currentCopy] = true;
}
}
wordIdx++;
@@ -2583,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 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();
diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h
index b6382d36..79224ed2 100644
--- a/bsp_q7s/core/CoreController.h
+++ b/bsp_q7s/core/CoreController.h
@@ -199,8 +199,8 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
* @param updateProtFile Specify whether the protection info file is updated
* @return
*/
- ReturnValue_t setBootCopyProtection(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect,
- bool& protOperationPerformed, bool updateProtFile = true);
+ ReturnValue_t setBootCopyProtectionAndUpdateFile(xsc::Chip targetChip, xsc::Copy targetCopy,
+ bool protect);
bool sdInitFinished() const;
@@ -304,12 +304,10 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
Countdown sdCardCheckCd = Countdown(INIT_SD_CARD_CHECK_TIMEOUT);
/**
- * Index 0: Chip 0 Copy 0
- * Index 1: Chip 0 Copy 1
- * Index 2: Chip 1 Copy 0
- * Index 3: Chip 1 Copy 1
+ * First index: Chip.
+ * Second index: Copy.
*/
- std::array protArray;
+ bool protArray[2][2]{};
PeriodicOperationDivider opDivider5;
PeriodicOperationDivider opDivider10;
@@ -374,9 +372,8 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
ReturnValue_t gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, bool& protOpPerformed);
ReturnValue_t handleProtInfoUpdateLine(std::string nextLine);
- int handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect,
- bool& protOperationPerformed, bool selfChip, bool selfCopy,
- bool allChips, bool allCopies, uint8_t arrIdx);
+ 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);
void resetRebootWatchdogCounters(xsc::Chip tgtChip, xsc::Copy tgtCopy);
diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp
index 3a1f7ac5..14cafdb2 100644
--- a/bsp_q7s/em/emObjectFactory.cpp
+++ b/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(battAndImtqI2cDev);
+
#if OBSW_ADD_MGT == 1
createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev);
#endif
diff --git a/bsp_q7s/fs/SdCardManager.cpp b/bsp_q7s/fs/SdCardManager.cpp
index 14e3e6aa..ffbed668 100644
--- a/bsp_q7s/fs/SdCardManager.cpp
+++ b/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,
- SdStatePair* statusPair) {
- std::pair active;
- ReturnValue_t result = getSdCardsStatus(active);
- if (result != returnvalue::OK) {
- return result;
- }
+ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, SdStatePair& sdStates,
+ bool doUnmountSdCard) {
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) {
diff --git a/bsp_q7s/fs/SdCardManager.h b/bsp_q7s/fs/SdCardManager.h
index 7a4a7cbe..23a3d193 100644
--- a/bsp_q7s/fs/SdCardManager.h
+++ b/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,
- SdStatePair* statusPair = nullptr);
+ ReturnValue_t switchOffSdCard(sd::SdCard sdCard, SdStatePair& sdStates, bool doUnmountSdCard);
/**
* Get the state of the SD cards. If the state file does not exist, this function will
diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv
index 61e3a82a..0cb9ba70 100644
--- a/generators/bsp_hosted_events.csv
+++ b/generators/bsp_hosted_events.csv
@@ -273,7 +273,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h
14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h
14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;mission/sysDefs.h
-14014;0x36be;ACTIVE_SD_INFO;INFO;Active SD card info. 0: OFF, 1: ON, 2: MOUNTED. P1: SD Card 0, P2: SD Card 1.;mission/sysDefs.h
+14014;0x36be;ACTIVE_SD_INFO;INFO;Active SD card info. SD States: 0: OFF, 1: ON, 2: MOUNTED. P1: Active SD Card Index, 0 if none is active P2: First two bytes: SD state of SD card 0, last two bytes SD state of SD card 1;mission/sysDefs.h
14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h
14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h
14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h
diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv
index 61e3a82a..0cb9ba70 100644
--- a/generators/bsp_q7s_events.csv
+++ b/generators/bsp_q7s_events.csv
@@ -273,7 +273,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
14011;0x36bb;I2C_REBOOT;HIGH;I2C is unavailable. Recovery did not work, performing full reboot.;mission/sysDefs.h
14012;0x36bc;PDEC_REBOOT;HIGH;PDEC recovery through reset was not possible, performing full reboot.;mission/sysDefs.h
14013;0x36bd;FIRMWARE_INFO;INFO;Version information of the firmware (not OBSW). P1: Byte 0: Major, Byte 1: Minor, Byte 2: Patch, Byte 3: Has Git Hash P2: First four letters of Git SHA is the last byte of P1 is set.;mission/sysDefs.h
-14014;0x36be;ACTIVE_SD_INFO;INFO;Active SD card info. 0: OFF, 1: ON, 2: MOUNTED. P1: SD Card 0, P2: SD Card 1.;mission/sysDefs.h
+14014;0x36be;ACTIVE_SD_INFO;INFO;Active SD card info. SD States: 0: OFF, 1: ON, 2: MOUNTED. P1: Active SD Card Index, 0 if none is active P2: First two bytes: SD state of SD card 0, last two bytes SD state of SD card 1;mission/sysDefs.h
14100;0x3714;NO_VALID_SENSOR_TEMPERATURE;MEDIUM;No description;mission/controller/tcsDefs.h
14101;0x3715;NO_HEALTHY_HEATER_AVAILABLE;MEDIUM;No description;mission/controller/tcsDefs.h
14102;0x3716;SYRLINKS_OVERHEATING;HIGH;No description;mission/controller/tcsDefs.h
diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp
index 65b11c49..2899592d 100644
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@@ -1,7 +1,7 @@
/**
* @brief Auto-generated event translation file. Contains 301 translations.
* @details
- * Generated on: 2023-07-21 11:04:23
+ * Generated on: 2023-07-26 12:51:20
*/
#include "translateEvents.h"
diff --git a/generators/objects/translateObjects.cpp b/generators/objects/translateObjects.cpp
index 8bd6ae3d..fef8fb71 100644
--- a/generators/objects/translateObjects.cpp
+++ b/generators/objects/translateObjects.cpp
@@ -2,7 +2,7 @@
* @brief Auto-generated object translation file.
* @details
* Contains 175 translations.
- * Generated on: 2023-07-21 11:04:23
+ * Generated on: 2023-07-26 12:51:20
*/
#include "translateObjects.h"
diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp
index 65b11c49..2899592d 100644
--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
/**
* @brief Auto-generated event translation file. Contains 301 translations.
* @details
- * Generated on: 2023-07-21 11:04:23
+ * Generated on: 2023-07-26 12:51:20
*/
#include "translateEvents.h"
diff --git a/linux/fsfwconfig/objects/translateObjects.cpp b/linux/fsfwconfig/objects/translateObjects.cpp
index 8bd6ae3d..fef8fb71 100644
--- a/linux/fsfwconfig/objects/translateObjects.cpp
+++ b/linux/fsfwconfig/objects/translateObjects.cpp
@@ -2,7 +2,7 @@
* @brief Auto-generated object translation file.
* @details
* Contains 175 translations.
- * Generated on: 2023-07-21 11:04:23
+ * Generated on: 2023-07-26 12:51:20
*/
#include "translateObjects.h"
diff --git a/mission/acs/defs.h b/mission/acs/defs.h
index 41d09976..677ce37a 100644
--- a/mission/acs/defs.h
+++ b/mission/acs/defs.h
@@ -26,10 +26,18 @@ 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_WITHOUT_MEKF = 11,
- SAFECTRL_ECLIPSE_DAMPING = 12,
- SAFECTRL_ECLIPSE_IDELING = 13,
+ // OBSW version <= v6.1.0
+ LEGACY_SAFECTRL_ACTIVE_MEKF = 10,
+ LEGACY_SAFECTRL_WITHOUT_MEKF = 11,
+ LEGACY_SAFECTRL_ECLIPSE_DAMPING = 12,
+ LEGACY_SAFECTRL_ECLIPSE_IDELING = 13,
+ // Added in v6.2.0
+ SAFECTRL_MEKF = 14,
+ SAFECTRL_GYR = 15,
+ SAFECTRL_SUSMGM = 16,
+ SAFECTRL_ECLIPSE_DAMPING_GYR = 17,
+ SAFECTRL_ECLIPSE_DAMPING_SUSMGM = 18,
+ SAFECTRL_ECLIPSE_IDELING = 19,
SAFECTRL_DETUMBLE_FULL = 20,
SAFECTRL_DETUMBLE_DETERIORATED = 21,
};
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 04009cb2..b79fb714 100644
--- a/mission/controller/AcsController.cpp
+++ b/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):
- safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
- susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
+ case (acs::SafeModeStrategy::SAFECTRL_GYR):
+ 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):
- safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
- sunTargetDir, magMomMtq, errAng);
+ case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
+ safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value,
+ 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,12 +235,20 @@ void AcsController::performSafe() {
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
// detumble check and switch
- if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
- VectorOperations::norm(mekfData.satRotRateMekf.value, 3) >
- acsParameters.detumbleParameter.omegaDetumbleStart) {
- detumbleCounter++;
- } else if (gyrDataProcessed.gyrVecTot.isValid() &&
- VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) >
+ if (acsParameters.safeModeControllerParameters.useMekf) {
+ if (mekfData.satRotRateMekf.isValid() and
+ VectorOperations::norm(mekfData.satRotRateMekf.value, 3) >
+ acsParameters.detumbleParameter.omegaDetumbleStart) {
+ detumbleCounter++;
+ }
+ } else if (acsParameters.safeModeControllerParameters.useGyr) {
+ if (gyrDataProcessed.gyrVecTot.isValid() and
+ VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) >
+ acsParameters.detumbleParameter.omegaDetumbleStart) {
+ detumbleCounter++;
+ }
+ } else if (fusedRotRateData.rotRateTotal.isValid() and
+ VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (detumbleCounter > 0) {
@@ -289,17 +318,26 @@ void AcsController::performDetumble() {
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
- if (mekfData.satRotRateMekf.isValid() &&
- VectorOperations::norm(mekfData.satRotRateMekf.value, 3) <
- acsParameters.detumbleParameter.omegaDetumbleEnd) {
- detumbleCounter++;
- } else if (gyrDataProcessed.gyrVecTot.isValid() &&
- VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) <
- acsParameters.detumbleParameter.omegaDetumbleEnd) {
+ if (acsParameters.safeModeControllerParameters.useMekf) {
+ if (mekfData.satRotRateMekf.isValid() and
+ VectorOperations::norm(mekfData.satRotRateMekf.value, 3) <
+ acsParameters.detumbleParameter.omegaDetumbleStart) {
+ detumbleCounter++;
+ }
+ } else if (acsParameters.safeModeControllerParameters.useGyr) {
+ if (gyrDataProcessed.gyrVecTot.isValid() and
+ VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) <
+ acsParameters.detumbleParameter.omegaDetumbleStart) {
+ detumbleCounter++;
+ }
+ } else if (fusedRotRateData.rotRateTotal.isValid() and
+ VectorOperations::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;
}
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 0c8b94bb..c78c7e15 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -13,6 +13,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -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 rwTargetSpeed = PoolEntry(4);
PoolEntry mtqTargetDipole = PoolEntry(3);
+ // Fused Rot Rate
+ acsctrl::FusedRotRateData fusedRotRateData;
+ PoolEntry rotRateOrthogonal = PoolEntry(3);
+ PoolEntry rotRateParallel = PoolEntry(3);
+ PoolEntry rotRateTotal = PoolEntry(3);
+
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
};
diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index eea30389..9b0755cd 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/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;
}
diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index 9e13070f..cdaf4899 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/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_alignNonMekf = 4.0e-5;
- double k_parallelNonMekf = 3.75e-4;
+ double k_orthoGyr = 4.4e-3;
+ double k_alignGyr = 4.0e-5;
+ 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 {
diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt
index 3c4a3475..a8b0e9a6 100644
--- a/mission/controller/acs/CMakeLists.txt
+++ b/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
diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp
new file mode 100644
index 00000000..20f12d05
--- /dev/null
+++ b/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::cross(mgmDataProcessed->mgmVecTot.value,
+ susDataProcessed->susVecTot.value, magSunCross);
+ double magSunCrossNorm = VectorOperations::norm(magSunCross, 3);
+ double magNorm = VectorOperations::norm(mgmDataProcessed->mgmVecTot.value, 3);
+ double fusedRotRateParallel[3] = {0, 0, 0};
+ if (magSunCrossNorm >
+ (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
+ double omegaParallel =
+ VectorOperations::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) *
+ pow(magSunCrossNorm, -2);
+ VectorOperations::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::cross(susDataProcessed->susVecTotDerivative.value,
+ susDataProcessed->susVecTot.value, fusedRotRateOrthogonal);
+ VectorOperations::mulScalar(
+ fusedRotRateOrthogonal,
+ pow(VectorOperations::norm(susDataProcessed->susVecTot.value, 3), -2),
+ fusedRotRateOrthogonal, 3);
+
+ // calculate total rotation rate
+ double fusedRotRateTotal[3] = {0, 0, 0};
+ VectorOperations::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::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::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
+ double fusedRotRateTotal[3] = {0, 0, 0};
+ VectorOperations::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);
+ }
+}
diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h
new file mode 100644
index 00000000..fa4fda1c
--- /dev/null
+++ b/mission/controller/acs/FusedRotationEstimation.h
@@ -0,0 +1,29 @@
+#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
+#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
+
+#include
+#include
+#include
+#include
+
+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_ */
diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index 0279215f..511cae35 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/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::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::normalize(susMeanValue, susVecTot, 3);
+ if (VectorOperations::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::norm(susVecTotDerivative, 3) != 0 and
+ susDataProcessed->susVecTotDerivative.isValid()) {
+ lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
+ susParameters->susRateFilterWeight);
+ }
timeOfSavedSusDirEst = timeOfSusMeasurement;
{
PoolReadGuard pg(susDataProcessed);
diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index 43677ccf..8ad8b578 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -9,20 +9,36 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
SafeCtrl::~SafeCtrl() {}
-acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
- const bool satRotRateValid, const bool sunDirValid,
- const uint8_t mekfEnabled,
- const uint8_t dampingEnabled) {
+acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(
+ const bool magFieldValid, const bool mekfValid, const bool satRotRateValid,
+ const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid,
+ 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;
- } else if (satRotRateValid and sunDirValid) {
- return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF;
- } else if (dampingEnabled and satRotRateValid and not sunDirValid) {
- return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING;
- } else if (not dampingEnabled and satRotRateValid and not sunDirValid) {
- return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING;
+ return acs::SafeModeStrategy::SAFECTRL_MEKF;
+ } else if (sunDirValid) {
+ if (gyrEnabled and satRotRateValid) {
+ return acs::SafeModeStrategy::SAFECTRL_GYR;
+ } else if (not gyrEnabled and fusedRateSplitValid) {
+ 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;
}
@@ -43,8 +59,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB,
errorAngle = acos(dotSun);
splitRotationalRate(satRotRateB, sunDirB);
- calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle,
- acsParameters->safeModeControllerParameters.k_parallelMekf,
+ calculateRotationalRateTorque(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,
- const double *sunDirB, const double *sunDirRefB, double *magMomB,
- double &errorAngle) {
+void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
+ const double *sunDirRefB, double *magMomB, double &errorAngle) {
// convert magFieldB from uT to T
VectorOperations::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,
- acsParameters->safeModeControllerParameters.k_parallelNonMekf,
- acsParameters->safeModeControllerParameters.k_orthoNonMekf);
+ calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
+ acsParameters->safeModeControllerParameters.k_orthoGyr);
calculateAngleErrorTorque(sunDirB, sunDirRefB,
- acsParameters->safeModeControllerParameters.k_alignNonMekf);
+ acsParameters->safeModeControllerParameters.k_alignGyr);
// sum of all torques
for (uint8_t i = 0; i < 3; i++) {
@@ -82,8 +95,33 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB,
calculateMagneticMoment(magMomB);
}
-void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB,
- const double *sunDirRefB, double *magMomB, double &errorAngle) {
+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::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
+
+ // calculate error angle between sunDirRef and sunDir
+ double dotSun = VectorOperations::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::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,
- acsParameters->safeModeControllerParameters.k_parallelNonMekf,
- acsParameters->safeModeControllerParameters.k_orthoNonMekf);
+ calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
+ acsParameters->safeModeControllerParameters.k_orthoGyr);
+
+ // sum of all torques
+ VectorOperations::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::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::add(cmdParallel, cmdOrtho, cmdTorque, 3);
@@ -110,9 +167,7 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD
VectorOperations::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
}
-void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB,
- double &errorAngle, const double gainParallel,
- const double gainOrtho) {
+void SafeCtrl::calculateRotationalRateTorque(const double gainParallel, const double gainOrtho) {
// calculate torque for parallel rotational rate
VectorOperations::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);
diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h
index 12f9ddb0..55bcbf08 100644
--- a/mission/controller/acs/control/SafeCtrl.h
+++ b/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,
- const double *sunDirRefB, double *magMomB, double &errorAngle);
+ 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,
- double *magMomB, double &errorAngle);
+ 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,
- double &errorAngle, const double gainParallel,
- const double gainOrtho);
+ void calculateRotationalRates(const double *magFieldB, const double *magRateB,
+ const double *sunDirB, const double *sunRateB,
+ double *fusedRotRate);
+
+ void calculateRotationalRateTorque(const double gainParallel, const double gainOrtho);
void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB,
const double gainAlign);
diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
index c8dbd275..86866c3f 100644
--- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
+++ b/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 {
private:
};
+class FusedRotRateData : public StaticLocalDataSet {
+ public:
+ FusedRotRateData(HasLocalDataPoolIF* hkOwner)
+ : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {}
+
+ lp_vec_t rotRateOrthogonal =
+ lp_vec_t(sid.objectId, ROT_RATE_ORTHOGONAL, this);
+ lp_vec_t rotRateParallel = lp_vec_t(sid.objectId, ROT_RATE_PARALLEL, this);
+ lp_vec_t rotRateTotal = lp_vec_t(sid.objectId, ROT_RATE_TOTAL, this);
+
+ private:
+};
+
} // namespace acsctrl
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */