eive/eive-obsw
eive/eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests Releases 118 Wiki Activity

Merge remote-tracking branch 'origin/main' into small-scex-fix
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller 2023-07-27 10:11:12 +02:00
commit 5615f51ae7
29 changed files with 584 additions and 291 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.idea/cmake.xml
View File

@ -2,7 +2,7 @@
<project version="4">
<component name="CMakeSharedSettings">
<configurations>
<configuration PROFILE_NAME="Debug Q7S" ENABLED="true" CONFIG_NAME="Debug" TOOLCHAIN_NAME="Q7S" GENERATION_OPTIONS="-DTGT_BSP=&quot;arm/q7s&quot;" NO_GENERATOR="true">
<configuration PROFILE_NAME="Debug Q7S" ENABLED="true" CONFIG_NAME="Debug" TOOLCHAIN_NAME="Default" GENERATION_OPTIONS="-DTGT_BSP=&quot;arm/q7s&quot;" NO_GENERATOR="true">
<ADDITIONAL_GENERATION_ENVIRONMENT>
<envs>
<env name="ZYNQ_7020_ROOTFS" value="/opt/xiphos/sdk/ark/sysroots/cortexa9hf-neon-xiphos-linux-gnueabi" />
@ -10,6 +10,7 @@
</envs>
</ADDITIONAL_GENERATION_ENVIRONMENT>
</configuration>
<configuration PROFILE_NAME="Debug" ENABLED="true" CONFIG_NAME="Debug" NO_GENERATOR="true" />
</configurations>
</component>
</project>

16
CHANGELOG.md
View File

@ -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.

2
CMakeLists.txt
View File

@ -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)

2
bsp_hosted/fsfwconfig/events/translateEvents.cpp
View File

@ -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"

2
bsp_hosted/fsfwconfig/objects/translateObjects.cpp
View File

@ -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"

35
bsp_q7s/boardtest/Q7STestTask.cpp
View File

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

292
bsp_q7s/core/CoreController.cpp
View File

@ -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<xsc::Copy>(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<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();

17
bsp_q7s/core/CoreController.h
View File

@ -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<bool, 4> 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);

2
bsp_q7s/em/emObjectFactory.cpp
View File

@ -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

18
bsp_q7s/fs/SdCardManager.cpp
View File

@ -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<sd::SdState, sd::SdState> 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) {

3
bsp_q7s/fs/SdCardManager.h
View File

@ -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

2
generators/bsp_hosted_events.csv
View File

@ -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

1 Event ID (dec) Event ID (hex) Name Severity Description File Path
273 14011 0x36bb I2C_REBOOT HIGH I2C is unavailable. Recovery did not work, performing full reboot. mission/sysDefs.h
274 14012 0x36bc PDEC_REBOOT HIGH PDEC recovery through reset was not possible, performing full reboot. mission/sysDefs.h
275 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
276 14014 0x36be ACTIVE_SD_INFO INFO Active SD card info. 0: OFF, 1: ON, 2: MOUNTED. P1: SD Card 0, P2: SD Card 1. 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
277 14100 0x3714 NO_VALID_SENSOR_TEMPERATURE MEDIUM No description mission/controller/tcsDefs.h
278 14101 0x3715 NO_HEALTHY_HEATER_AVAILABLE MEDIUM No description mission/controller/tcsDefs.h
279 14102 0x3716 SYRLINKS_OVERHEATING HIGH No description mission/controller/tcsDefs.h

2
generators/bsp_q7s_events.csv
View File

@ -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

1 Event ID (dec) Event ID (hex) Name Severity Description File Path
273 14011 0x36bb I2C_REBOOT HIGH I2C is unavailable. Recovery did not work, performing full reboot. mission/sysDefs.h
274 14012 0x36bc PDEC_REBOOT HIGH PDEC recovery through reset was not possible, performing full reboot. mission/sysDefs.h
275 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
276 14014 0x36be ACTIVE_SD_INFO INFO Active SD card info. 0: OFF, 1: ON, 2: MOUNTED. P1: SD Card 0, P2: SD Card 1. 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
277 14100 0x3714 NO_VALID_SENSOR_TEMPERATURE MEDIUM No description mission/controller/tcsDefs.h
278 14101 0x3715 NO_HEALTHY_HEATER_AVAILABLE MEDIUM No description mission/controller/tcsDefs.h
279 14102 0x3716 SYRLINKS_OVERHEATING HIGH No description mission/controller/tcsDefs.h

2
generators/events/translateEvents.cpp
View File

@ -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"

2
generators/objects/translateObjects.cpp
View File

@ -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"

2
linux/fsfwconfig/events/translateEvents.cpp
View File

@ -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"

2
linux/fsfwconfig/objects/translateObjects.cpp
View File

@ -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"

16
mission/acs/defs.h
View File

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

87
mission/controller/AcsController.cpp
View File

@ -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<double>::norm(mekfData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} 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) {
@ -289,17 +318,26 @@ void AcsController::performDetumble() {
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
if (mekfData.satRotRateMekf.isValid() &&
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
detumbleCounter++;
} else if (gyrDataProcessed.gyrVecTot.isValid() &&
VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleEnd) {
if (acsParameters.safeModeControllerParameters.useMekf) {
if (mekfData.satRotRateMekf.isValid() and
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
acsParameters.detumbleParameter.omegaDetumbleStart) {
detumbleCounter++;
}
} 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;
}
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;
}

8
mission/controller/AcsController.h
View File

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

36
mission/controller/acs/AcsParameters.cpp
View File

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

18
mission/controller/acs/AcsParameters.h
View File

@ -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 {

1
mission/controller/acs/CMakeLists.txt
View File

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

103
mission/controller/acs/FusedRotationEstimation.cpp Normal file
View File

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

29
mission/controller/acs/FusedRotationEstimation.h Normal file
View File

@ -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_ */

14
mission/controller/acs/SensorProcessing.cpp
View File

@ -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);

111
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -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<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,
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<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,
acsParameters->safeModeControllerParameters.k_parallelNonMekf,
acsParameters->safeModeControllerParameters.k_orthoNonMekf);
calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr,
acsParameters->safeModeControllerParameters.k_orthoGyr);
// 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,
double &errorAngle, const double gainParallel,
const double gainOrtho) {
void SafeCtrl::calculateRotationalRateTorque(const double gainParallel, const double gainOrtho) {
// calculate torque for parallel rotational rate
VectorOperations<double>::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3);

27
mission/controller/acs/control/SafeCtrl.h
View File

@ -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);

21
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -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_ */