diff --git a/fsfw b/fsfw index 64537d44..6e17e455 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 64537d442a335500bdc87ff382099de65f0b7aa7 +Subproject commit 6e17e45506b0d9834d3ae9ded6f044e13e3c4abd diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index 3b35167d..947d0730 100644 --- 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(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); diff --git a/linux/devices/Max31865RtdPolling.cpp b/linux/devices/Max31865RtdPolling.cpp index 53aad8fe..dad75411 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; @@ -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(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(), @@ -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; 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 9ef9acf5..23a86551 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; @@ -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::normalize(magFieldB, magDirB, 3); + VectorOperations::normalize(magFieldBT, magDirB, 3); VectorOperations::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::cross(magFieldB, torqueB, crossMagFieldTorque); - double magMomFactor = pow(VectorOperations::norm(magFieldB, 3), 2); + VectorOperations::cross(magFieldBT, torqueB, crossMagFieldTorque); + double magMomFactor = pow(VectorOperations::norm(magFieldBT, 3), 2); VectorOperations::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); diff --git a/tmtc b/tmtc index 77fbcede..783d5a8e 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc +Subproject commit 783d5a8ed56a9683fc75d2aaffcabe82af34ffa9