SoC Calculator #754
@ -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.
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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,
|
||||
};
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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
103
mission/controller/acs/FusedRotationEstimation.cpp
Normal 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
29
mission/controller/acs/FusedRotationEstimation.h
Normal 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_ */
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit a82cbff5a83eb37c68234bdeecd3b7d308d65eb1
|
||||
Subproject commit 15716c988b6d26ae7f00e44b919d5ae7505d81ad
|
||||
Loading…
Reference in New Issue
Block a user