Merge remote-tracking branch 'origin/develop' into bugfix_countdown_uses_sysclock

2023-03-03 15:30:18 +01:00 · 2023-03-03 15:30:18 +01:00 · 2165f34a28
commit 2165f34a28
parent 53133573e1 70e1d41a08
6 changed files with 102 additions and 91 deletions
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 64537d442a335500bdc87ff382099de65f0b7aa7
+Subproject commit 6e17e45506b0d9834d3ae9ded6f044e13e3c4abd
--- a/linux/devices/AcsBoardPolling.cpp
+++ b/linux/devices/AcsBoardPolling.cpp
@ -26,7 +26,7 @@ AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF,

 ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
  while (true) {
-    ipcLock->lockMutex();
+    ipcLock->lockMutex(LOCK_TYPE, LOCK_TIMEOUT);
    state = InternalState::IDLE;
    ipcLock->unlockMutex();
    semaphore->acquire();
@ -177,7 +177,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
      return returnvalue::FAILED;
    }
    auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData);
-    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    if (req->mode != mgm.mode) {
      if (req->mode == acs::SimpleSensorMode::NORMAL) {
        mgm.performStartup = true;
@ -188,45 +187,47 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
    }
    return returnvalue::OK;
  };
-  switch (spiCookie->getChipSelectPin()) {
-    case (gpioIds::MGM_0_LIS3_CS): {
-      handleLis3Request(mgm0Lis3);
-      break;
+  {
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    switch (spiCookie->getChipSelectPin()) {
+      case (gpioIds::MGM_0_LIS3_CS): {
+        handleLis3Request(mgm0Lis3);
+        break;
+      }
+      case (gpioIds::MGM_1_RM3100_CS): {
+        handleRm3100Request(mgm1Rm3100);
+        break;
+      }
+      case (gpioIds::MGM_2_LIS3_CS): {
+        handleLis3Request(mgm2Lis3);
+        break;
+      }
+      case (gpioIds::MGM_3_RM3100_CS): {
+        handleRm3100Request(mgm3Rm3100);
+        break;
+      }
+      case (gpioIds::GYRO_0_ADIS_CS): {
+        handleAdisRequest(gyro0Adis);
+        break;
+      }
+      case (gpioIds::GYRO_2_ADIS_CS): {
+        handleAdisRequest(gyro2Adis);
+        break;
+      }
+      case (gpioIds::GYRO_1_L3G_CS): {
+        handleL3gRequest(gyro1L3g);
+        break;
+      }
+      case (gpioIds::GYRO_3_L3G_CS): {
+        handleL3gRequest(gyro3L3g);
+        break;
+      }
    }
-    case (gpioIds::MGM_1_RM3100_CS): {
-      handleRm3100Request(mgm1Rm3100);
-      break;
-    }
-    case (gpioIds::MGM_2_LIS3_CS): {
-      handleLis3Request(mgm2Lis3);
-      break;
-    }
-    case (gpioIds::MGM_3_RM3100_CS): {
-      handleRm3100Request(mgm3Rm3100);
-      break;
-    }
-    case (gpioIds::GYRO_0_ADIS_CS): {
-      handleAdisRequest(gyro0Adis);
-      break;
-    }
-    case (gpioIds::GYRO_2_ADIS_CS): {
-      handleAdisRequest(gyro2Adis);
-      break;
-    }
-    case (gpioIds::GYRO_1_L3G_CS): {
-      handleL3gRequest(gyro1L3g);
-      break;
-    }
-    case (gpioIds::GYRO_3_L3G_CS): {
-      handleL3gRequest(gyro3L3g);
-      break;
+    if (state == InternalState::IDLE) {
+      state = InternalState::BUSY;
    }
  }
-  MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-  if (state == InternalState::IDLE) {
-    state = InternalState::BUSY;
-    semaphore->release();
-  }
+  semaphore->release();
  return returnvalue::OK;
 }

@ -298,8 +299,8 @@ ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** b

 void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
  ReturnValue_t result;
-  acs::SimpleSensorMode mode;
-  bool gyroPerformStartup;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+  bool gyroPerformStartup = false;
  {
    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
    mode = l3g.mode;
@ -444,7 +445,7 @@ ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen

 void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
  ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
  bool cdHasTimedOut = false;
  bool mustPerformStartup = false;
  {
@ -546,7 +547,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {

 void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
  ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
  bool mustPerformStartup = false;
  {
    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
@ -639,7 +640,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {

 void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
  ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
  bool mustPerformStartup = false;
  {
    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
--- a/linux/devices/Max31865RtdPolling.cpp
+++ b/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;
    }
-    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;
+    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;
+        continue;
+      }
+      mustPerformInitHandling =
+          (rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut();
+      doWriteHighThreshold = rtd->writeHighThreshold;
+      doWriteLowThreshold = rtd->writeLowThreshold;
    }
-    if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
+    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;
-  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;
+  return returnvalue::OK;
 }

 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);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
+    updateActiveRtdsArray();
+    if (activeRtdsArray[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);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
+    updateActiveRtdsArray();
+    if (activeRtdsArray[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;
@ -310,15 +304,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(),
@ -459,6 +453,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;
--- a/linux/devices/Max31865RtdPolling.h
+++ b/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);
 };

--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/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,
-                          bool sunRateBValid, double *magFieldB, bool magFieldBValid,
-                          double *magRateB, bool magRateBValid, double *sunDirRef,
-                          double *satRateRef, double *outputAngle, double *outputMagMomB) {
+ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid,
+                                   double *sunRateB, bool sunRateBValid, double *magFieldB,
+                                   bool magFieldBValid, double *magRateB, bool magRateBValid,
+                                   double *sunDirRef, double *satRateRef, double *outputAngle,
+                                   double *outputMagMomB) {
  // Check for invalid Inputs
  if (!susDirBValid || !magFieldBValid || !magRateBValid) {
    return returnvalue::FAILED;
@ -99,7 +100,7 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal

  // normalize sunDir and magDir
  double magDirB[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(magFieldB, magDirB, 3);
+  VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
  VectorOperations<double>::normalize(susDirB, susDirB, 3);

  // Cosinus angle between sunDir and magDir
@ -160,8 +161,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
  // Magnetic moment
  double magMomB[3] = {0, 0, 0};
  double crossMagFieldTorque[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
-  double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
+  VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
+  double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
  VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);

  std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc
+Subproject commit 783d5a8ed56a9683fc75d2aaffcabe82af34ffa9