diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index a963f326..e4026300 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -23,8 +23,8 @@ ReturnValue_t SensorValues::updateMgm() { std::vector results; { - PoolReadGuard pgMgm0(&mgm0Lis3Set); - results.push_back(pgMgm0.getReadResult()); + PoolReadGuard pgMgm(&mgm0Lis3Set); + results.push_back(pgMgm.getReadResult()); } { PoolReadGuard pgMgm(&mgm1Rm3100Set); @@ -51,31 +51,49 @@ ReturnValue_t SensorValues::updateMgm() { } ReturnValue_t SensorValues::updateSus() { - ReturnValue_t result; - PoolReadGuard pgSus0(&susSets[0]), pgSus1(&susSets[1]), pgSus2(&susSets[2]), pgSus3(&susSets[3]), - pgSus4(&susSets[4]), pgSus5(&susSets[5]), pgSus6(&susSets[6]), pgSus7(&susSets[7]), - pgSus8(&susSets[8]), pgSus9(&susSets[9]), pgSus10(&susSets[10]), pgSus11(&susSets[11]); - - result = (pgSus0.getReadResult() || pgSus1.getReadResult() || pgSus2.getReadResult() || - pgSus3.getReadResult() || pgSus4.getReadResult() || pgSus5.getReadResult() || - pgSus6.getReadResult() || pgSus7.getReadResult() || pgSus8.getReadResult() || - pgSus9.getReadResult() || pgSus10.getReadResult() || pgSus11.getReadResult()); - return result; + std::vector results; + for (auto& susSet : susSets) { + { + PoolReadGuard pgSus(&susSet); + results.push_back(pgSus.getReadResult()); + } + } + for (const auto& result : results) { + if (result != returnvalue::OK) { + return result; + } + } + return returnvalue::OK; } ReturnValue_t SensorValues::updateGyr() { - ReturnValue_t result; - PoolReadGuard pgGyr0(&gyr0AdisSet), pgGyr1(&gyr1L3gSet), pgGyr2(&gyr2AdisSet), - pgGyr3(&gyr3L3gSet); - - result = (pgGyr0.getReadResult() || pgGyr1.getReadResult() || pgGyr2.getReadResult() || - pgGyr3.getReadResult()); - return result; + std::vector results; + { + PoolReadGuard pgGyr(&gyr0AdisSet); + results.push_back(pgGyr.getReadResult()); + } + { + PoolReadGuard pgGyr(&gyr1L3gSet); + results.push_back(pgGyr.getReadResult()); + } + { + PoolReadGuard pgGyr(&gyr2AdisSet); + results.push_back(pgGyr.getReadResult()); + } + { + PoolReadGuard pgGyr(&gyr3L3gSet); + results.push_back(pgGyr.getReadResult()); + } + for (const auto& result : results) { + if (result != returnvalue::OK) { + return result; + } + } + return returnvalue::OK; } ReturnValue_t SensorValues::updateStr() { PoolReadGuard pgStr(&strSet); - return pgStr.getReadResult(); } @@ -85,12 +103,29 @@ ReturnValue_t SensorValues::updateGps() { } ReturnValue_t SensorValues::updateRw() { - ReturnValue_t result; - PoolReadGuard pgRw1(&rw1Set), pgRw2(&rw2Set), pgRw3(&rw3Set), pgRw4(&rw4Set); - - result = (pgRw1.getReadResult() || pgRw2.getReadResult() || pgRw3.getReadResult() || - pgRw4.getReadResult()); - return result; + std::vector results; + { + PoolReadGuard pgRw(&rw1Set); + results.push_back(pgRw.getReadResult()); + } + { + PoolReadGuard pgRw(&rw2Set); + results.push_back(pgRw.getReadResult()); + } + { + PoolReadGuard pgRw(&rw3Set); + results.push_back(pgRw.getReadResult()); + } + { + PoolReadGuard pgRw(&rw4Set); + results.push_back(pgRw.getReadResult()); + } + for (const auto& result : results) { + if (result != returnvalue::OK) { + return result; + } + } + return returnvalue::OK; } ReturnValue_t SensorValues::update() {