mission/controller/acs/AttitudeEstimation.cpp

@@ -9,97 +9,8 @@ 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())) {
-    // Normalize Data
-    double normMgmB[3] = {0, 0, 0}, normMgmI[3] = {0, 0, 0}, normSusB[3] = {0, 0, 0},
-           normSusI[3] = {0, 0, 0};
-    VectorOperations<double>::normalize(susData->susVecTot.value, normMgmB, 3);
-    VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
-    VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
-    VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normSusI, 3);
-
-    // Create Helper Vectors
-    double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
-           helperSum[3] = {0, 0, 0};
-    VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
-    VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
-    VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
-    VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
-    VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
-    VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
-
-    // Sensor Weights
-    double kSus = 0, kMgm = 0;
-    kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
-    kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
-
-    // Weighted Vectors
-    double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
-           kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
-    VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
-    VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
-    VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
-    VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
-    VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
-
-    // Some weird Angles
-    double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
-                       (VectorOperations<double>::dot(weightedSusB, normSusI) +
-                        VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
-                   VectorOperations<double>::dot(helperCross, kSumVec);
-    double beta = VectorOperations<double>::dot(helperSum, kSumVec);
-    double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
-
-    // I don't even know what this is supposed to be
-    double constPlus =
-        1. / (2 * std::sqrt(gamma * (gamma + alpha) *
-                            (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
-    double constMinus =
-        1. / (2 * std::sqrt(gamma * (gamma - alpha) *
-                            (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
-
-    // Calculate Quaternion
-    double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
-           qRotVecPt1[3] = {0, 0, 0};
-    if (alpha >= 0) {
-      // Scalar Part
-      qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
-      // Rotational Vector Part
-      VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
-      VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
-      VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
-      VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
-      std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
-
-      VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
-      QuaternionOperations::normalize(qBI, qBI);
-    } else {
-      // Scalar Part
-      qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
-      // Rotational Vector Part
-      VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
-      VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
-      VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
-      VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
-      std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
-
-      VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
-      QuaternionOperations::normalize(qBI, qBI);
-
-      // Low Pass
-      if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
-        QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
-      }
-    }
-    {
-      PoolReadGuard pg{attitudeEstimation};
-      if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double));
-        attitudeEstimation->quatQuest.setValid(true);
-      }
-    }
-  } else {
     {
       PoolReadGuard pg{attitudeEstimation};
       if (pg.getReadResult() == returnvalue::OK) {
@@ -107,5 +18,95 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
         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};
+  VectorOperations<double>::normalize(susData->susVecTot.value, normMgmB, 3);
+  VectorOperations<double>::normalize(susData->sunIjkModel.value, normMgmI, 3);
+  VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normSusB, 3);
+  VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normSusI, 3);
+
+  // Create Helper Vectors
+  double normHelperB[3] = {0, 0, 0}, normHelperI[3] = {0, 0, 0}, helperCross[3] = {0, 0, 0},
+         helperSum[3] = {0, 0, 0};
+  VectorOperations<double>::cross(normSusB, normMgmB, normHelperB);
+  VectorOperations<double>::cross(normSusI, normMgmI, normHelperI);
+  VectorOperations<double>::normalize(normHelperB, normHelperB, 3);
+  VectorOperations<double>::normalize(normHelperI, normHelperI, 3);
+  VectorOperations<double>::cross(normHelperB, normHelperI, helperCross);
+  VectorOperations<double>::add(normHelperB, normHelperI, helperSum, 3);
+
+  // Sensor Weights
+  double kSus = 0, kMgm = 0;
+  kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
+  kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
+
+  // Weighted Vectors
+  double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},
+         kMgmVec[3] = {0, 0, 0}, kSumVec[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(normSusB, kSus, weightedSusB, 3);
+  VectorOperations<double>::mulScalar(normMgmB, kMgm, weightedMgmB, 3);
+  VectorOperations<double>::cross(weightedSusB, normSusI, kSusVec);
+  VectorOperations<double>::cross(weightedMgmB, normMgmI, kMgmVec);
+  VectorOperations<double>::add(kSusVec, kMgmVec, kSumVec, 3);
+
+  // Some weird Angles
+  double alpha = (1 + VectorOperations<double>::dot(normHelperB, normHelperI)) *
+                     (VectorOperations<double>::dot(weightedSusB, normSusI) +
+                      VectorOperations<double>::dot(weightedMgmB, normMgmI)) +
+                 VectorOperations<double>::dot(helperCross, kSumVec);
+  double beta = VectorOperations<double>::dot(helperSum, kSumVec);
+  double gamma = std::sqrt(std::pow(alpha, 2) + std::pow(beta, 2));
+
+  // I don't even know what this is supposed to be
+  double constPlus =
+      1. / (2 * std::sqrt(gamma * (gamma + alpha) *
+                          (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
+  double constMinus =
+      1. / (2 * std::sqrt(gamma * (gamma - alpha) *
+                          (1 + VectorOperations<double>::dot(normHelperB, normHelperI))));
+
+  // Calculate Quaternion
+  double qBI[4] = {0, 0, 0, 0}, qRotVecTot[3] = {0, 0, 0}, qRotVecPt0[3] = {0, 0, 0},
+         qRotVecPt1[3] = {0, 0, 0};
+  if (alpha >= 0) {
+    // Scalar Part
+    qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
+    // Rotational Vector Part
+    VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
+    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
+    VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
+    std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
+
+    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
+    QuaternionOperations::normalize(qBI, qBI);
+  } else {
+    // Scalar Part
+    qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
+    // Rotational Vector Part
+    VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
+    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
+    VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
+    std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
+
+    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
+    QuaternionOperations::normalize(qBI, qBI);
+
+    // Low Pass
+    if (VectorOperations<double>::norm(qOld, 4) != 0.0) {
+      QuaternionOperations::slerp(qBI, qOld, acsParameters->onBoardParams.questFilterWeight, qBI);
+    }
+  }
+  {
+    PoolReadGuard pg{attitudeEstimation};
+    if (pg.getReadResult() == returnvalue::OK) {
+      std::memcpy(attitudeEstimation->quatQuest.value, qBI, 4 * sizeof(double));
+      attitudeEstimation->quatQuest.setValid(true);
+    }
   }
 }

mission/controller/acs/FusedRotationEstimation.cpp

@@ -73,51 +73,52 @@ 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())) {
+          sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid())) {
-    double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
+    {
-                         sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
+      PoolReadGuard pg(fusedRotRateSourcesData);
-    if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
+      if (pg.getReadResult() == returnvalue::OK) {
-      double quatOldInv[4] = {0, 0, 0, 0};
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
-      double quatDelta[4] = {0, 0, 0, 0};
+        fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
-
-      QuaternionOperations::inverse(quatOldStr, quatOldInv);
-      QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
-      if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
-        QuaternionOperations::normalize(quatDelta);
       }
+    }
+    std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
+    return;
+  }
 
-      double rotVec[3] = {0, 0, 0};
+  double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
-      double angle = QuaternionOperations::getAngle(quatDelta);
+                       sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
-      if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
+  if (VectorOperations<double>::norm(quatOldStr, 4) != 0 and timeDelta != 0) {
-        {
+    double quatOldInv[4] = {0, 0, 0, 0};
-          PoolReadGuard pg(fusedRotRateSourcesData);
+    double quatDelta[4] = {0, 0, 0, 0};
-          if (pg.getReadResult() == returnvalue::OK) {
+
-            std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
+    QuaternionOperations::inverse(quatOldStr, quatOldInv);
-                        3 * sizeof(double));
+    QuaternionOperations::multiply(quatNew, quatOldInv, quatDelta);
-            fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
+    if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
-          }
+      QuaternionOperations::normalize(quatDelta);
-        }
+    }
-        std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
+
-        return;
+    double rotVec[3] = {0, 0, 0};
-      }
+    double angle = QuaternionOperations::getAngle(quatDelta);
-      VectorOperations<double>::normalize(quatDelta, rotVec, 3);
+    if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
-      VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotVec, 3);
       {
         PoolReadGuard pg(fusedRotRateSourcesData);
         if (pg.getReadResult() == returnvalue::OK) {
-          std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
+          std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3,
+                      3 * sizeof(double));
           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);
     {
       PoolReadGuard pg(fusedRotRateSourcesData);
       if (pg.getReadResult() == returnvalue::OK) {
-        std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, ZERO_VEC3, 3 * sizeof(double));
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalStr.value, rotVec, 3 * sizeof(double));
-        fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
+        fusedRotRateSourcesData->rotRateTotalStr.setValid(true);
       }
     }
     std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
@@ -130,50 +131,14 @@ void FusedRotationEstimation::estimateFusedRotationRateStr(
       fusedRotRateSourcesData->rotRateTotalStr.setValid(false);
     }
   }
-  std::memcpy(quatOldStr, ZERO_VEC4, sizeof(quatOldStr));
+  std::memcpy(quatOldStr, quatNew, sizeof(quatOldStr));
+  return;
 }
 
 void FusedRotationEstimation::estimateFusedRotationRateQuest(
     acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta,
     acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData) {
-  if (attitudeEstimationData->quatQuest.isValid()) {
+  if (not attitudeEstimationData->quatQuest.isValid()) {
-    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,
-                                     quatDelta);
-      if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
-        QuaternionOperations::normalize(quatDelta);
-      }
-
-      double rotVec[3] = {0, 0, 0};
-      double angle = QuaternionOperations::getAngle(quatDelta);
-      if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
-        {
-          PoolReadGuard pg(fusedRotRateSourcesData);
-          if (pg.getReadResult() == returnvalue::OK) {
-            std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
-                        3 * sizeof(double));
-            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);
-      {
-        PoolReadGuard pg(fusedRotRateSourcesData);
-        if (pg.getReadResult() == returnvalue::OK) {
-          std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
-          fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
-        }
-      }
-      std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
-      return;
-    }
     {
       PoolReadGuard pg(fusedRotRateSourcesData);
       if (pg.getReadResult() == returnvalue::OK) {
@@ -182,6 +147,42 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
         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, quatDelta);
+    if (VectorOperations<double>::norm(quatDelta, 4) != 0.0) {
+      QuaternionOperations::normalize(quatDelta);
+    }
+
+    double rotVec[3] = {0, 0, 0};
+    double angle = QuaternionOperations::getAngle(quatDelta);
+    if (VectorOperations<double>::norm(quatDelta, 3) == 0.0) {
+      {
+        PoolReadGuard pg(fusedRotRateSourcesData);
+        if (pg.getReadResult() == returnvalue::OK) {
+          std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, ZERO_VEC3,
+                      3 * sizeof(double));
+          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);
+    {
+      PoolReadGuard pg(fusedRotRateSourcesData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        std::memcpy(fusedRotRateSourcesData->rotRateTotalQuest.value, rotVec, 3 * sizeof(double));
+        fusedRotRateSourcesData->rotRateTotalQuest.setValid(true);
+      }
+    }
     std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
     return;
   }
@@ -192,7 +193,8 @@ void FusedRotationEstimation::estimateFusedRotationRateQuest(
       fusedRotRateSourcesData->rotRateTotalQuest.setValid(false);
     }
   }
-  std::memcpy(quatOldQuest, ZERO_VEC4, sizeof(quatOldQuest));
+  std::memcpy(quatOldQuest, attitudeEstimationData->quatQuest.value, sizeof(quatOldQuest));
+  return;
 }
 
 void FusedRotationEstimation::estimateFusedRotationRateSusMgm(