eive/eive-obsw
eive
/
eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 117 Wiki Activity

overloading is fun (but should be done right)
EIVE/eive-obsw/pipeline/head This commit looks good Details
EIVE/eive-obsw/pipeline/pr-main This commit looks good Details

This commit is contained in:
Marius Eggert 2024-02-02 12:38:16 +01:00
parent aa521e89f6
commit 8be94cf2dc
1 changed files with 4 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -408,9 +408,7 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
// First calculate error quaternion between current and target orientation
double invTargetQuat[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(targetQuat, invTargetQuat);
QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat);
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
// Last calculate add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
// 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],
double targetSatRotRate[3], double errorQuat[4],
double errorSatRotRate[3], double &errorAngle) {
// First calculate error quaternion between current and target orientation
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
// Keep scalar part of quaternion positive
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);
double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0};
comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate,
errorQuat, errorSatRotRate, errorAngle);
}
void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double timeDelta,