eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 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
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -73,51 +73,52 @@ void FusedRotationEstimation::estimateFusedRotationRate(
void FusedRotationEstimation::estimateFusedRotationRateStr(
ACS::SensorValues *sensorValues, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
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(quatOldStr, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldStr, quatOldInv);
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
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 rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldStr, quatOldInv);
QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
}
}
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
@@ -130,50 +131,14 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
}
}
std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
return;
}
void FusedRotationEstimation::estimateFusedRotationRateQuest(
acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
if (attitudeEstimationData->quatQuest.isValid()) {
if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldQuest, quatOldInv);
QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv,
quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
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;
}
if (not attitudeEstimationData->quatQuest.isValid()) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
@@ -182,6 +147,42 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
}
if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
double quatOldInv[4] = {0, 0, 0, 0};
double quatDelta[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatOldQuest, quatOldInv);
QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta);
if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
QuaternionOperations::normalize(quatDelta);
}
double rotVec[3] = {0, 0, 0};
double angle = QuaternionOperations::getAngle(quatDelta);
if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
{
PoolReadGuard pg(fusedRotRateSourcesData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
3 * sizeof(double));
fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
}
}
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
VectorOperations<double>::normalize(quatDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
{
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;
}
@@ -192,7 +193,8 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
}
}
std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
return;
}
void FusedRotationEstimation::estimateFusedRotationRateSusMgm(