From 1cf3fbaabe8a45bcf40feed4c9428fa823b862f2 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 3 Mar 2023 10:48:59 +0100 Subject: [PATCH 1/4] fixed that mgm value was overwritten in wrong unit --- mission/controller/acs/control/SafeCtrl.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 3a69e343..9ef9acf5 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -94,7 +94,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal } // change unit from uT to T - VectorOperations::mulScalar(magFieldB, 1e-6, magFieldB, 3); + double magFieldBT[3] = {0,0,0}; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); // normalize sunDir and magDir double magDirB[3] = {0, 0, 0}; From ff50203fbb463bfb66df5715b47fa2ba3e7e4d21 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Mar 2023 11:04:08 +0100 Subject: [PATCH 2/4] smaller lock tweaks --- linux/devices/AcsBoardPolling.cpp | 87 +++++++++++---------- mission/controller/acs/control/SafeCtrl.cpp | 9 ++- tmtc | 2 +- 3 files changed, 50 insertions(+), 48 deletions(-) diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index 6a16d816..aad802d2 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -25,7 +25,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(); @@ -173,7 +173,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; @@ -184,45 +183,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; } @@ -294,8 +295,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; @@ -440,7 +441,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; { @@ -542,7 +543,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); @@ -635,7 +636,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/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 From 60afb573a0f47eba4865b99d2f338ec487c40250 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Mar 2023 11:06:30 +0100 Subject: [PATCH 3/4] comment --- linux/devices/AcsBoardPolling.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/devices/AcsBoardPolling.cpp b/linux/devices/AcsBoardPolling.cpp index aad802d2..a9128242 100644 --- a/linux/devices/AcsBoardPolling.cpp +++ b/linux/devices/AcsBoardPolling.cpp @@ -39,7 +39,7 @@ ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) { mgmRm3100Handler(mgm3Rm3100); mgmLis3Handler(mgm0Lis3); mgmLis3Handler(mgm2Lis3); - // To prevent task being not reactivated by tardy tasks + // To prevent task being reactivated by tardy tasks TaskFactory::delayTask(20); } return returnvalue::OK; From ec2ec32af420e3004e09e5915c013142a45e7a88 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 3 Mar 2023 11:56:31 +0100 Subject: [PATCH 4/4] actually use the stuff i wrote --- mission/controller/acs/control/SafeCtrl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 9ef9acf5..32a77591 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -99,7 +99,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 +160,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));