From f2d7f32952838a658f452defa96122e8b2027c42 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Jun 2024 15:50:47 +0200 Subject: [PATCH 01/11] changed source --- mission/controller/AcsController.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d3bbfcd8..7eb9b004 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -555,8 +555,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 (fusedRotRateSourcesData.rotRateTotalSusMgm.isValid() and + VectorOperations::norm(fusedRotRateSourcesData.rotRateTotalSusMgm.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (detumbleCounter > 0) { @@ -599,8 +599,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 (fusedRotRateSourcesData.rotRateTotalSusMgm.isValid() and + VectorOperations::norm(fusedRotRateSourcesData.rotRateTotalSusMgm.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } else if (detumbleCounter > 0) { -- 2.43.0 From f19b12960907adbba0b393cd23bf5bda1916aff3 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 10 Jun 2024 15:51:10 +0200 Subject: [PATCH 02/11] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8f92ffec..40f21f87 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Changed + +- Changed source for state machine of detumbling to SUS and MGM only. + # [v8.1.1] 2024-06-05 ## Added -- 2.43.0 From abbbb0cabb5f486dbe89938f896218f6edc389e9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Jun 2024 16:32:47 +0200 Subject: [PATCH 03/11] make detumble relevant rotational rate always observable --- mission/controller/AcsController.cpp | 25 ++++++----- mission/controller/AcsController.h | 5 +-- .../acs/FusedRotationEstimation.cpp | 44 ++++++++----------- .../AcsCtrlDefinitions.h | 15 +++---- 4 files changed, 40 insertions(+), 49 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7eb9b004..4d69c882 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" @@ -829,9 +831,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD 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); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SOURCE, &rotRateTotalSource); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); // Fused Rot Rate Sources diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 86100547..a7da3223 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 rotRateTotalSusMgm = PoolEntry(3); + PoolEntry rotRateTotalSource = PoolEntry(3); PoolEntry rotRateSource = PoolEntry(); // Fused Rot Rate Sources diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 2aea742c..19825a46 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,37 @@ 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->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(false); + } 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..c7ca5fd3 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_TOTAL_SUSMGM, + ROT_RATE_TOTAL_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_TOTAL_SUSMGM, this); + lp_vec_t rotRateTotalSource = + lp_vec_t(sid.objectId, ROT_RATE_TOTAL_SOURCE, this); lp_var_t rotRateSource = lp_var_t(sid.objectId, ROT_RATE_SOURCE, this); private: -- 2.43.0 From e305d77b328b695696840498088ab0a9a261585c Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Jun 2024 16:34:00 +0200 Subject: [PATCH 04/11] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 40f21f87..4662e482 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,7 @@ will consitute of a breaking change warranting a new major release: ## Changed - Changed source for state machine of detumbling to SUS and MGM only. +- Changed `FusedRotRateData` dataset to always display rotation rate from SUS and MGM. # [v8.1.1] 2024-06-05 -- 2.43.0 From b47e8b1dddcfd9ac5fdd1d0d9e221c77144f2bfb Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Jun 2024 16:45:33 +0200 Subject: [PATCH 05/11] fix enum --- .../controller/controllerdefinitions/AcsCtrlDefinitions.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c7ca5fd3..ec99fe54 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -129,8 +129,8 @@ enum PoolIds : lp_id_t { RW_TARGET_SPEED, MTQ_TARGET_DIPOLE, // Fused Rotation Rate - ROT_RATE_TOTAL_SUSMGM, - ROT_RATE_TOTAL_SOURCE, + ROT_RATE_TOT_SUSMGM, + ROT_RATE_TOT_SOURCE, ROT_RATE_SOURCE, // Fused Rotation Rate Sources ROT_RATE_ORTHOGONAL_SUSMGM, @@ -318,9 +318,9 @@ class FusedRotRateData : public StaticLocalDataSet { : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {} lp_vec_t rotRateTotalSusMgm = - lp_vec_t(sid.objectId, ROT_RATE_TOTAL_SUSMGM, this); + lp_vec_t(sid.objectId, ROT_RATE_TOT_SUSMGM, this); lp_vec_t rotRateTotalSource = - lp_vec_t(sid.objectId, ROT_RATE_TOTAL_SOURCE, this); + lp_vec_t(sid.objectId, ROT_RATE_TOT_SOURCE, this); lp_var_t rotRateSource = lp_var_t(sid.objectId, ROT_RATE_SOURCE, this); private: -- 2.43.0 From e44f1974cd66d91cc354bd519b57aa809096fd20 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Jun 2024 16:53:35 +0200 Subject: [PATCH 06/11] missed that one --- mission/controller/AcsController.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 4d69c882..8bd63b9c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -831,8 +831,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0}); // Fused Rot Rate - localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SUSMGM, &rotRateTotalSusMgm); - localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL_SOURCE, &rotRateTotalSource); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SUSMGM, &rotRateTotalSusMgm); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SOURCE, &rotRateTotalSource); localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_SOURCE, &rotRateSource); poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); // Fused Rot Rate Sources -- 2.43.0 From 80349820674a29defcc93ba4a2fde00434447ee9 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Wed, 12 Jun 2024 17:04:13 +0200 Subject: [PATCH 07/11] and that one --- mission/controller/AcsController.cpp | 4 ++-- mission/controller/AcsController.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8bd63b9c..542da9b4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -831,8 +831,8 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0}); // Fused Rot Rate - localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SUSMGM, &rotRateTotalSusMgm); - localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOT_SOURCE, &rotRateTotalSource); + 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, 10.0}); // Fused Rot Rate Sources diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index a7da3223..74ff8fdf 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -271,8 +271,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes // Fused Rot Rate acsctrl::FusedRotRateData fusedRotRateData; - PoolEntry rotRateTotalSusMgm = PoolEntry(3); - PoolEntry rotRateTotalSource = PoolEntry(3); + PoolEntry rotRateTotSusMgm = PoolEntry(3); + PoolEntry rotRateTotSource = PoolEntry(3); PoolEntry rotRateSource = PoolEntry(); // Fused Rot Rate Sources -- 2.43.0 From 1731b21953cd85e84e44260cfec1466dd8b46954 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 24 Jun 2024 11:02:54 +0200 Subject: [PATCH 08/11] set quest to invalid if sun vector and mgm vector nearly align --- mission/controller/acs/AcsParameters.cpp | 3 +++ mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/AttitudeEstimation.cpp | 14 ++++++++++++++ 3 files changed, 18 insertions(+) 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}; -- 2.43.0 From 8da305b2477e396115d1fdaa30f564d041a953de Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 24 Jun 2024 11:18:49 +0200 Subject: [PATCH 09/11] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 714ec301..b90d1be1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,8 @@ will consitute of a breaking change warranting a new major release: - 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. # [v8.1.1] 2024-06-05 -- 2.43.0 From ce7d1441de9647d81e398191ba8a70559c8ef6ed Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 24 Jun 2024 13:19:25 +0200 Subject: [PATCH 10/11] whoopsies --- mission/controller/acs/FusedRotationEstimation.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 19825a46..21c7f331 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -39,6 +39,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { std::memcpy(fusedRotRateData->rotRateTotalSource.value, fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); + fusedRotRateData->rotRateTotalSource.setValid(true); fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM; fusedRotRateData->rotRateSource.setValid(true); } else { @@ -53,7 +54,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { std::memcpy(fusedRotRateData->rotRateTotalSusMgm.value, fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); - fusedRotRateData->rotRateTotalSusMgm.setValid(false); + fusedRotRateData->rotRateTotalSusMgm.setValid(true); } else { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { -- 2.43.0 From 54e4d27fe19bcaa1ca1678d56c61c12bb2b6c693 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Mon, 24 Jun 2024 13:26:56 +0200 Subject: [PATCH 11/11] this is cleaner --- mission/controller/AcsController.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 542da9b4..a462032d 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -557,8 +557,8 @@ void AcsController::performPointingCtrl() { void AcsController::handleDetumbling() { switch (detumbleState) { case DetumbleState::NO_DETUMBLE: - if (fusedRotRateSourcesData.rotRateTotalSusMgm.isValid() and - VectorOperations::norm(fusedRotRateSourcesData.rotRateTotalSusMgm.value, 3) > + if (fusedRotRateData.rotRateTotalSusMgm.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (detumbleCounter > 0) { @@ -601,8 +601,8 @@ void AcsController::handleDetumbling() { detumbleState = DetumbleState::NO_DETUMBLE; break; case DetumbleState::IN_DETUMBLE: - if (fusedRotRateSourcesData.rotRateTotalSusMgm.isValid() and - VectorOperations::norm(fusedRotRateSourcesData.rotRateTotalSusMgm.value, 3) < + if (fusedRotRateData.rotRateTotalSusMgm.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotalSusMgm.value, 3) < acsParameters.detumbleParameter.omegaDetumbleEnd) { detumbleCounter++; } else if (detumbleCounter > 0) { -- 2.43.0