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

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

@ -9,97 +9,8 @@ AttitudeEstimation::~AttitudeEstimation() {}
void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
acsctrl::MgmDataProcessed *mgmData,
acsctrl::AttitudeEstimationData *attitudeEstimation) {
if (susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid()) {
// Normalize Data
double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
normSusI[3] = {0, 0, 0};
VectorOperations<double>::normalize(susData->susVecTot.value, normMgmB, 3);
VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normSusI, 3);
// Create Helper Vectors
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
helperSum[3] = {0, 0, 0};
VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
// Sensor Weights
double kSus = 0, kMgm = 0;
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
// Weighted Vectors
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
// Some weird Angles
double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
(VectorOperations<double>::dot(weightedSusB, normSusI) +
VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
VectorOperations<double>::dot(helperCross, kSumVec);
double beta = VectorOperations<double>::dot(helperSum, kSumVec);
double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
// I don't even know what this is supposed to be
double constPlus =
1. / (2 * std::sqrt(gamma * (gamma + alpha) *
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
double constMinus =
1. / (2 * std::sqrt(gamma * (gamma - alpha) *
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
// Calculate Quaternion
double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
qRotVecPt1[3] = {0, 0, 0};
if (alpha >= 0) {
// Scalar Part
qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
} else {
// Scalar Part
qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
// Low Pass
if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
}
}
{
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double));
attitudeEstimation->quatQuest.setValid(true);
}
}
} else {
if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid())) {
{
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
@ -107,5 +18,95 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
attitudeEstimation->quatQuest.setValid(false);
}
}
return;
}
// Normalize Data
double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
normSusI[3] = {0, 0, 0};
VectorOperations<double>::normalize(susData->susVecTot.value, normMgmB, 3);
VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normSusI, 3);
// Create Helper Vectors
double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
helperSum[3] = {0, 0, 0};
VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
// Sensor Weights
double kSus = 0, kMgm = 0;
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
// Weighted Vectors
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
// Some weird Angles
double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
(VectorOperations<double>::dot(weightedSusB, normSusI) +
VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
VectorOperations<double>::dot(helperCross, kSumVec);
double beta = VectorOperations<double>::dot(helperSum, kSumVec);
double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
// I don't even know what this is supposed to be
double constPlus =
1. / (2 * std::sqrt(gamma * (gamma + alpha) *
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
double constMinus =
1. / (2 * std::sqrt(gamma * (gamma - alpha) *
(1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
// Calculate Quaternion
double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
qRotVecPt1[3] = {0, 0, 0};
if (alpha >= 0) {
// Scalar Part
qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
} else {
// Scalar Part
qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
QuaternionOperations::normalize(qBI, qBI);
// Low Pass
if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
}
}
{
PoolReadGuard pg{attitudeEstimation};
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double));
attitudeEstimation->quatQuest.setValid(true);
}
}
}

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(