overloading is fun (but should be done right)
This commit is contained in:
parent
aa521e89f6
commit
8be94cf2dc
@ -408,9 +408,7 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
|||||||
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
|
||||||
double invTargetQuat[4] = {0, 0, 0, 0};
|
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
|
||||||
QuaternionOperations::inverse(targetQuat, invTargetQuat);
|
|
||||||
QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat);
|
|
||||||
// Last calculate add rotation from reference quaternion
|
// Last calculate add rotation from reference quaternion
|
||||||
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
|
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
|
||||||
// Keep scalar part of quaternion positive
|
// Keep scalar part of quaternion positive
|
||||||
@ -435,17 +433,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
|||||||
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 errorQuat[4],
|
double targetSatRotRate[3], double errorQuat[4],
|
||||||
double errorSatRotRate[3], double &errorAngle) {
|
double errorSatRotRate[3], double &errorAngle) {
|
||||||
// First calculate error quaternion between current and target orientation
|
double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0};
|
||||||
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
|
comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate,
|
||||||
// Keep scalar part of quaternion positive
|
errorQuat, errorSatRotRate, errorAngle);
|
||||||
if (errorQuat[3] < 0) {
|
|
||||||
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
|
|
||||||
}
|
|
||||||
// Calculate error angle
|
|
||||||
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
|
|
||||||
|
|
||||||
// Calculate error satellite rotational rate
|
|
||||||
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
|
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,
|
||||||
|
Loading…
Reference in New Issue
Block a user