From fcba76b7f1098e3d062e8f32c3340cd77caf116a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Mar 2023 11:43:47 +0100 Subject: [PATCH] rework lock handling in rtd handler --- linux/devices/Max31865RtdPolling.cpp | 82 +++++++++++---------- linux/devices/Max31865RtdPolling.h | 5 +- mission/controller/acs/control/SafeCtrl.cpp | 9 ++- tmtc | 2 +- 4 files changed, 54 insertions(+), 44 deletions(-) diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp index 004e7c60..6cfb2000 100644 --- 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(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(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(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(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; diff --git a/linux/devices/Max31865RtdPolling.h b/linux/devices/Max31865RtdPolling.h index da27de68..ef73fb8b 100644 --- a/linux/devices/Max31865RtdPolling.h +++ b/linux/devices/Max31865RtdPolling.h @@ -47,6 +47,7 @@ class Max31865RtdPolling : public SystemObject, private: std::vector rtds; std::array cmdBuf = {}; + std::array 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); }; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 3a69e343..e6b45318 100644 --- 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; diff --git a/tmtc b/tmtc index 77fbcede..74e6d6fe 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc +Subproject commit 74e6d6fe5f10121e8444662a79fa10fada3d978c -- 2.43.0