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

.
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-14 13:22:35 +01:00
parent 60a348a08f
commit c155f399b1
7 changed files with 67 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -4,6 +4,31 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_)
acsParameters = acsParameters_;
}
void FusedRotationEstimation::estimateFusedRotationRate(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues,
acsctrl::FusedRotRateData *fusedRotRateData) {
if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) {
double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
if (VectorOperations<double>::norm(quatOld, 4) != 0) {
estimateFusedRotationRateStr(quatNew, fusedRotRateData);
} else {
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateData);
}
std::memcpy(quatOld, quatNew, sizeof(quatOld));
} else {
std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld));
estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed,
fusedRotRateData);
}
}
void FusedRotationEstimation::estimateFusedRotationRateStr(
double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData) {}
void FusedRotationEstimation::estimateFusedRotationRateSafe(
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,
acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) {
@ -13,9 +38,9 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe(
not mgmDataProcessed->mgmVecTotDerivative.isValid())) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
// store for calculation of angular acceleration
@ -92,9 +117,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse(
VectorOperations<double>::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) {
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->setValidity(false, true);
}
return;
@ -106,9 +131,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse(
3);
{
PoolReadGuard pg(fusedRotRateData);
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateOrthogonal.setValid(false);
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double));
std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateData->rotRateParallel.setValid(false);
std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double));
fusedRotRateData->rotRateTotal.setValid(true);