diff --git a/CHANGELOG.md b/CHANGELOG.md index 4a9e7e42..70e97138 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,11 @@ will consitute of a breaking change warranting a new major release: and is triggered by the `AcsController` now. - Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`. +## 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 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8d35a8fd..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( - &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, + mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &attitudeEstimationData, &acsParameters); 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; diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index b4d0f0c4..2aea742c 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -5,18 +5,18 @@ 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 Mode_t mode, 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 - 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 (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 2fc2ab8f..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(acsctrl::SusDataProcessed *susDataProcessed, + void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,