new class YAY

mission/controller/acs/CMakeLists.txt

@@ -2,6 +2,7 @@ target_sources(
   ${LIB_EIVE_MISSION}
   PRIVATE AcsParameters.cpp
           ActuatorCmd.cpp
+          FusedRotationEstimation.cpp
           Guidance.cpp
           Igrf13Model.cpp
           MultiplicativeKalmanFilter.cpp

mission/controller/acs/FusedRotationEstimation.cpp

@@ -0,0 +1,68 @@
+#include "FusedRotationEstimation.h"
+
+FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) {
+  acsParameters = acsParameters_;
+}
+
+void FusedRotationEstimation::estimateFusedRotationRateSafe(
+    bool sunValid, double sunB[3], double sunRateB[3], bool magValid, double magB[3],
+    double magRateB[3], bool rotRateValid, double rotRateB[3], double fusedRateB[3],
+    double fusedRateParallelB[3], double fusedRateOrthogonalB[3]) {
+  if ((not magValid) or (not sunValid and not VectorOperations<double>::norm(fusedRateOldB, 3)) or
+      ((not VectorOperations<double>::norm(sunRateB, 3) and
+        not VectorOperations<double>::norm(magRateB, 3)))) {
+    return;
+  }
+  if (not sunValid) {
+    estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
+                                     fusedRateOrthogonalB);  // ToDo
+    return;
+  }
+
+  // calculate rotation around the sun
+  double magSunCross[3] = {0, 0, 0};
+
+  VectorOperations<double>::cross(magB, sunB, magSunCross);
+  double magSunCrossNorm = VectorOperations<double>::norm(magSunCross, 3);
+  double magNorm = VectorOperations<double>::norm(magB, 3);
+  if (magSunCrossNorm >
+      (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) {
+    double omegaParallel =
+        VectorOperations<double>::dot(magRateB, magSunCross) * pow(magSunCrossNorm, -2);
+    double omegaParallelVec[3] = {0, 0, 0};
+    VectorOperations<double>::mulScalar(sunB, omegaParallel, omegaParallelVec, 3);
+    std::memcpy(fusedRateParallelB, omegaParallelVec, 3 * sizeof(double));  // to dataset
+  } else {
+    estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB,
+                                     fusedRateOrthogonalB);  // ToDo
+    return;
+  }
+
+  // calculate rotation orthogonal to the sun
+  VectorOperations<double>::cross(sunRateB, sunB, fusedRateOrthogonalB);
+  VectorOperations<double>::mulScalar(fusedRateOrthogonalB,
+                                      pow(VectorOperations<double>::norm(sunB, 2), 2),
+                                      fusedRateOrthogonalB, 3);
+
+  // calculate total rotation rate
+  VectorOperations<double>::add(fusedRateParallelB, fusedRateOrthogonalB, fusedRateB);
+
+  std::memcpy(fusedRateOldB, fusedRateB, 3 * sizeof(double));
+  if (rotRateValid) {
+    std::memcpy(rotRateOldB, rotRateB, 3 * sizeof(double));
+  }
+}
+
+void FusedRotationEstimation::estimateFusedRotationRateEclipse(bool rotRateValid,
+                                                               double rotRateB[3],
+                                                               double fusedRateB[3],
+                                                               double fusedRateParallelB[3],
+                                                               double fusedRateOrthogonalB[3]) {
+  if (not rotRateValid or not VectorOperations<double>::norm(fusedRateOldB, 3)) {
+    return;
+  }
+  double angAccelB[3] = {0, 0, 0};
+  VectorOperations<double>::subtract(rotRateB, rotRateOldB, angAccelB, 3);
+  double omegaTotVec[3] = {0, 0, 0};
+  VectorOperations<double>::add(fusedRateOldB, angAccelB, fusedRateB, 3);
+}

mission/controller/acs/FusedRotationEstimation.h

@@ -0,0 +1,26 @@
+#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
+#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_
+
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <mission/controller/acs/AcsParameters.h>
+
+class FusedRotationEstimation {
+ public:
+  FusedRotationEstimation(AcsParameters *acsParameters_);
+
+  void estimateFusedRotationRateSafe(bool sunValid, double sunB[3], double sunRateB[3],
+                                     bool magValid, double magB[3], double magRateB[3],
+                                     bool rotRateValid, double rotRateB[3], double fusedRateB[3],
+                                     double fusedRateParallelB[3], double fusedRateOrthogonalB[3]);
+  void estimateFusedRotationRateEclipse(bool rotRateValid, double rotRateB[3], double fusedRateB[3],
+                                        double fusedRateParallelB[3],
+                                        double fusedRateOrthogonalB[3]);
+
+ protected:
+ private:
+  AcsParameters *acsParameters;
+  double rotRateOldB[3] = {0, 0, 0};
+  double fusedRateOldB[3] = {0, 0, 0};
+};
+
+#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */