diff --git a/linux/acs/SusPolling.cpp b/linux/acs/SusPolling.cpp index aea92ffe..7dfb93e5 100644 --- a/linux/acs/SusPolling.cpp +++ b/linux/acs/SusPolling.cpp @@ -95,11 +95,14 @@ ReturnValue_t SusPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer if (susIdx < 0) { return FAILED; } + if(susDevs[susIdx].replyResult != returnvalue::OK) { + return susDevs[susIdx].replyResult; + } MutexGuard mg(ipcLock); std::memcpy(&susDevs[susIdx].readerReply, &susDevs[susIdx].ownReply, sizeof(acs::SusReply)); *buffer = reinterpret_cast(&susDevs[susIdx].readerReply); *size = sizeof(acs::SusReply); - return OK; + return susDevs[susIdx].replyResult; } ReturnValue_t SusPolling::handleSusPolling() { @@ -164,11 +167,18 @@ ReturnValue_t SusPolling::handleSusPolling() { } MutexGuard mg(ipcLock); susDevs[idx].ownReply.tempRaw = ((rawReply[0] & 0x0f) << 8) | rawReply[1]; - for (unsigned chIdx = 0; chIdx < 6; chIdx++) { - susDevs[idx].ownReply.channelsRaw[chIdx] = - (rawReply[chIdx * 2 + 2] << 8) | rawReply[chIdx * 2 + 3]; + // Reply is all ones. Sensor is probably off or faulty when + // it should not be. + if(susDevs[idx].ownReply.tempRaw == 0x0fff) { + susDevs[idx].replyResult = returnvalue::FAILED; + } else { + susDevs[idx].replyResult = returnvalue::OK; + for (unsigned chIdx = 0; chIdx < 6; chIdx++) { + susDevs[idx].ownReply.channelsRaw[chIdx] = + (rawReply[chIdx * 2 + 2] << 8) | rawReply[chIdx * 2 + 3]; + } + susDevs[idx].ownReply.dataWasSet = true; } - susDevs[idx].ownReply.dataWasSet = true; } } return OK;