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

and i wasted a day for this little shit bug
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
Details

Browse Source
This commit is contained in:
Marius Eggert 2023-12-13 16:52:42 +01:00
parent 51ccbc6898
commit 9833a4e043
1 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -77,24 +77,22 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI)); qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part // Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3); VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3); VectorOperations<double>::mulScalar(helperSum, beta, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3); VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 4);
QuaternionOperations::normalize(qBI, qBI); QuaternionOperations::normalize(qBI, qBI);
} else { } else {
// Scalar Part // Scalar Part
qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI)); qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
// Rotational Vector Part // Rotational Vector Part
VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3); VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3); VectorOperations<double>::mulScalar(helperSum, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3); VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 4);
QuaternionOperations::normalize(qBI, qBI); QuaternionOperations::normalize(qBI, qBI);
} }
// Low Pass // Low Pass