maybe this makes him happy

mission/controller/acs/AttitudeEstimation.cpp

@@ -9,8 +9,18 @@ AttitudeEstimation::~AttitudeEstimation() {}
 void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
                                acsctrl::MgmDataProcessed *mgmData,
                                acsctrl::AttitudeEstimationData *attitudeEstimation) {
-  if (susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
+  if (not(susData->susVecTot.isValid() and susData->sunIjkModel.isValid() and
-      mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid()) {
+          mgmData->mgmVecTot.value and mgmData->magIgrfModel.isValid())) {
+    {
+      PoolReadGuard pg{attitudeEstimation};
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
+        attitudeEstimation->quatQuest.setValid(false);
+      }
+    }
+    return;
+  }
+
   // Normalize Data
   double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
          normSusI[3] = {0, 0, 0};
@@ -99,13 +109,4 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
       attitudeEstimation->quatQuest.setValid(true);
     }
   }
-  } else {
-    {
-      PoolReadGuard pg{attitudeEstimation};
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(attitudeEstimation->quatQuest.value, ZERO_VEC4, 4 * sizeof(double));
-        attitudeEstimation->quatQuest.setValid(false);
-      }
-    }
-  }
 }

mission/controller/acs/FusedRotationEstimation.cpp

@@ -73,8 +73,19 @@ void FusedRotationEstimation::estimateFusedRotationRate(
 void FusedRotationEstimation::estimateFusedRotationRateStr(
     ACS::SensorValues *sensorValues, const double timeDelta,
     acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
-  if ((sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
+  if (not(sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and
           sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
+    {
+      PoolReadGuard pg(fusedRotRateSourcesData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
+      }
+    }
+    std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
+    return;
+  }
+
   double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                        sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
   if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
@@ -123,27 +134,28 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
   std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
   return;
 }
-  {
-    PoolReadGuard pg(fusedRotRateSourcesData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
-    }
-  }
-  std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
-}
 
 void FusedRotationEstimation::estimateFusedRotationRateQuest(
     acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
     acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
-  if (attitudeEstimationData->quatQuest.isValid()) {
+  if (not attitudeEstimationData->quatQuest.isValid()) {
+    {
+      PoolReadGuard pg(fusedRotRateSourcesData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
+                    3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
+      }
+    }
+    std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
+  }
+
   if (VectorOperations<double>::norm(quatOldQuest, 4) != 0 and timeDelta != 0) {
     double quatOldInv[4] = {0, 0, 0, 0};
     double quatDelta[4] = {0, 0, 0, 0};
 
     QuaternionOperations::inverse(quatOldQuest, quatOldInv);
-      QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv,
+    QuaternionOperations::multiply(attitudeEstimationData->quatQuest.value, quatOldInv, quatDelta);
-                                     quatDelta);
     if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
       QuaternionOperations::normalize(quatDelta);
     }
@@ -177,23 +189,13 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
   {
     PoolReadGuard pg(fusedRotRateSourcesData);
     if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
+      std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
-                    3 * sizeof(double));
       fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
     }
   }
   std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
   return;
 }
-  {
-    PoolReadGuard pg(fusedRotRateSourcesData);
-    if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3, 3 * sizeof(double));
-      fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
-    }
-  }
-  std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
-}
 
 void FusedRotationEstimation::estimateFusedRotationRateSusMgm(
     acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed,