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

this is much better
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-11-24 10:52:36 +01:00
parent 4a67f9ffe5
commit 647d5fda7c
2 changed files with 194 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -12,113 +12,194 @@ void FusedRotationEstimation::estimateFusedRotationRate(
acsctrl::FusedRotRateData *fusedRotRateData) { acsctrl::FusedRotRateData *fusedRotRateData) {
estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData); estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData);
estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData); estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData);
estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateSourcesData);
// Fused Rotation Estimation of STR if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and
acsParameters->onBoardParams.fusedRateFromStr) {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true);
}
} else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and
acsParameters->onBoardParams.fusedRateFromQuest) {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true);
}
} else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(
fusedRotRateSourcesData->rotRateOrthogonalSusMgm.isValid());
std::memcpy(fusedRotRateData->rotRateParallel.value,
fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(
fusedRotRateSourcesData->rotRateParallelSusMgm.isValid());
std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(true);
} else {
PoolReadGuard pg(fusedRotRateData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
}
}
void FusedRotationEstimation::estimateFusedRotationRateStr(
ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if ((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and if ((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())) {
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) {
estimateFusedRotationRateQuat(quatNew, timeDelta, true, fusedRotRateSourcesData); double quatOldInv[4] = {0, 0, 0, 0};
} else { double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldStr, quatOldInv);
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
QuaternionOperations::normalize(quatDelta);
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (angle == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{ {
PoolReadGuard pg(fusedRotRateSourcesData); PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false); fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
} }
} }
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr)); std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
} else { return;
std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr)); }
{ {
PoolReadGuard pg(fusedRotRateSourcesData); PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false); fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
} }
} }
std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
}
// Fused Rotation Estimation of QUEST void FusedRotationEstimation::estimateFusedRotationRateQuest(
if ((attitudeEstimationData->quatQuest.isValid())) { acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (attitudeEstimationData->quatQuest.isValid()) {
if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) { if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
estimateFusedRotationRateQuat(attitudeEstimationData->quatQuest.value, timeDelta, true, double quatOldInv[4] = {0, 0, 0, 0};
fusedRotRateSourcesData); double quatDelta[4] = {0, 0, 0, 0};
} else {
QuaternionOperations::inverse(quatOldQuest, quatOldInv);
QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv,
quatDelta);
QuaternionOperations::normalize(quatDelta);
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (angle == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{ {
PoolReadGuard pg(fusedRotRateSourcesData); PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
{
PoolReadGuard pg(fusedRotRateSourcesData);
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));
} else { return;
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest)); }
{ {
PoolReadGuard pg(fusedRotRateSourcesData); PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false); fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
} }
} }
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
// Fused Rotation Estimation of SUS&MGM
} }
void FusedRotationEstimation::estimateFusedRotationRateQuat( void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
double *quatNew, const double timeDelta, const bool str,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
if (str) {
QuaternionOperations::inverse(quatOldStr, quatOldInv);
} else {
QuaternionOperations::inverse(quatOldQuest, quatOldInv);
}
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
QuaternionOperations::normalize(quatDelta);
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (angle == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (str) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
} else {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (str) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
} else {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
}
void FusedRotationEstimation::estimateFusedRotationRateSafe(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and
not fusedRotRateData->rotRateTotal.isValid()) or not fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) or
(not susDataProcessed->susVecTotDerivative.isValid() and (not susDataProcessed->susVecTotDerivative.isValid() and
not mgmDataProcessed->mgmVecTotDerivative.isValid())) { not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); 3 * sizeof(double));
fusedRotRateData->setValidity(false, true); fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
}
} }
// store for calculation of angular acceleration // store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) { if (gyrDataProcessed->gyrVecTot.isValid()) {
@ -127,7 +208,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
return; return;
} }
if (not susDataProcessed->susVecTot.isValid()) { if (not susDataProcessed->susVecTot.isValid()) {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
// store for calculation of angular acceleration // store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) { if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@ -151,7 +232,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel, VectorOperations<double>::mulScalar(susDataProcessed->susVecTot.value, omegaParallel,
fusedRotRateParallel, 3); fusedRotRateParallel, 3);
} else { } else {
estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateSourcesData);
// store for calculation of angular acceleration // store for calculation of angular acceleration
if (gyrDataProcessed->gyrVecTot.isValid()) { if (gyrDataProcessed->gyrVecTot.isValid()) {
std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double));
@ -173,12 +254,18 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal); VectorOperations<double>::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal);
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal, if (pg.getReadResult() == returnvalue::OK) {
3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, fusedRotRateOrthogonal,
std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double)); 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(true);
fusedRotRateData->setValidity(true, true); std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, fusedRotRateParallel,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(true);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
}
} }
// store for calculation of angular acceleration // store for calculation of angular acceleration
@ -188,31 +275,44 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
} }
void FusedRotationEstimation::estimateFusedRotationRateEclipse( void FusedRotationEstimation::estimateFusedRotationRateEclipse(
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or if (not acsParameters->onBoardParams.fusedRateSafeDuringEclipse or
not gyrDataProcessed->gyrVecTot.isValid() or not gyrDataProcessed->gyrVecTot.isValid() or
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { VectorOperations<double>::norm(fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3) == 0) {
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); 3 * sizeof(double));
fusedRotRateData->setValidity(false, true); fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(false);
}
} }
return; return;
} }
double angAccelB[3] = {0, 0, 0}; double angAccelB[3] = {0, 0, 0};
VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3); VectorOperations<double>::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3);
double fusedRotRateTotal[3] = {0, 0, 0}; double fusedRotRateTotal[3] = {0, 0, 0};
VectorOperations<double>::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal, VectorOperations<double>::add(fusedRotRateSourcesData->rotRateTotalSusMgm.value, angAccelB,
3); fusedRotRateTotal, 3);
{ {
PoolReadGuard pg(fusedRotRateData); PoolReadGuard pg(fusedRotRateSourcesData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); if (pg.getReadResult() == returnvalue::OK) {
fusedRotRateData->rotRateOrthogonal.setValid(false); std::memcpy(fusedRotRateSourcesData->rotRateOrthogonalSusMgm.value, ZERO_VEC3,
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false); fusedRotRateSourcesData->rotRateOrthogonalSusMgm.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); std::memcpy(fusedRotRateSourcesData->rotRateParallelSusMgm.value, ZERO_VEC3,
fusedRotRateData->rotRateTotal.setValid(true); 3 * sizeof(double));
fusedRotRateSourcesData->rotRateParallelSusMgm.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalSusMgm.value, fusedRotRateTotal,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalSusMgm.setValid(true);
}
} }
} }

2
mission/controller/acs/FusedRotationEstimation.h
View File

@ -36,7 +36,7 @@ class FusedRotationEstimation {
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed,
acsctrl::FusedRotRateData *fusedRotRateData); acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);
void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData, void estimateFusedRotationRateQuest(acsctrl::AttitudeEstimationData *attitudeEstimationData,
const double timeDelta, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData); acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData);