From 13aa6f50ff97bc436cef080ea3e9b6d388e2747f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 14:53:28 +0100 Subject: [PATCH 1/6] ignore rotation rates from quest and str for safe mode --- mission/controller/AcsController.cpp | 5 ++++- mission/controller/AcsController.h | 2 ++ mission/controller/acs/FusedRotationEstimation.cpp | 12 ++++++------ mission/controller/acs/FusedRotationEstimation.h | 2 +- 4 files changed, 13 insertions(+), 8 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8d35a8fd..6b43bc5c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -173,7 +173,7 @@ void AcsController::performAttitudeControl() { &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); fusedRotationEstimation.estimateFusedRotationRate( - &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, + modeIsSafe, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &attitudeEstimationData, &acsParameters); @@ -914,6 +914,9 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) { PoolReadGuard pg(&rw4SpeedSet); rw4SpeedSet.setRwSpeed(0, 10); } + modeIsSafe = true; + } else { + modeIsSafe = false; } if (submode == acs::SafeSubmode::DETUMBLE) { detumbleState = DetumbleState::IN_DETUMBLE; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e8102f35..c878bb6e 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -62,6 +62,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes double timeDelta = 0.0; timeval oldTimeRelative; + bool modeIsSafe = true; + AcsParameters acsParameters; SensorProcessing sensorProcessing; AttitudeEstimation attitudeEstimation; diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index b4d0f0c4..943d5733 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -5,17 +5,17 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) } void FusedRotationEstimation::estimateFusedRotationRate( - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, - acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, - acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, + const bool modeIsSafe, acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, + ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData, + const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, acsctrl::FusedRotRateData *fusedRotRateData) { estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData); estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData); estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed, fusedRotRateSourcesData); - if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and + if (not modeIsSafe and fusedRotRateSourcesData->rotRateTotalStr.isValid() and acsParameters->onBoardParams.fusedRateFromStr) { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { @@ -29,7 +29,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.setValid(true); } - } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and + } else if (not modeIsSafe and fusedRotRateSourcesData->rotRateTotalQuest.isValid() and acsParameters->onBoardParams.fusedRateFromQuest) { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index 2fc2ab8f..b95acd05 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -12,7 +12,7 @@ class FusedRotationEstimation { public: FusedRotationEstimation(AcsParameters *acsParameters_); - void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, + void estimateFusedRotationRate(const bool modeIsSafe, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, From 9adb4af0e5faa5e9d0f87f194781a9f7816290df Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 14:53:53 +0100 Subject: [PATCH 2/6] update params --- mission/controller/acs/AcsParameters.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 09804612..5c4513ff 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -20,8 +20,8 @@ class AcsParameters : public HasParametersIF { double sampleTime = 0.4; // [s] uint16_t ptgCtrlLostTimer = 750; uint8_t fusedRateSafeDuringEclipse = true; - uint8_t fusedRateFromStr = false; - uint8_t fusedRateFromQuest = false; + uint8_t fusedRateFromStr = true; + uint8_t fusedRateFromQuest = true; double questFilterWeight = 0.0; } onBoardParams; From 3313f589056ae6320081e9689c9ab5fd9034b721 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 23 Feb 2024 14:56:02 +0100 Subject: [PATCH 3/6] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index baccb4cb..a14d7d97 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,11 @@ will consitute of a breaking change warranting a new major release: - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode and is triggered by the `AcsController` now. +## Changed + +- `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes +- QUEST and STR rates are now allowed per default + # [v7.6.1] 2024-02-05 ## Changed From 7517b33d83123a586619489c047c420a0ab80476 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 09:39:13 +0100 Subject: [PATCH 4/6] if this doesnt fix the invalid speeds ... --- mission/controller/acs/control/PtgCtrl.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 0294e8b6..eb1e35c8 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -221,6 +221,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; } else if (rwCmdSpeeds[i] < currRwSpeed[i]) { rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; + } else { + rwCmdSpeeds[i] = 0; } } } From d14b83fab9bb9ff310211bb4998a949abd5e9814 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 09:44:09 +0100 Subject: [PATCH 5/6] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index baccb4cb..4a9e7e42 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,7 @@ will consitute of a breaking change warranting a new major release: - Bugfix in PLOC MPSoC HK set: Set and variables were not set valid. - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode and is triggered by the `AcsController` now. +- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`. # [v7.6.1] 2024-02-05 From 96acca48474875ef3fd82a463d46fcdc90aa7835 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 26 Feb 2024 10:14:15 +0100 Subject: [PATCH 6/6] removed redundant variable --- mission/controller/AcsController.cpp | 5 +---- mission/controller/AcsController.h | 2 -- mission/controller/acs/FusedRotationEstimation.cpp | 11 ++++++----- mission/controller/acs/FusedRotationEstimation.h | 2 +- 4 files changed, 8 insertions(+), 12 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 6b43bc5c..8fcbcbca 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -173,7 +173,7 @@ void AcsController::performAttitudeControl() { &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); fusedRotationEstimation.estimateFusedRotationRate( - modeIsSafe, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, + mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &attitudeEstimationData, &acsParameters); @@ -914,9 +914,6 @@ void AcsController::modeChanged(Mode_t mode, Submode_t submode) { PoolReadGuard pg(&rw4SpeedSet); rw4SpeedSet.setRwSpeed(0, 10); } - modeIsSafe = true; - } else { - modeIsSafe = false; } if (submode == acs::SafeSubmode::DETUMBLE) { detumbleState = DetumbleState::IN_DETUMBLE; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c878bb6e..e8102f35 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -62,8 +62,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes double timeDelta = 0.0; timeval oldTimeRelative; - bool modeIsSafe = true; - AcsParameters acsParameters; SensorProcessing sensorProcessing; AttitudeEstimation attitudeEstimation; diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 943d5733..2aea742c 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -5,7 +5,7 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) } void FusedRotationEstimation::estimateFusedRotationRate( - const bool modeIsSafe, acsctrl::SusDataProcessed *susDataProcessed, + const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, @@ -15,8 +15,8 @@ void FusedRotationEstimation::estimateFusedRotationRate( estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed, fusedRotRateSourcesData); - if (not modeIsSafe and fusedRotRateSourcesData->rotRateTotalStr.isValid() and - acsParameters->onBoardParams.fusedRateFromStr) { + if (not(mode == acs::AcsMode::SAFE) and (fusedRotRateSourcesData->rotRateTotalStr.isValid() and + acsParameters->onBoardParams.fusedRateFromStr)) { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); @@ -29,8 +29,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.setValid(true); } - } else if (not modeIsSafe and fusedRotRateSourcesData->rotRateTotalQuest.isValid() and - acsParameters->onBoardParams.fusedRateFromQuest) { + } else if (not(mode == acs::AcsMode::SAFE) and + (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and + acsParameters->onBoardParams.fusedRateFromQuest)) { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index b95acd05..d8e0bdae 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -12,7 +12,7 @@ class FusedRotationEstimation { public: FusedRotationEstimation(AcsParameters *acsParameters_); - void estimateFusedRotationRate(const bool modeIsSafe, acsctrl::SusDataProcessed *susDataProcessed, + void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,