diff --git a/CHANGELOG.md b/CHANGELOG.md index f262af28..1d3394da 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,10 @@ will consitute of a breaking change warranting a new major release: - STR quaternions are now used by the `MEKF` by default - Changed nominal `SUS Assembly` side to `B Side`. +- Changed source for state machine of detumbling to SUS and MGM only. +- Changed `FusedRotRateData` dataset to always display rotation rate from SUS and MGM. +- Solution from `QUEST` will be set to invalid if sun vector and magnetic field vector are too close + to each other. - Changed collection intervals of dataset collection - `GPS Controller`: `GPS Set` to 60s - `MTQ Handler`: `HK with Torque`, `HK without Torque` to 60s diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1badf97d..d75a2d51 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -232,7 +232,8 @@ void AcsController::performSafe() { acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), - fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf, + fusedRotRateSourcesData.rotRateTotalSusMgm.isValid(), + acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { @@ -251,9 +252,10 @@ void AcsController::performSafe() { safeCtrlFailureCounter = 0; break; case (acs::ControlModeStrategy::SAFECTRL_SUSMGM): - safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, - fusedRotRateData.rotRateParallel.value, - fusedRotRateData.rotRateOrthogonal.value, + safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, + fusedRotRateSourcesData.rotRateTotalSusMgm.value, + fusedRotRateSourcesData.rotRateParallelSusMgm.value, + fusedRotRateSourcesData.rotRateOrthogonalSusMgm.value, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; @@ -267,8 +269,8 @@ void AcsController::performSafe() { break; case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, - fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, - errAng); + fusedRotRateSourcesData.rotRateTotalSusMgm.value, sunTargetDir, + magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; @@ -355,7 +357,7 @@ void AcsController::performPointingCtrl() { } acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid, - attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(), + attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotalSource.isValid(), fusedRotRateData.rotRateSource.value, useMekf); if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or @@ -387,11 +389,11 @@ void AcsController::performPointingCtrl() { quatBI[1] = sensorValues.strSet.caliQy.value; quatBI[2] = sensorValues.strSet.caliQz.value; quatBI[3] = sensorValues.strSet.caliQw.value; - std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); + std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB)); break; case acs::ControlModeStrategy::PTGCTRL_QUEST: std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI)); - std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); + std::memcpy(rotRateB, fusedRotRateData.rotRateTotalSource.value, sizeof(rotRateB)); break; default: sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl" @@ -555,8 +557,8 @@ void AcsController::performPointingCtrl() { void AcsController::handleDetumbling() { switch (detumbleState) { case DetumbleState::NO_DETUMBLE: - if (fusedRotRateData.rotRateTotal.isValid() and - VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) > + if (fusedRotRateData.rotRateTotalSusMgm.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (detumbleCounter > 0) { @@ -599,8 +601,8 @@ void AcsController::handleDetumbling() { detumbleState = DetumbleState::NO_DETUMBLE; break; case DetumbleState::IN_DETUMBLE: - if (fusedRotRateData.rotRateTotal.isValid() and - VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) < + if (fusedRotRateData.rotRateTotalSusMgm.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } else if (detumbleCounter > 0) { @@ -829,9 +831,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 30.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); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SUSMGM, &rotRateTotSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SOURCE, &rotRateTotSource); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 30.0}); // Fused Rot Rate Sources diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 86100547..74ff8fdf 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -271,9 +271,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes // Fused Rot Rate acsctrl::FusedRotRateData fusedRotRateData; - PoolEntry rotRateOrthogonal = PoolEntry(3); - PoolEntry rotRateParallel = PoolEntry(3); - PoolEntry rotRateTotal = PoolEntry(3); + PoolEntry rotRateTotSusMgm = PoolEntry(3); + PoolEntry rotRateTotSource = PoolEntry(3); PoolEntry rotRateSource = PoolEntry(); // Fused Rot Rate Sources diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index a9f806ec..2797dfa4 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -38,6 +38,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x5: parameterWrapper->set(onBoardParams.questFilterWeight); break; + case 0x6: + parameterWrapper->set(onBoardParams.questAngleLimit); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index eab708e5..6319f900 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -26,6 +26,7 @@ class AcsParameters : public HasParametersIF { uint8_t fusedRateFromStr = true; uint8_t fusedRateFromQuest = true; double questFilterWeight = 0.9; + double questAngleLimit = 5 * DEG2RAD; } onBoardParams; struct InertiaEIVE { diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index 3b6bf181..d59afe8a 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -29,6 +29,20 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, VectorOperations::normalize(mgmData->mgmVecTot.value, normMgmB, 3); VectorOperations::normalize(mgmData->magIgrfModel.value, normMgmI, 3); + if ((std::acos(VectorOperations::dot(normSusB, normMgmB)) < + acsParameters->onBoardParams.questAngleLimit) or + (std::acos(VectorOperations::dot(normSusI, normMgmI)) < + acsParameters->onBoardParams.questAngleLimit)) { + { + PoolReadGuard pg{attitudeEstimationData}; + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(attitudeEstimationData->quatQuest.value, ZERO_VEC4, 4 * sizeof(double)); + attitudeEstimationData->quatQuest.setValid(false); + } + } + return; + } + // Create Helper Vectors double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0}, helperSum[3] = {0, 0, 0}; diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 2aea742c..21c7f331 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -19,13 +19,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( acsParameters->onBoardParams.fusedRateFromStr)) { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateTotal.value, + std::memcpy(fusedRotRateData->rotRateTotalSource.value, fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + fusedRotRateData->rotRateTotalSource.setValid(true); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.setValid(true); } @@ -34,41 +30,38 @@ void FusedRotationEstimation::estimateFusedRotationRate( acsParameters->onBoardParams.fusedRateFromQuest)) { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->rotRateParallel.setValid(false); - std::memcpy(fusedRotRateData->rotRateTotal.value, + std::memcpy(fusedRotRateData->rotRateTotalSource.value, fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + fusedRotRateData->rotRateTotalSource.setValid(true); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST; fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, - fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double)); - fusedRotRateData->rotRateOrthogonal.setValid( - fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid()); - std::memcpy(fusedRotRateData->rotRateParallel.value, - fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double)); - fusedRotRateData->rotRateParallel.setValid( - fusedRotRateSourcesData->rotRateParallelSusMgm.isValid()); - std::memcpy(fusedRotRateData->rotRateTotal.value, + std::memcpy(fusedRotRateData->rotRateTotalSource.value, fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); - fusedRotRateData->rotRateTotal.setValid(true); + fusedRotRateData->rotRateTotalSource.setValid(true); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM; fusedRotRateData->rotRateSource.setValid(true); } else { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); - fusedRotRateData->setValidity(false, true); + std::memcpy(fusedRotRateData->rotRateTotalSource.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateTotalSource.setValid(false); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE; fusedRotRateData->rotRateSource.setValid(true); } } + if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { + std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value, + fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateTotalSusMgm.setValid(true); + } else { + PoolReadGuard pg(fusedRotRateData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateTotalSusMgm.setValid(false); + } + } } void FusedRotationEstimation::estimateFusedRotationRateStr( diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index 61489db2..ec99fe54 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -129,9 +129,8 @@ enum PoolIds : lp_id_t { RW_TARGET_SPEED, MTQ_TARGET_DIPOLE, // Fused Rotation Rate - ROT_RATE_ORTHOGONAL, - ROT_RATE_PARALLEL, - ROT_RATE_TOTAL, + ROT_RATE_TOT_SUSMGM, + ROT_RATE_TOT_SOURCE, ROT_RATE_SOURCE, // Fused Rotation Rate Sources ROT_RATE_ORTHOGONAL_SUSMGM, @@ -151,7 +150,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6; static constexpr uint8_t ATTITUDE_ESTIMATION_SET_ENTRIES = 4; 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 = 4; +static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3; static constexpr uint8_t FUSED_ROT_RATE_SOURCES_SET_ENTRIES = 5; /** @@ -318,10 +317,10 @@ class FusedRotRateData : public StaticLocalDataSet { 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); + lp_vec_t rotRateTotalSusMgm = + lp_vec_t(sid.objectId, ROT_RATE_TOT_SUSMGM, this); + lp_vec_t rotRateTotalSource = + lp_vec_t(sid.objectId, ROT_RATE_TOT_SOURCE, this); lp_var_t rotRateSource = lp_var_t(sid.objectId, ROT_RATE_SOURCE, this); private: