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

fixed quaternion multiplication bug
All checks were successful
EIVE/eive-obsw/pipeline/pr-main This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert 2024-02-05 09:24:41 +01:00
parent f0e551fa54
commit 6f3876d204
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -192,10 +192,12 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
// First calculate error quaternion between current and target orientation // First calculate error quaternion between current and target orientation without reference
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // quaternion
// Last calculate add rotation from reference quaternion double errorQuatWoRef[4] = {0, 0, 0, 0};
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
// Then add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
// Keep scalar part of quaternion positive // Keep scalar part of quaternion positive
if (errorQuat[3] < 0) { if (errorQuat[3] < 0) {
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4); VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);