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

PTG Something #856

Merged
meggert merged 9 commits from fix-your-rfs into main 2024-02-05 10:08:03 +01:00
Conversation 0 Commits 9 Files Changed 4 +6 -4
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6f3876d204 - Show all commits

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);