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,