rework lock handling in rtd handler
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
EIVE/eive-obsw/pipeline/pr-develop This commit looks good

This commit is contained in:
Robin Müller 2023-03-03 11:43:47 +01:00
parent 245b5dfcc6
commit fcba76b7f1
No known key found for this signature in database
GPG Key ID: 71B58F8A3CDFA9AC
4 changed files with 54 additions and 44 deletions

View File

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

View File

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

View File

@ -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) // 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 // Check for invalid Inputs
if (!susDirBValid || !magFieldBValid || !magRateBValid) { if (!susDirBValid || !magFieldBValid || !magRateBValid) {
return returnvalue::FAILED; return returnvalue::FAILED;

2
tmtc

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