wörkz

linux/devices/SusPolling.cpp

@@ -1,7 +1,13 @@
 #include "SusPolling.h"
 
 #include <fsfw/tasks/SemaphoreFactory.h>
+#include <fsfw/tasks/TaskFactory.h>
 #include <fsfw_hal/linux/spi/SpiCookie.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/devices/max1227.h>
+#include <unistd.h>
+
+#include "mission/devices/devicedefinitions/susMax1227Helpers.h"
 
 using namespace returnvalue;
 
@@ -18,21 +24,11 @@ ReturnValue_t SusPolling::performOperation(uint8_t operationCode) {
     state = InternalState::IDLE;
     ipcLock->unlockMutex();
     semaphore->acquire();
-    for(auto& susDev: susDevs) {
-      acs::SimpleSensorMode mode;
-      bool doPerformStartup = false;
-      {
-        MutexGuard mg(ipcLock);
-        mode = susDev.mode;
-        doPerformStartup = susDev.performStartup;
-      }
-      if (mode == acs::SimpleSensorMode::NORMAL) {
-        if(doPerformStartup) {
-          // Startup handling.
-        }
-        // Regular sensor polling.
-      }
-    }
+    // Give SUS handlers a chance to submit all requests.
+    TaskFactory::delayTask(2);
+    handleSusPolling();
+    // Protection against tardy tasks unlocking the thread again immediately.
+    TaskFactory::delayTask(20);
   }
   return OK;
 }
@@ -67,13 +63,18 @@ ReturnValue_t SusPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
   const auto* susReq = reinterpret_cast<const acs::SusRequest*>(sendData);
   MutexGuard mg(ipcLock);
   if (susDevs[susIdx].mode != susReq->mode) {
-    if(susReq->mode == acs::SimpleSensorMode::NORMAL) {
-     susDevs[susIdx].performStartup = true;
+    if (susReq->mode == acs::SimpleSensorMode::NORMAL) {
+      susDevs[susIdx].performStartup = true;
     } else {
+      susDevs[susIdx].ownReply.cfgWasSet = false;
       susDevs[susIdx].ownReply.dataWasSet = false;
     }
     susDevs[susIdx].mode = susReq->mode;
   }
+  if (state == InternalState::IDLE) {
+    state = InternalState::BUSY;
+    semaphore->release();
+  }
   return OK;
 }
 
@@ -97,6 +98,78 @@ ReturnValue_t SusPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer
   return OK;
 }
 
+ReturnValue_t SusPolling::handleSusPolling() {
+  ReturnValue_t result;
+  acs::SimpleSensorMode modes[12];
+  bool performStartups[12]{};
+  bool cfgsWereSet[12]{};
+  uint8_t idx = 0;
+  {
+    MutexGuard mg(ipcLock);
+    for (idx = 0; idx < 12; idx++) {
+      modes[idx] = susDevs[idx].mode;
+      performStartups[idx] = susDevs[idx].performStartup;
+    }
+  }
+  for (idx = 0; idx < 12; idx++) {
+    if (modes[idx] == acs::SimpleSensorMode::NORMAL) {
+      if (performStartups[idx]) {
+        // Startup handling.
+        cmdBuf[0] = susMax1227::SETUP_INT_CLOKED;
+        result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 1);
+        if (result != OK) {
+          susDevs[idx].replyResult = result;
+          continue;
+        }
+        MutexGuard mg(ipcLock);
+        susDevs[idx].ownReply.cfgWasSet = true;
+        cfgsWereSet[idx] = true;
+        susDevs[idx].performStartup = true;
+      }
+    }
+  }
+  for (idx = 0; idx < 12; idx++) {
+    if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) {
+      // Regular sensor polling.
+      cmdBuf[0] = max1227::buildResetByte(true);
+      cmdBuf[1] = susMax1227::CONVERSION;
+      result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        susDevs[idx].replyResult = result;
+        continue;
+      }
+    }
+  }
+
+  // Internal conversion time is 3.5 us
+  usleep(4);
+
+  for (idx = 0; idx < 12; idx++) {
+    if (modes[idx] == acs::SimpleSensorMode::NORMAL and cfgsWereSet[idx]) {
+      std::memset(cmdBuf.data(), 0, susMax1227::SIZE_READ_INT_CONVERSIONS);
+      result = spiComIF.sendMessage(susDevs[idx].cookie, cmdBuf.data(),
+                                    susMax1227::SIZE_READ_INT_CONVERSIONS);
+      if (result != OK) {
+        susDevs[idx].replyResult = result;
+        continue;
+      }
+      result = spiComIF.readReceivedMessage(susDevs[idx].cookie, &rawReply, &dummy);
+      if (result != OK) {
+        susDevs[idx].replyResult = result;
+        continue;
+      }
+      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];
+      }
+      susDevs[idx].ownReply.dataWasSet = true;
+    }
+  }
+  return OK;
+}
+
 int SusPolling::addressToIndex(address_t addr) {
   switch (addr) {
     case (addresses::SUS_0):