diff --git a/CHANGELOG.md b/CHANGELOG.md index baccb4cb..6753b02a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,13 @@ 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`. + +## Changed + +- `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes +- QUEST and STR rates are now allowed per default +- Changed PTG Strat priorities to favor STR before MEKF. # [v7.6.1] 2024-02-05 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1b7ac2a4..1b77701d 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, diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 446fd661..f9ef3019 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -15,10 +15,10 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) { if (not magFieldValid) { return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; - } else if (mekfEnabled and mekfValid) { - return acs::ControlModeStrategy::PTGCTRL_MEKF; } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { return acs::ControlModeStrategy::PTGCTRL_STR; + } else if (mekfEnabled and mekfValid) { + return acs::ControlModeStrategy::PTGCTRL_MEKF; } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { return acs::ControlModeStrategy::PTGCTRL_QUEST; } @@ -227,6 +227,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; } } }