eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

Merge branch 'develop' into tweaks_rtd_locks

Browse Source
This commit is contained in:
Robin Müller 2023-03-03 15:21:46 +01:00
commit 3b0490835c
2 changed files with 50 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
linux/devices/AcsBoardPolling.cpp
View File

@ -25,7 +25,7 @@ AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF,
ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) { ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
while (true) { while (true) {
ipcLock->lockMutex(); ipcLock->lockMutex(LOCK_TYPE, LOCK_TIMEOUT);
state = InternalState::IDLE; state = InternalState::IDLE;
ipcLock->unlockMutex(); ipcLock->unlockMutex();
semaphore->acquire(); semaphore->acquire();
@ -39,7 +39,7 @@ ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
mgmRm3100Handler(mgm3Rm3100); mgmRm3100Handler(mgm3Rm3100);
mgmLis3Handler(mgm0Lis3); mgmLis3Handler(mgm0Lis3);
mgmLis3Handler(mgm2Lis3); mgmLis3Handler(mgm2Lis3);
// To prevent task being not reactivated by tardy tasks // To prevent task being reactivated by tardy tasks
TaskFactory::delayTask(20); TaskFactory::delayTask(20);
} }
return returnvalue::OK; return returnvalue::OK;
@ -173,7 +173,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
return returnvalue::FAILED; return returnvalue::FAILED;
} }
auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData); 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 != mgm.mode) {
if (req->mode == acs::SimpleSensorMode::NORMAL) { if (req->mode == acs::SimpleSensorMode::NORMAL) {
mgm.performStartup = true; mgm.performStartup = true;
@ -184,45 +183,47 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
} }
return returnvalue::OK; return returnvalue::OK;
}; };
switch (spiCookie->getChipSelectPin()) { {
case (gpioIds::MGM_0_LIS3_CS): { MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
handleLis3Request(mgm0Lis3); switch (spiCookie->getChipSelectPin()) {
break; 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): { if (state == InternalState::IDLE) {
handleRm3100Request(mgm1Rm3100); state = InternalState::BUSY;
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;
} }
} }
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); semaphore->release();
if (state == InternalState::IDLE) {
state = InternalState::BUSY;
semaphore->release();
}
return returnvalue::OK; return returnvalue::OK;
} }
@ -294,8 +295,8 @@ ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** b
void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) { void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
ReturnValue_t result; ReturnValue_t result;
acs::SimpleSensorMode mode; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
bool gyroPerformStartup; bool gyroPerformStartup = false;
{ {
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
mode = l3g.mode; mode = l3g.mode;
@ -440,7 +441,7 @@ ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen
void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) { void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
ReturnValue_t result; ReturnValue_t result;
acs::SimpleSensorMode mode; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
bool cdHasTimedOut = false; bool cdHasTimedOut = false;
bool mustPerformStartup = false; bool mustPerformStartup = false;
{ {
@ -542,7 +543,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) { void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
ReturnValue_t result; ReturnValue_t result;
acs::SimpleSensorMode mode; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
bool mustPerformStartup = false; bool mustPerformStartup = false;
{ {
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
@ -635,7 +636,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) { void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
ReturnValue_t result; ReturnValue_t result;
acs::SimpleSensorMode mode; acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
bool mustPerformStartup = false; bool mustPerformStartup = false;
{ {
MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX); MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);

9
mission/controller/acs/control/SafeCtrl.cpp
View File

@ -95,11 +95,12 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
} }
// change unit from uT to T // change unit from uT to T
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldB, 3); double magFieldBT[3] = {0,0,0};
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
// normalize sunDir and magDir // normalize sunDir and magDir
double magDirB[3] = {0, 0, 0}; double magDirB[3] = {0, 0, 0};
VectorOperations<double>::normalize(magFieldB, magDirB, 3); VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
VectorOperations<double>::normalize(susDirB, susDirB, 3); VectorOperations<double>::normalize(susDirB, susDirB, 3);
// Cosinus angle between sunDir and magDir // Cosinus angle between sunDir and magDir
@ -160,8 +161,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
// Magnetic moment // Magnetic moment
double magMomB[3] = {0, 0, 0}; double magMomB[3] = {0, 0, 0};
double crossMagFieldTorque[3] = {0, 0, 0}; double crossMagFieldTorque[3] = {0, 0, 0};
VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque); VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2); double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3); VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double)); std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));