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

Higher ACS Modes STR Only #818

Merged
meggert merged 38 commits from higher-acs-modes-only-str into dev-7.5.0 2023-12-06 17:15:52 +01:00
Conversation 3 Commits 38 Files Changed 27 +165 -162
2 changed files with 165 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8cd773d18b - Show all commits

23
mission/controller/acs/AttitudeEstimation.cpp
View File

@ -9,8 +9,18 @@ AttitudeEstimation::~AttitudeEstimation() {}
void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
meggert marked this conversation as resolved
muellerr commented 2023-12-06 15:42:23 +01:00
Review
Copy Link

some subroutines might be a good idea, a lot of if else if else blocks in this function..

some subroutines might be a good idea, a lot of if else if else blocks in this function..
acsctrl::MgmDataProcessed *mgmData, acsctrl::MgmDataProcessed *mgmData,
acsctrl::AttitudeEstimationData *attitudeEstimation) { acsctrl::AttitudeEstimationData *attitudeEstimation) {
if (susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid()) { mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid())) {
{
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
attitudeEstimation->quatQuest.setValid(false);
}
}
return;
}
// Normalize Data // Normalize Data
double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0}, double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
normSusI[3] = {0, 0, 0}; normSusI[3] = {0, 0, 0};
@ -99,13 +109,4 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
attitudeEstimation->quatQuest.setValid(true); attitudeEstimation->quatQuest.setValid(true);
} }
} }
} else {
{
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
attitudeEstimation->quatQuest.setValid(false);
}
}
}
} }

50
mission/controller/acs/FusedRotationEstimation.cpp
View File

@ -73,8 +73,19 @@ void FusedRotationEstimation::estimateFusedRotationRate(
void FusedRotationEstimation::estimateFusedRotationRateStr( void FusedRotationEstimation::estimateFusedRotationRateStr(
ACS::SensorValues *sensorValues, const double timeDelta, ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if ((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) { sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
return;
}
double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) { if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
@ -122,28 +133,29 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
} }
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return; return;
}
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
} }
void FusedRotationEstimation::estimateFusedRotationRateQuest( void FusedRotationEstimation::estimateFusedRotationRateQuest(
acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
meggert marked this conversation as resolved
muellerr commented 2023-12-06 15:41:12 +01:00
Review
Copy Link

same as above: Put the error handling code with the pre-emptive return at the top, save one level of indentation for the regular code.

same as above: Put the error handling code with the pre-emptive return at the top, save one level of indentation for the regular code.
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) { acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (attitudeEstimationData->quatQuest.isValid()) { if (not attitudeEstimationData->quatQuest.isValid()) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
}
if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) { if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0}; double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0}; double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldQuest, quatOldInv); QuaternionOperations::inverse(quatOldQuest, quatOldInv);
QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta);
quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) { if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta); QuaternionOperations::normalize(quatDelta);
} }
@ -177,22 +189,12 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
{ {
PoolReadGuard pg(fusedRotRateSourcesData); PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) { if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
} }
} }
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest)); std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return; return;
}
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
} }
void FusedRotationEstimation::estimateFusedRotationRateSusMgm( void FusedRotationEstimation::estimateFusedRotationRateSusMgm(