Init Commit

2020-09-16 10:33:06 +02:00
commit 7852b88e6a
821 changed files with 115954 additions and 0 deletions
@@ -0,0 +1,47 @@
+#include "Detumble.h"
+
+#include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <math.h>
+
+Detumble::Detumble() {}
+
+Detumble::~Detumble() {}
+
+acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
+                                                    const bool satRotRateValid,
+                                                    const bool magFieldRateValid,
+                                                    const bool useFullDetumbleLaw) {
+  if (not magFieldValid) {
+    return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL;
+  } else if (satRotRateValid and useFullDetumbleLaw) {
+    return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL;
+  } else if (magFieldRateValid) {
+    return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
+  } else {
+    return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
+  }
+}
+
+void Detumble::bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
+                           double gain) {
+  // convert uT to T
+  double magFieldBT[3];
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
+  // control law
+  double factor = gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
+  double magFieldNormed[3] = {0, 0, 0}, crossProduct[3] = {0, 0, 0};
+  VectorOperations<double>::normalize(magFieldBT, magFieldNormed, 3);
+  VectorOperations<double>::cross(satRotRateB, magFieldNormed, crossProduct);
+  VectorOperations<double>::mulScalar(crossProduct, factor, magMomB, 3);
+}
+
+void Detumble::bDotLaw(const double *magRateB, const double *magFieldB, double *magMomB,
+                       double gain) {
+  // convert uT to T
+  double magFieldBT[3], magRateBT[3];
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
+  VectorOperations<double>::mulScalar(magRateB, 1e-6, magRateBT, 3);
+  // control law
+  double factor = -gain / pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
+  VectorOperations<double>::mulScalar(magRateBT, factor, magMomB, 3);
+}