This commit is contained in:
parent
4ed516e0bc
commit
eff9116aec
@ -303,9 +303,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
||||
// First calculate error quaternion between current and target orientation without reference
|
||||
// quaternion
|
||||
double errorQuatWoRef[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::multiply(targetQuat, currentQuat, errorQuatWoRef);
|
||||
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
|
||||
// Then add rotation from reference quaternion
|
||||
QuaternionOperations::multiply(errorQuatWoRef, refQuat, errorQuat);
|
||||
QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
|
||||
// Keep scalar part of quaternion positive
|
||||
if (errorQuat[3] < 0) {
|
||||
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
|
||||
|
Loading…
Reference in New Issue
Block a user