diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 83088b41..10053da8 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -107,8 +107,6 @@ void AcsController::performControlOperation() { } void AcsController::performSafe() { - ACS::SensorValues sensorValues; - timeval now; Clock::getClock_timeval(&now); @@ -197,8 +195,6 @@ void AcsController::performSafe() { } void AcsController::performDetumble() { - ACS::SensorValues sensorValues; - timeval now; Clock::getClock_timeval(&now); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e1e31a2a..0ae4f338 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -38,6 +38,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes private: AcsParameters acsParameters; + ACS::SensorValues sensorValues; SensorProcessing sensorProcessing; Navigation navigation; ActuatorCmd actuatorCmd; @@ -70,7 +71,6 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void announceMode(bool recursive); /* ACS Datasets */ - ACS::SensorValues sensorValues; IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); // MGMs acsctrl::MgmDataRaw mgmDataRaw; diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 568a9e1c..a963f326 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -20,13 +20,34 @@ SensorValues::SensorValues() {} SensorValues::~SensorValues() {} ReturnValue_t SensorValues::updateMgm() { - ReturnValue_t result; - PoolReadGuard pgMgm0(&mgm0Lis3Set), pgMgm1(&mgm1Rm3100Set), pgMgm2(&mgm2Lis3Set), - pgMgm3(&mgm3Rm3100Set), pgImtq(&imtqMgmSet); + std::vector results; - result = (pgMgm0.getReadResult() || pgMgm1.getReadResult() || pgMgm2.getReadResult() || - pgMgm3.getReadResult() || pgImtq.getReadResult()); - return result; + { + PoolReadGuard pgMgm0(&mgm0Lis3Set); + results.push_back(pgMgm0.getReadResult()); + } + { + PoolReadGuard pgMgm(&mgm1Rm3100Set); + results.push_back(pgMgm.getReadResult()); + } + { + PoolReadGuard pgMgm(&mgm2Lis3Set); + results.push_back(pgMgm.getReadResult()); + } + { + PoolReadGuard pgMgm(&mgm3Rm3100Set); + results.push_back(pgMgm.getReadResult()); + } + { + PoolReadGuard pgMgm(&imtqMgmSet); + results.push_back(pgMgm.getReadResult()); + } + for (const auto& result : results) { + if (result != returnvalue::OK) { + return result; + } + } + return returnvalue::OK; } ReturnValue_t SensorValues::updateSus() { @@ -53,19 +74,14 @@ ReturnValue_t SensorValues::updateGyr() { } ReturnValue_t SensorValues::updateStr() { - ReturnValue_t result; PoolReadGuard pgStr(&strSet); - result = pgStr.getReadResult(); - return result; + return pgStr.getReadResult(); } ReturnValue_t SensorValues::updateGps() { - ReturnValue_t result; PoolReadGuard pgGps(&gpsSet); - - result = pgGps.getReadResult(); - return result; + return pgGps.getReadResult(); } ReturnValue_t SensorValues::updateRw() {