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

Fix NaN for Limiting Rotation Rates #872

Merged
meggert merged 17 commits from limit-rot-rate-fix into main 2024-03-06 11:13:02 +01:00
Conversation 0 Commits 17 Files Changed 12 +5 -1
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 141f82d0a9 - Show all commits

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

@ -273,7 +273,11 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde); QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde);
double phiResidual = 0, phiResidualVec[3] = {0, 0, 0}; double phiResidual = 0, phiResidualVec[3] = {0, 0, 0};
phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX)); if ((phiX * phiX) > (phiMax * phiMax)) {
phiResidual = 0;
} else {
phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX));
}
std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec)); std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec));
VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3); VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);