From 271830422aa2e05cb627357872ee28f6f2808ab9 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 11 Apr 2023 17:12:31 +0200
Subject: [PATCH] add a way to detect off devices

---
 linux/acs/SusPolling.cpp | 20 +++++++++++++++-----
 1 file changed, 15 insertions(+), 5 deletions(-)

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<uint8_t*>(&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;