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

maybe this makes him happy
All checks were successful
EIVE/eive-obsw/pipeline/pr-dev-7.5.0 This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-12-06 15:52:20 +01:00
parent 42036f45f9
commit 8cd773d18b
2 changed files with 165 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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,
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,
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(