From 141f82d0a9c86913a9479862a26afdeef8cd9f11 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 1 Mar 2024 15:27:13 +0100
Subject: [PATCH] prevent ctrl values from becoming NaN

---
 mission/controller/acs/Guidance.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp
index f9182cbd..0a70e003 100644
--- a/mission/controller/acs/Guidance.cpp
+++ b/mission/controller/acs/Guidance.cpp
@@ -273,7 +273,11 @@ void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4])
   QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde);
 
   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));
   VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);