From dab10596f6a9935d07b1fd030514344c59419fc5 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 5 Dec 2023 15:02:17 +0100
Subject: [PATCH] fixes

---
 .../acs/FusedRotationEstimation.cpp           | 30 ++++++++++++-------
 1 file changed, 19 insertions(+), 11 deletions(-)

diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp
index c936bc16..c2b69746 100644
--- a/mission/controller/acs/FusedRotationEstimation.cpp
+++ b/mission/controller/acs/FusedRotationEstimation.cpp
@@ -23,9 +23,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
       fusedRotRateData->rotRateOrthogonal.setValid(false);
       std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
       fusedRotRateData->rotRateParallel.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
+      std::memcpy(fusedRotRateData->rotRateTotal.value,
                   fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double));
-      fusedRotRateData->rotRateOrthogonal.setValid(true);
+      fusedRotRateData->rotRateTotal.setValid(true);
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR;
       fusedRotRateData->rotRateSource.setValid(true);
     }
@@ -37,9 +37,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
       fusedRotRateData->rotRateOrthogonal.setValid(false);
       std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
       fusedRotRateData->rotRateParallel.setValid(false);
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
+      std::memcpy(fusedRotRateData->rotRateTotal.value,
                   fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double));
-      fusedRotRateData->rotRateOrthogonal.setValid(true);
+      fusedRotRateData->rotRateTotal.setValid(true);
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::QUEST;
       fusedRotRateData->rotRateSource.setValid(true);
     }
@@ -52,9 +52,9 @@ void FusedRotationEstimation::estimateFusedRotationRate(
                 fusedRotRateSourcesData->rotRateParallelSusMgm.value, 3 * sizeof(double));
     fusedRotRateData->rotRateParallel.setValid(
         fusedRotRateSourcesData->rotRateParallelSusMgm.isValid());
-    std::memcpy(fusedRotRateData->rotRateOrthogonal.value,
+    std::memcpy(fusedRotRateData->rotRateTotal.value,
                 fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double));
-    fusedRotRateData->rotRateOrthogonal.setValid(true);
+    fusedRotRateData->rotRateTotal.setValid(true);
     fusedRotRateData->rotRateSource.value = acs::rotrate::Source::SUSMGM;
     fusedRotRateData->rotRateSource.setValid(true);
   } else {
@@ -62,7 +62,7 @@ void FusedRotationEstimation::estimateFusedRotationRate(
     if (pg.getReadResult() == returnvalue::OK) {
       std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
       std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double));
-      std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double));
+      std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double));
       fusedRotRateData->rotRateSource.value = acs::rotrate::Source::NONE;
       fusedRotRateData->setValidity(false, true);
     }
@@ -82,11 +82,13 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
 
       QuaternionOperations::inverse(quatOldStr, quatOldInv);
       QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
-      QuaternionOperations::normalize(quatDelta);
+      if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
+        QuaternionOperations::normalize(quatDelta);
+      }
 
       double rotVec[3] = {0, 0, 0};
       double angle = QuaternionOperations::getAngle(quatDelta);
-      if (angle == 0.0) {
+      if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
         {
           PoolReadGuard pg(fusedRotRateSourcesData);
           if (pg.getReadResult() == returnvalue::OK) {
@@ -95,6 +97,8 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
             fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
           }
         }
+        std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
+        return;
       }
       VectorOperations<double>::normalize(quatDelta, rotVec, 3);
       VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
@@ -139,11 +143,13 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
       QuaternionOperations::inverse(quatOldQuest, quatOldInv);
       QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv,
                                      quatDelta);
-      QuaternionOperations::normalize(quatDelta);
+      if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
+        QuaternionOperations::normalize(quatDelta);
+      }
 
       double rotVec[3] = {0, 0, 0};
       double angle = QuaternionOperations::getAngle(quatDelta);
-      if (angle == 0.0) {
+      if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
         {
           PoolReadGuard pg(fusedRotRateSourcesData);
           if (pg.getReadResult() == returnvalue::OK) {
@@ -152,6 +158,8 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
             fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
           }
         }
+        std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
+        return;
       }
       VectorOperations<double>::normalize(quatDelta, rotVec, 3);
       VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);