eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

SoC Calculator #754

Merged
muellerr merged 74 commits from soc-calculator into main 2023-10-11 10:50:48 +02:00
Conversation 1 Commits 74 Files Changed 52 +502 -120
19 changed files with 502 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8c97ad0213 - Show all commits

9
CHANGELOG.md
View File

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

122
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,27 @@ 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);
// 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;
}
// 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);
result = setBootCopyProtection(xsc::Chip::SELF_CHIP, xsc::Copy::SELF_COPY, true, protOpPerformed,
false);
if (result == returnvalue::OK and protOpPerformed) {
// 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;
}
@ -2583,6 +2586,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();

1
bsp_q7s/core/CoreController.h
View File

@ -374,6 +374,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
ReturnValue_t gracefulShutdownTasks(xsc::Chip chip, xsc::Copy copy, bool& protOpPerformed);
ReturnValue_t handleProtInfoUpdateLine(std::string nextLine);
ReturnValue_t handleSwitchingSdCardsOffNonBlocking();
int handleBootCopyProtAtIndex(xsc::Chip targetChip, xsc::Copy targetCopy, bool protect,
bool& protOperationPerformed, bool selfChip, bool selfCopy,
bool allChips, bool allCopies, uint8_t arrIdx);

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

10
mission/acs/defs.h
View File

@ -26,10 +26,12 @@ enum SafeModeStrategy : uint8_t {
SAFECTRL_OFF = 0,
SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1,
SAFECTRL_NO_SENSORS_FOR_CONTROL = 2,
SAFECTRL_ACTIVE_MEKF = 10,
SAFECTRL_WITHOUT_MEKF = 11,
SAFECTRL_ECLIPSE_DAMPING = 12,
SAFECTRL_ECLIPSE_IDELING = 13,
SAFECTRL_MEKF = 10,
SAFECTRL_GYR = 11,
SAFECTRL_SUSMGM = 12,
SAFECTRL_ECLIPSE_DAMPING_GYR = 13,
SAFECTRL_ECLIPSE_DAMPING_SUSMGM = 14,
SAFECTRL_ECLIPSE_IDELING = 15,
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_ */

2
tmtc

@ -1 +1 @@
Subproject commit a82cbff5a83eb37c68234bdeecd3b7d308d65eb1
Subproject commit 15716c988b6d26ae7f00e44b919d5ae7505d81ad