rework lock handling in rtd handler

linux/devices/Max31865RtdPolling.cpp

@@ -56,19 +56,28 @@ bool Max31865RtdPolling::rtdIsActive(uint8_t idx) {
   return false;
 }
 
-bool Max31865RtdPolling::periodicInitHandling() {
+ReturnValue_t Max31865RtdPolling::periodicInitHandling() {
   using namespace MAX31865;
   ReturnValue_t result = returnvalue::OK;
   for (auto& rtd : rtds) {
     if (rtd == nullptr) {
       continue;
     }
+    bool mustPerformInitHandling = false;
+    bool doWriteLowThreshold = false;
+    bool doWriteHighThreshold = false;
+    {
       MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       if (mg.getLockResult() != returnvalue::OK) {
         sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
-      return false;
+        continue;
       }
-    if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
+      mustPerformInitHandling =
+          (rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut();
+      doWriteHighThreshold = rtd->writeHighThreshold;
+      doWriteLowThreshold = rtd->writeLowThreshold;
+    }
+    if (mustPerformInitHandling) {
       // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
       // or hardware specific issue where the CS needs to be pulled high and then low again
       // between transfers
@@ -77,13 +86,13 @@ bool Max31865RtdPolling::periodicInitHandling() {
         handleSpiError(rtd, result, "writeCfgReg");
         continue;
       }
-      if (rtd->writeLowThreshold) {
+      if (doWriteLowThreshold) {
         result = writeLowThreshold(rtd->spiCookie, rtd->lowThreshold);
         if (result != returnvalue::OK) {
           handleSpiError(rtd, result, "writeLowThreshold");
         }
       }
-      if (rtd->writeHighThreshold) {
+      if (doWriteHighThreshold) {
         result = writeHighThreshold(rtd->spiCookie, rtd->highThreshold);
         if (result != returnvalue::OK) {
           handleSpiError(rtd, result, "writeHighThreshold");
@@ -93,23 +102,12 @@ bool Max31865RtdPolling::periodicInitHandling() {
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "clearFaultStatus");
       }
+      MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       rtd->db.configured = true;
       rtd->db.active = true;
     }
   }
-  bool someRtdUsable = false;
+  return returnvalue::OK;
-  for (auto& rtd : rtds) {
-    if (rtd == nullptr) {
-      continue;
-    }
-    if (rtdIsActive(rtd->idx)) {
-#if OBSW_RTD_AUTO_MODE == 0
-      result = writeBiasSel(Bias::ON, rtd->spiCookie, BASE_CFG);
-#endif
-      someRtdUsable = true;
-    }
-  }
-  return someRtdUsable;
 }
 
 ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
@@ -119,12 +117,8 @@ ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    updateActiveRtdsArray();
-    if (mg.getLockResult() != returnvalue::OK) {
+    if (activeRtdsArray[rtd->idx]) {
-      sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
       ReturnValue_t result = writeCfgReg(rtd->spiCookie, BASE_CFG | (1 << CfgBitPos::ONE_SHOT));
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "writeCfgReg");
@@ -144,12 +138,8 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    updateActiveRtdsArray();
-    if (mg.getLockResult() != returnvalue::OK) {
+    if (activeRtdsArray[rtd->idx]) {
-      sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
       // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
       // or hardware specific issue where the CS needs to be pulled high and then low again
       // between transfers
@@ -166,6 +156,7 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
         handleSpiError(rtd, result, "readRtdVal");
         continue;
       }
+      MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       if (faultBitSet) {
         rtd->db.faultBitSet = faultBitSet;
       }
@@ -212,6 +203,10 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
   if (cookie == nullptr) {
     return returnvalue::FAILED;
   }
+  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
+  if (rtdCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
   // Empty command.. don't fail for now
   if (sendLen < 1) {
     return returnvalue::OK;
@@ -221,7 +216,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
     sif::warning << "Max31865RtdReader::sendMessage: Mutex lock failed" << std::endl;
     return returnvalue::FAILED;
   }
-  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
   uint8_t cmdRaw = sendData[0];
   if (cmdRaw > EiveMax31855::RtdCommands::NUM_CMDS) {
     sif::warning << "Max31865RtdReader::sendMessage: Invalid command" << std::endl;
@@ -312,15 +306,15 @@ ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t
 
 ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                       size_t* size) {
+  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
+  if (rtdCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
   MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (mg.getLockResult() != returnvalue::OK) {
     // TODO: Emit warning
     return returnvalue::FAILED;
   }
-  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
-  if (rtdCookie == nullptr) {
-    return returnvalue::FAILED;
-  }
   uint8_t* exchangePtr = rtdCookie->exchangeBuf.data();
   size_t serLen = 0;
   auto result = rtdCookie->db.serialize(&exchangePtr, &serLen, rtdCookie->exchangeBuf.size(),
@@ -461,6 +455,18 @@ ReturnValue_t Max31865RtdPolling::readNFromReg(SpiCookie* cookie, uint8_t reg, s
   return returnvalue::OK;
 }
 
+ReturnValue_t Max31865RtdPolling::updateActiveRtdsArray() {
+  MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+  if (mg.getLockResult() != returnvalue::OK) {
+    sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
+    return returnvalue::FAILED;
+  }
+  for (const auto& rtd : rtds) {
+    activeRtdsArray[rtd->idx] = rtdIsActive(rtd->idx);
+  }
+  return returnvalue::OK;
+}
+
 ReturnValue_t Max31865RtdPolling::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
                                                  const char* ctx) {
   cookie->db.spiErrorCount.value += 1;

linux/devices/Max31865RtdPolling.h

@@ -47,6 +47,7 @@ class Max31865RtdPolling : public SystemObject,
  private:
   std::vector<Max31865ReaderCookie*> rtds;
   std::array<uint8_t, 4> cmdBuf = {};
+  std::array<bool, 12> activeRtdsArray{};
   size_t dbLen = 0;
   MutexIF* readerLock;
   static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
@@ -59,7 +60,7 @@ class Max31865RtdPolling : public SystemObject,
   uint32_t csTimeoutMs = spi::RTD_CS_TIMEOUT;
   MutexIF* csLock = nullptr;
 
-  bool periodicInitHandling();
+  ReturnValue_t periodicInitHandling();
   ReturnValue_t periodicReadReqHandling();
   ReturnValue_t periodicReadHandling();
 
@@ -84,6 +85,8 @@ class Max31865RtdPolling : public SystemObject,
   ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
   ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
 
+  ReturnValue_t updateActiveRtdsArray();
+
   ReturnValue_t handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result, const char* ctx);
 };
 

mission/controller/acs/control/SafeCtrl.cpp

@@ -84,10 +84,11 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
 }
 
 // Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
-ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
+ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid,
-                          bool sunRateBValid, double *magFieldB, bool magFieldBValid,
+                                   double *sunRateB, bool sunRateBValid, double *magFieldB,
-                          double *magRateB, bool magRateBValid, double *sunDirRef,
+                                   bool magFieldBValid, double *magRateB, bool magRateBValid,
-                          double *satRateRef, double *outputAngle, double *outputMagMomB) {
+                                   double *sunDirRef, double *satRateRef, double *outputAngle,
+                                   double *outputMagMomB) {
   // Check for invalid Inputs
   if (!susDirBValid || !magFieldBValid || !magRateBValid) {
     return returnvalue::FAILED;

tmtc

@@ -1 +1 @@
-Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc
+Subproject commit 74e6d6fe5f10121e8444662a79fa10fada3d978c