diff --git a/CHANGELOG.md b/CHANGELOG.md index 045b7121..1329d338 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,12 @@ will consitute of a breaking change warranting a new major release: - Re-ordered some functions of the core controller in the initialize function. - Rad sensor is now only polled every 30 minutes instead of every device cycle to reduce wear of the RADFET electronics. +- The SD cards will still be switched OFF on a reboot, but this is done in a non-blocking manner + now with a timeout of 10 seconds where the reboot will be performed in any case. +- ACS Controller now includes the safe mode from FLP, which will calculate its rotational rate + from SUS and MGM measurements. To accommodate these changes, low-pass filters for SUS + measurements and rates as well as MGM measurements and rates are included. Usage of the new + controller as well as settings of the low-pass filters can be handled via parameter commands. ## Added @@ -41,6 +47,9 @@ will consitute of a breaking change warranting a new major release: ## Fixed +- General bugs in the SD card state machine. This might fix some other known bugs for certain + combinations of switching ON and OFF SD cards and also makes the whole state machine a lot more + robust against hanging up. - SUS dummy handler went to `MODE_NORMAL` for ON commands. - PL PCDU dummy went to `MODE_NORMAL` for ON commands. diff --git a/bsp_q7s/core/CoreController.cpp b/bsp_q7s/core/CoreController.cpp index 6d8f60c5..e4fb07ec 100644 --- a/bsp_q7s/core/CoreController.cpp +++ b/bsp_q7s/core/CoreController.cpp @@ -554,21 +554,24 @@ ReturnValue_t CoreController::sdStateMachine() { } // This lambda checks the non-blocking operation of the SD card manager and assigns the new - // state on success. It returns true for an operation success and false otherwise + // state on success. It returns 0 for an operation success, -1 for failed operations, and 1 + // for pending operations auto nonBlockingSdcOpChecking = [&](SdStates newStateOnSuccess, uint16_t maxCycleCount, std::string opPrintout) { SdCardManager::OpStatus status = sdcMan->checkCurrentOp(operation); - if (status == SdCardManager::OpStatus::SUCCESS) { + if (status == SdCardManager::OpStatus::SUCCESS or sdInfo.cycleCount > maxCycleCount) { sdFsmState = newStateOnSuccess; sdInfo.commandPending = false; + if (sdInfo.cycleCount > maxCycleCount) { + sif::warning << "CoreController::sdStateMachine: " << opPrintout << " takes too long" + << std::endl; + sdInfo.cycleCount = 0; + return -1; + } sdInfo.cycleCount = 0; - return true; - } else if (sdInfo.cycleCount > 4) { - sif::warning << "CoreController::sdStateMachine: " << opPrintout << " takes too long" - << std::endl; - return false; - } - return false; + return 0; + }; + return 1; }; if (sdFsmState == SdStates::UPDATE_SD_INFO_START) { @@ -644,7 +647,7 @@ ReturnValue_t CoreController::sdStateMachine() { sdFsmState = tgtState; } } else { - if (nonBlockingSdcOpChecking(SdStates::MOUNT_SELF, 10, "Setting SDC state")) { + if (nonBlockingSdcOpChecking(SdStates::MOUNT_SELF, 10, "Setting SDC state") <= 0) { sdInfo.activeState = sd::SdState::ON; currentStateSetter(sdInfo.active, sd::SdState::ON); // Skip the two cycles now. @@ -672,7 +675,7 @@ ReturnValue_t CoreController::sdStateMachine() { result = sdCardSetup(sdInfo.active, sd::SdState::MOUNTED, sdInfo.activeChar); sdInfo.commandPending = true; } else { - if (nonBlockingSdcOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card")) { + if (nonBlockingSdcOpChecking(SdStates::DETERMINE_OTHER, 5, "Mounting SD card") <= 0) { sdcMan->setActiveSdCard(sdInfo.active); currMntPrefix = sdcMan->getCurrentMountPrefix(); sdInfo.activeState = sd::SdState::MOUNTED; @@ -714,12 +717,7 @@ ReturnValue_t CoreController::sdStateMachine() { sdInfo.commandPending = true; } else { if (nonBlockingSdcOpChecking(SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE, 10, - "Switching off other SD card")) { - sdInfo.otherState = sd::SdState::OFF; - currentStateSetter(sdInfo.other, sd::SdState::OFF); - } else { - // Continue.. avoid being stuck here.. - sdFsmState = SdStates::SKIP_CYCLE_BEFORE_INFO_UPDATE; + "Switching off other SD card") <= 0) { sdInfo.otherState = sd::SdState::OFF; currentStateSetter(sdInfo.other, sd::SdState::OFF); } @@ -730,12 +728,7 @@ ReturnValue_t CoreController::sdStateMachine() { sdInfo.commandPending = true; } else { if (nonBlockingSdcOpChecking(SdStates::MOUNT_UNMOUNT_OTHER, 10, - "Switching on other SD card")) { - sdInfo.otherState = sd::SdState::ON; - currentStateSetter(sdInfo.other, sd::SdState::ON); - } else { - // Contnue.. avoid being stuck here. - sdFsmState = SdStates::MOUNT_UNMOUNT_OTHER; + "Switching on other SD card") <= 0) { sdInfo.otherState = sd::SdState::ON; currentStateSetter(sdInfo.other, sd::SdState::ON); } @@ -750,7 +743,8 @@ ReturnValue_t CoreController::sdStateMachine() { result = sdCardSetup(sdInfo.other, sd::SdState::ON, sdInfo.otherChar); sdInfo.commandPending = true; } else { - if (nonBlockingSdcOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card")) { + if (nonBlockingSdcOpChecking(SdStates::SET_STATE_OTHER, 10, "Unmounting other SD card") <= + 0) { sdInfo.otherState = sd::SdState::ON; currentStateSetter(sdInfo.other, sd::SdState::ON); } else { @@ -764,7 +758,8 @@ ReturnValue_t CoreController::sdStateMachine() { result = sdCardSetup(sdInfo.other, sd::SdState::MOUNTED, sdInfo.otherChar); sdInfo.commandPending = true; } else { - if (nonBlockingSdcOpChecking(SdStates::UPDATE_SD_INFO_END, 4, "Mounting other SD card")) { + if (nonBlockingSdcOpChecking(SdStates::UPDATE_SD_INFO_END, 4, "Mounting other SD card") <= + 0) { sdInfo.otherState = sd::SdState::MOUNTED; currentStateSetter(sdInfo.other, sd::SdState::MOUNTED); } @@ -840,7 +835,7 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS if (state == sd::SdState::MOUNTED) { if (targetState == sd::SdState::OFF) { sif::info << "Switching off SD card " << sdChar << std::endl; - return sdcMan->switchOffSdCard(sdCard, true, &sdInfo.currentState); + return sdcMan->switchOffSdCard(sdCard, sdInfo.currentState, true); } else if (targetState == sd::SdState::ON) { sif::info << "Unmounting SD card " << sdChar << std::endl; return sdcMan->unmountSdCard(sdCard); @@ -874,7 +869,7 @@ ReturnValue_t CoreController::sdCardSetup(sd::SdCard sdCard, sd::SdState targetS return sdcMan->mountSdCard(sdCard); } else if (targetState == sd::SdState::OFF) { sif::info << "Switching off SD card " << sdChar << std::endl; - return sdcMan->switchOffSdCard(sdCard, false, &sdInfo.currentState); + return sdcMan->switchOffSdCard(sdCard, sdInfo.currentState, false); } } else { sif::warning << "CoreController::sdCardSetup: Invalid state for this call" << std::endl; @@ -898,8 +893,7 @@ ReturnValue_t CoreController::sdColdRedundantBlockingInit() { sif::info << "Switching off secondary SD card " << sdInfo.otherChar << std::endl; // Switch off other SD card in cold redundant mode if setting up preferred one worked // without issues - ReturnValue_t result2 = - sdcMan->switchOffSdCard(sdInfo.other, sdInfo.otherState, &sdInfo.currentState); + ReturnValue_t result2 = sdcMan->switchOffSdCard(sdInfo.other, sdInfo.currentState, true); if (result2 != returnvalue::OK and result2 != SdCardManager::ALREADY_OFF) { sif::warning << "Switching off secondary SD card " << sdInfo.otherChar << " in cold redundant mode failed" << std::endl; @@ -1229,18 +1223,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 sdStatus; + ReturnValue_t result = sdcMan->getSdCardsStatus(sdStatus); + if (result != returnvalue::OK) { + return result; + } + Countdown maxWaitTimeCd(10000); + // Stopwatch watch; + auto waitingForFinish = [&]() { + auto currentState = sdcMan->checkCurrentOp(op); + if (currentState == SdCardManager::OpStatus::IDLE) { + return returnvalue::OK; + } + while (currentState == SdCardManager::OpStatus::ONGOING) { + if (maxWaitTimeCd.hasTimedOut()) { + return returnvalue::FAILED; + } + TaskFactory::delayTask(50); + currentState = sdcMan->checkCurrentOp(op); + } + return returnvalue::OK; + }; + if (sdStatus.first != sd::SdState::OFF) { + sdcMan->unmountSdCard(sd::SdCard::SLOT_0); + result = waitingForFinish(); + if (result != returnvalue::OK) { + return result; + } + sdcMan->switchOffSdCard(sd::SdCard::SLOT_0, sdStatus, false); + result = waitingForFinish(); + if (result != returnvalue::OK) { + return result; + } + } + if (sdStatus.second != sd::SdState::OFF) { + sdcMan->unmountSdCard(sd::SdCard::SLOT_1); + result = waitingForFinish(); + if (result != returnvalue::OK) { + return result; + } + sdcMan->switchOffSdCard(sd::SdCard::SLOT_1, sdStatus, false); + result = waitingForFinish(); + if (result != returnvalue::OK) { + return result; + } + } + return result; +} + bool CoreController::isNumber(const std::string &s) { return !s.empty() && std::find_if(s.begin(), s.end(), [](unsigned char c) { return !std::isdigit(c); }) == s.end(); diff --git a/bsp_q7s/core/CoreController.h b/bsp_q7s/core/CoreController.h index b6382d36..7c17de6f 100644 --- a/bsp_q7s/core/CoreController.h +++ b/bsp_q7s/core/CoreController.h @@ -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); diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index f887ad6f..b7195b38 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -124,6 +124,8 @@ void ObjectFactory::produce(void* args) { if (core::FW_VERSION_MAJOR >= 4) { battAndImtqI2cDev = q7s::I2C_PS_EIVE; } + static_cast(battAndImtqI2cDev); + #if OBSW_ADD_MGT == 1 createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev); #endif diff --git a/bsp_q7s/fs/SdCardManager.cpp b/bsp_q7s/fs/SdCardManager.cpp index 14e3e6aa..ffbed668 100644 --- a/bsp_q7s/fs/SdCardManager.cpp +++ b/bsp_q7s/fs/SdCardManager.cpp @@ -125,13 +125,8 @@ ReturnValue_t SdCardManager::switchOnSdCard(sd::SdCard sdCard, bool doMountSdCar return mountSdCard(sdCard); } -ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, bool doUnmountSdCard, - SdStatePair* statusPair) { - std::pair active; - ReturnValue_t result = getSdCardsStatus(active); - if (result != returnvalue::OK) { - return result; - } +ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, SdStatePair& sdStates, + bool doUnmountSdCard) { if (doUnmountSdCard) { if (not blocking) { sif::warning << "SdCardManager::switchOffSdCard: Two-step command but manager is" @@ -147,17 +142,17 @@ ReturnValue_t SdCardManager::switchOffSdCard(sd::SdCard sdCard, bool doUnmountSd return returnvalue::FAILED; } if (sdCard == sd::SdCard::SLOT_0) { - if (active.first == sd::SdState::OFF) { + if (sdStates.first == sd::SdState::OFF) { return ALREADY_OFF; } } else if (sdCard == sd::SdCard::SLOT_1) { - if (active.second == sd::SdState::OFF) { + if (sdStates.second == sd::SdState::OFF) { return ALREADY_OFF; } } if (doUnmountSdCard) { - result = unmountSdCard(sdCard); + ReturnValue_t result = unmountSdCard(sdCard); if (result != returnvalue::OK) { return result; } @@ -189,7 +184,7 @@ ReturnValue_t SdCardManager::setSdCardState(sd::SdCard sdCard, bool on) { command << "q7hw sd set " << sdstring << " " << statestring; cmdExecutor.load(command.str(), blocking, printCmdOutput); ReturnValue_t result = cmdExecutor.execute(); - if (blocking and result != returnvalue::OK) { + if (result != returnvalue::OK) { utility::handleSystemError(cmdExecutor.getLastError(), "SdCardManager::setSdCardState"); } return result; @@ -204,6 +199,7 @@ ReturnValue_t SdCardManager::getSdCardsStatus(SdStatePair& sdStates) { ReturnValue_t SdCardManager::mountSdCard(sd::SdCard sdCard) { using namespace std; if (cmdExecutor.getCurrentState() == CommandExecutor::States::PENDING) { + sif::warning << "SdCardManager::mountSdCard: Command still pending" << std::endl; return CommandExecutor::COMMAND_PENDING; } if (sdCard == sd::SdCard::BOTH) { diff --git a/bsp_q7s/fs/SdCardManager.h b/bsp_q7s/fs/SdCardManager.h index 7a4a7cbe..23a3d193 100644 --- a/bsp_q7s/fs/SdCardManager.h +++ b/bsp_q7s/fs/SdCardManager.h @@ -114,8 +114,7 @@ class SdCardManager : public SystemObject, public SdCardMountedIF { * @return - returnvalue::OK on success, ALREADY_ON if it is already on, * SYSTEM_CALL_ERROR on system error */ - ReturnValue_t switchOffSdCard(sd::SdCard sdCard, bool doUnmountSdCard = true, - SdStatePair* statusPair = nullptr); + ReturnValue_t switchOffSdCard(sd::SdCard sdCard, SdStatePair& sdStates, bool doUnmountSdCard); /** * Get the state of the SD cards. If the state file does not exist, this function will diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 41d09976..c02b692c 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -26,10 +26,12 @@ enum SafeModeStrategy : uint8_t { SAFECTRL_OFF = 0, SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, - SAFECTRL_ACTIVE_MEKF = 10, - SAFECTRL_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, }; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 04009cb2..b79fb714 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -7,6 +7,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), + fusedRotationEstimation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), ptgCtrl(&acsParameters), @@ -20,7 +21,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) gpsDataProcessed(this), mekfData(this), ctrlValData(this), - actuatorCmdData(this) {} + actuatorCmdData(this), + fusedRotRateData(this) {} ReturnValue_t AcsController::initialize() { ReturnValue_t result = parameterHelper.initialize(); @@ -146,6 +148,8 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, + &gyrDataProcessed, &fusedRotRateData); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && @@ -172,25 +176,42 @@ void AcsController::performSafe() { acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), + fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf, + acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): + case (acs::SafeModeStrategy::SAFECTRL_MEKF): safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF): - safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + case (acs::SafeModeStrategy::SAFECTRL_GYR): + safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING): - safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - sunTargetDir, magMomMtq, errAng); + case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): + safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value, + fusedRotRateData.rotRateOrthogonal.value, + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): + safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value, + gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, + errAng); + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): + safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, + fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, + errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; @@ -214,12 +235,20 @@ void AcsController::performSafe() { acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); // detumble check and switch - if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + if (acsParameters.safeModeControllerParameters.useMekf) { + if (mekfData.satRotRateMekf.isValid() and + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (acsParameters.safeModeControllerParameters.useGyr) { + if (gyrDataProcessed.gyrVecTot.isValid() and + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (fusedRotRateData.rotRateTotal.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (detumbleCounter > 0) { @@ -289,17 +318,26 @@ void AcsController::performDetumble() { actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); - if (mekfData.satRotRateMekf.isValid() && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { + if (acsParameters.safeModeControllerParameters.useMekf) { + if (mekfData.satRotRateMekf.isValid() and + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (acsParameters.safeModeControllerParameters.useGyr) { + if (gyrDataProcessed.gyrVecTot.isValid() and + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (fusedRotRateData.rotRateTotal.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (detumbleCounter > 0) { detumbleCounter -= 1; } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; // Triggers safe mode transition in subsystem @@ -707,6 +745,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0}); + // Fused Rot Rate + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal); + poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); return returnvalue::OK; } @@ -732,6 +775,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { return &ctrlValData; case acsctrl::ACTUATOR_CMD_DATA: return &actuatorCmdData; + case acsctrl::FUSED_ROTATION_RATE_DATA: + return &fusedRotRateData; default: return nullptr; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 0c8b94bb..c78c7e15 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +50,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes AcsParameters acsParameters; SensorProcessing sensorProcessing; + FusedRotationEstimation fusedRotationEstimation; Navigation navigation; ActuatorCmd actuatorCmd; Guidance guidance; @@ -226,6 +228,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry rwTargetSpeed = PoolEntry(4); PoolEntry mtqTargetDipole = PoolEntry(3); + // Fused Rot Rate + acsctrl::FusedRotRateData fusedRotRateData; + PoolEntry rotRateOrthogonal = PoolEntry(3); + PoolEntry rotRateParallel = PoolEntry(3); + PoolEntry rotRateTotal = PoolEntry(3); + // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index eea30389..9b0755cd 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -105,6 +105,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setVector(mgmHandlingParameters.mgm4variance); break; case 0x12: + parameterWrapper->set(mgmHandlingParameters.mgmVectorFilterWeight); + break; + case 0x13: parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight); break; default: @@ -224,6 +227,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x24: parameterWrapper->set(susHandlingParameters.susBrightnessThreshold); break; + case 0x25: + parameterWrapper->set(susHandlingParameters.susVectorFilterWeight); + break; + case 0x26: + parameterWrapper->set(susHandlingParameters.susRateFilterWeight); + break; default: return INVALID_IDENTIFIER_ID; } @@ -339,26 +348,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(safeModeControllerParameters.k_parallelMekf); break; case 0x3: - parameterWrapper->set(safeModeControllerParameters.k_orthoNonMekf); + parameterWrapper->set(safeModeControllerParameters.k_orthoGyr); break; case 0x4: - parameterWrapper->set(safeModeControllerParameters.k_alignNonMekf); + parameterWrapper->set(safeModeControllerParameters.k_alignGyr); break; case 0x5: - parameterWrapper->set(safeModeControllerParameters.k_parallelNonMekf); + parameterWrapper->set(safeModeControllerParameters.k_parallelGyr); break; case 0x6: - parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); + parameterWrapper->set(safeModeControllerParameters.k_orthoSusMgm); break; case 0x7: - parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); + parameterWrapper->set(safeModeControllerParameters.k_alignSusMgm); break; case 0x8: - parameterWrapper->set(safeModeControllerParameters.useMekf); + parameterWrapper->set(safeModeControllerParameters.k_parallelSusMgm); break; case 0x9: + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); + break; + case 0xA: + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); + break; + case 0xB: + parameterWrapper->set(safeModeControllerParameters.useMekf); + break; + case 0xC: + parameterWrapper->set(safeModeControllerParameters.useGyr); + break; + case 0xD: parameterWrapper->set(safeModeControllerParameters.dampingDuringEclipse); break; + case 0xE: + parameterWrapper->set(safeModeControllerParameters.sineLimitSunRotRate); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9e13070f..cdaf4899 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -77,7 +77,8 @@ class AcsParameters : public HasParametersIF { float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; - float mgmDerivativeFilterWeight = 0.5; + float mgmVectorFilterWeight = 0.85; + float mgmDerivativeFilterWeight = 0.85; } mgmHandlingParameters; struct SusHandlingParameters { @@ -767,6 +768,8 @@ class AcsParameters : public HasParametersIF { 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; float susBrightnessThreshold = 0.7; + float susVectorFilterWeight = .85; + float susRateFilterWeight = .85; } susHandlingParameters; struct GyrHandlingParameters { @@ -825,15 +828,22 @@ class AcsParameters : public HasParametersIF { double k_alignMekf = 4.0e-5; double k_parallelMekf = 3.75e-4; - double k_orthoNonMekf = 4.4e-3; - double k_alignNonMekf = 4.0e-5; - double k_parallelNonMekf = 3.75e-4; + double k_orthoGyr = 4.4e-3; + double k_alignGyr = 4.0e-5; + double k_parallelGyr = 3.75e-4; + + double k_orthoSusMgm = 1.1e-2; + double k_alignSusMgm = 2.0e-5; + double k_parallelSusMgm = 4.4e-4; double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)}; double sunTargetDir[3] = {0, 0, 1}; uint8_t useMekf = false; + uint8_t useGyr = true; uint8_t dampingDuringEclipse = true; + + float sineLimitSunRotRate = 0.24; } safeModeControllerParameters; struct PointingLawParameters { diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index 3c4a3475..a8b0e9a6 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -2,6 +2,7 @@ target_sources( ${LIB_EIVE_MISSION} PRIVATE AcsParameters.cpp ActuatorCmd.cpp + FusedRotationEstimation.cpp Guidance.cpp Igrf13Model.cpp MultiplicativeKalmanFilter.cpp diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp new file mode 100644 index 00000000..20f12d05 --- /dev/null +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -0,0 +1,103 @@ +#include "FusedRotationEstimation.h" + +FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) { + acsParameters = acsParameters_; +} + +void FusedRotationEstimation::estimateFusedRotationRateSafe( + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and + not fusedRotRateData->rotRateTotal.isValid()) or + (not susDataProcessed->susVecTotDerivative.isValid() and + not mgmDataProcessed->mgmVecTotDerivative.isValid())) { + { + PoolReadGuard pg(fusedRotRateData); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); + fusedRotRateData->setValidity(false, true); + } + return; + } + if (not susDataProcessed->susVecTot.isValid()) { + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + return; + } + + // calculate rotation around the sun + double magSunCross[3] = {0, 0, 0}; + + VectorOperations::cross(mgmDataProcessed->mgmVecTot.value, + susDataProcessed->susVecTot.value, magSunCross); + double magSunCrossNorm = VectorOperations::norm(magSunCross, 3); + double magNorm = VectorOperations::norm(mgmDataProcessed->mgmVecTot.value, 3); + double fusedRotRateParallel[3] = {0, 0, 0}; + if (magSunCrossNorm > + (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) { + double omegaParallel = + VectorOperations::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) * + pow(magSunCrossNorm, -2); + VectorOperations::mulScalar(susDataProcessed->susVecTot.value, omegaParallel, + fusedRotRateParallel, 3); + } else { + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); + return; + } + + // calculate rotation orthogonal to the sun + double fusedRotRateOrthogonal[3] = {0, 0, 0}; + VectorOperations::cross(susDataProcessed->susVecTotDerivative.value, + susDataProcessed->susVecTot.value, fusedRotRateOrthogonal); + VectorOperations::mulScalar( + fusedRotRateOrthogonal, + pow(VectorOperations::norm(susDataProcessed->susVecTot.value, 3), -2), + fusedRotRateOrthogonal, 3); + + // calculate total rotation rate + double fusedRotRateTotal[3] = {0, 0, 0}; + VectorOperations::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal); + + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } + + { + PoolReadGuard pg(fusedRotRateData); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal, + 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); + fusedRotRateData->setValidity(true, true); + } +} + +void FusedRotationEstimation::estimateFusedRotationRateEclipse( + acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + if (not gyrDataProcessed->gyrVecTot.isValid() or + VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { + { + PoolReadGuard pg(fusedRotRateData); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); + fusedRotRateData->setValidity(false, true); + } + return; + } + double angAccelB[3] = {0, 0, 0}; + VectorOperations::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3); + double fusedRotRateTotal[3] = {0, 0, 0}; + VectorOperations::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal, + 3); + { + PoolReadGuard pg(fusedRotRateData); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); + fusedRotRateData->rotRateParallel.setValid(false); + std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); + fusedRotRateData->rotRateTotal.setValid(true); + } +} diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h new file mode 100644 index 00000000..fa4fda1c --- /dev/null +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -0,0 +1,29 @@ +#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ +#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ + +#include +#include +#include +#include + +class FusedRotationEstimation { + public: + FusedRotationEstimation(AcsParameters *acsParameters_); + + void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateData *fusedRotRateData); + + protected: + private: + static constexpr double ZERO_VEC[3] = {0, 0, 0}; + + AcsParameters *acsParameters; + double rotRateOldB[3] = {0, 0, 0}; + + void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateData *fusedRotRateData); +}; + +#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */ diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 0279215f..511cae35 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -132,6 +132,10 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const for (uint8_t i = 0; i < 3; i++) { mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } + if (VectorOperations::norm(mgmVecTot, 3) != 0 and mgmDataProcessed->mgmVecTot.isValid()) { + lowPassFilter(mgmVecTot, mgmDataProcessed->mgmVecTot.value, + mgmParameters->mgmVectorFilterWeight); + } //-----------------------Mgm Rate Computation --------------------------------------------------- double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; @@ -351,6 +355,11 @@ void SensorProcessing::processSus( double susVecTot[3] = {0.0, 0.0, 0.0}; VectorOperations::normalize(susMeanValue, susVecTot, 3); + if (VectorOperations::norm(susVecTot, 3) != 0 and susDataProcessed->susVecTot.isValid()) { + lowPassFilter(susVecTot, susDataProcessed->susVecTot.value, + susParameters->susVectorFilterWeight); + } + /* -------- Sun Derivatiative --------------------- */ double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; @@ -363,6 +372,11 @@ void SensorProcessing::processSus( susVecTotDerivativeValid = true; } } + if (VectorOperations::norm(susVecTotDerivative, 3) != 0 and + susDataProcessed->susVecTotDerivative.isValid()) { + lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, + susParameters->susRateFilterWeight); + } timeOfSavedSusDirEst = timeOfSusMeasurement; { PoolReadGuard pg(susDataProcessed); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 43677ccf..8ad8b578 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,20 +9,36 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, - const uint8_t dampingEnabled) { +acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, + const bool sunDirValid, const bool fusedRateSplitValid, const bool fusedRateTotalValid, + const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF; - } else if (satRotRateValid and sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF; - } else if (dampingEnabled and satRotRateValid and not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING; - } else if (not dampingEnabled and satRotRateValid and not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + return acs::SafeModeStrategy::SAFECTRL_MEKF; + } else if (sunDirValid) { + if (gyrEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_GYR; + } else if (not gyrEnabled and fusedRateSplitValid) { + return acs::SafeModeStrategy::SAFECTRL_SUSMGM; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else if (not sunDirValid) { + if (dampingEnabled) { + if (gyrEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else if (not dampingEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } } else { return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } @@ -43,8 +59,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelMekf, + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_orthoMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, acsParameters->safeModeControllerParameters.k_alignMekf); @@ -57,9 +72,8 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, calculateMagneticMoment(magMomB); } -void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, - const double *sunDirB, const double *sunDirRefB, double *magMomB, - double &errorAngle) { +void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -68,11 +82,10 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelNonMekf, - acsParameters->safeModeControllerParameters.k_orthoNonMekf); + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr, + acsParameters->safeModeControllerParameters.k_orthoGyr); calculateAngleErrorTorque(sunDirB, sunDirRefB, - acsParameters->safeModeControllerParameters.k_alignNonMekf); + acsParameters->safeModeControllerParameters.k_alignGyr); // sum of all torques for (uint8_t i = 0; i < 3; i++) { @@ -82,8 +95,33 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, calculateMagneticMoment(magMomB); } -void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, - const double *sunDirRefB, double *magMomB, double &errorAngle) { +void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB, + const double *rotRateOrthogonalB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { + // convert magFieldB from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // calculate error angle between sunDirRef and sunDir + double dotSun = VectorOperations::dot(sunDirRefB, sunDirB); + errorAngle = acos(dotSun); + + std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB)); + std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB)); + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm, + acsParameters->safeModeControllerParameters.k_orthoSusMgm); + calculateAngleErrorTorque(sunDirB, sunDirRefB, + acsParameters->safeModeControllerParameters.k_alignSusMgm); + + // sum of all torques + for (uint8_t i = 0; i < 3; i++) { + cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i]; + } + + calculateMagneticMoment(magMomB); +} + +void SafeCtrl::safeRateDampingGyr(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -91,9 +129,28 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate errorAngle = NAN; splitRotationalRate(satRotRateB, sunDirRefB); - calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelNonMekf, - acsParameters->safeModeControllerParameters.k_orthoNonMekf); + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr, + acsParameters->safeModeControllerParameters.k_orthoGyr); + + // sum of all torques + VectorOperations::add(cmdParallel, cmdOrtho, cmdTorque, 3); + + // calculate magnetic moment to command + calculateMagneticMoment(magMomB); +} + +void SafeCtrl::safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, + double &errorAngle) { + // convert magFieldB from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // no error angle available for eclipse + errorAngle = NAN; + + splitRotationalRate(satRotRateB, sunDirRefB); + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm, + acsParameters->safeModeControllerParameters.k_orthoSusMgm); // sum of all torques VectorOperations::add(cmdParallel, cmdOrtho, cmdTorque, 3); @@ -110,9 +167,7 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD VectorOperations::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); } -void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, - double &errorAngle, const double gainParallel, - const double gainOrtho) { +void SafeCtrl::calculateRotationalRateTorque(const double gainParallel, const double gainOrtho) { // calculate torque for parallel rotational rate VectorOperations::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 12f9ddb0..55bcbf08 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -14,23 +14,34 @@ class SafeCtrl { acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled); + const bool fusedRateSplitValid, + const bool fusedRateTotalValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, double &errorAngle); - void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB, - const double *sunDirRefB, double *magMomB, double &errorAngle); + void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle); - void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, - double *magMomB, double &errorAngle); + void safeSusMgm(const double *magFieldB, const double *rotRateParallelB, + const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB, + double *magMomB, double &errorAngle); + + void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle); + + void safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle); void splitRotationalRate(const double *satRotRateB, const double *sunDirB); - void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, - double &errorAngle, const double gainParallel, - const double gainOrtho); + void calculateRotationalRates(const double *magFieldB, const double *magRateB, + const double *sunDirB, const double *sunRateB, + double *fusedRotRate); + + void calculateRotationalRateTorque(const double gainParallel, const double gainOrtho); void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, const double gainAlign); diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c8dbd275..86866c3f 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -18,7 +18,8 @@ enum SetIds : uint32_t { GPS_PROCESSED_DATA, MEKF_DATA, CTRL_VAL_DATA, - ACTUATOR_CMD_DATA + ACTUATOR_CMD_DATA, + FUSED_ROTATION_RATE_DATA, }; enum PoolIds : lp_id_t { @@ -103,6 +104,10 @@ enum PoolIds : lp_id_t { RW_TARGET_TORQUE, RW_TARGET_SPEED, MTQ_TARGET_DIPOLE, + // Fused Rotation Rate + ROT_RATE_ORTHOGONAL, + ROT_RATE_PARALLEL, + ROT_RATE_TOTAL, }; static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6; @@ -115,6 +120,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; +static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3; /** * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. @@ -273,6 +279,19 @@ class ActuatorCmdData : public StaticLocalDataSet { private: }; +class FusedRotRateData : public StaticLocalDataSet { + public: + FusedRotRateData(HasLocalDataPoolIF* hkOwner) + : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {} + + lp_vec_t rotRateOrthogonal = + lp_vec_t(sid.objectId, ROT_RATE_ORTHOGONAL, this); + lp_vec_t rotRateParallel = lp_vec_t(sid.objectId, ROT_RATE_PARALLEL, this); + lp_vec_t rotRateTotal = lp_vec_t(sid.objectId, ROT_RATE_TOTAL, this); + + private: +}; + } // namespace acsctrl #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */ diff --git a/tmtc b/tmtc index a82cbff5..15716c98 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit a82cbff5a83eb37c68234bdeecd3b7d308d65eb1 +Subproject commit 15716c988b6d26ae7f00e44b919d5ae7505d81ad