shameless copy of FLP code

mission/controller/acs/control/SafeCtrl.cpp

@@ -4,6 +4,7 @@
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
+#include <fsfw/globalfunctions/sign.h>
 #include <math.h>
 
 #include "../util/MathOperations.h"
@@ -70,89 +71,72 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
   return returnvalue::OK;
 }
 
-// Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
-ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid,
-                                   double *sunRateB, bool sunRateBValid, double *magFieldB,
-                                   bool magFieldBValid, double *magRateB, bool magRateBValid,
-                                   double *sunDirRef, double *satRateRef, double *outputAngle,
-                                   double *outputMagMomB) {
-  // Check for invalid Inputs
-  if (!susDirBValid || !magFieldBValid || !magRateBValid) {
+// Will be the version in worst case scenario in event of no working MEKF
+ReturnValue_t SafeCtrl::safeNoMekf(const double *magneticFieldVector,
+                                   const double *magneticFieldVectorDerivative,
+                                   const double *sunVector, const double *sunvectorDerivative,
+                                   double omegaRef, double *torqueCommand, double *spinAxis) {
+  if (0) {
     return returnvalue::FAILED;
   }
 
-  // change unit from uT to T
-  double magFieldBT[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
+  double magneticFieldVectorT[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(magneticFieldVector, 1e-6, magneticFieldVectorT, 3);
 
-  // normalize sunDir and magDir
-  double magDirB[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
-  VectorOperations<double>::normalize(susDirB, susDirB, 3);
+  double commandParallel[3] = {0, 0, 0}, commandAlign[3] = {0, 0, 0}, commandOrtho[3] = {0, 0, 0};
 
-  // Cosinus angle between sunDir and magDir
-  double cosAngleSunMag = VectorOperations<double>::dot(magDirB, susDirB);
+  bool valid;
+  double omega = estimateRotationAroundSun(magneticFieldVector, magneticFieldVectorDerivative,
+                                           sunVector, &valid);
 
-  // Rate parallel to sun direction and magnetic field direction
-  double dotSunRateMag = VectorOperations<double>::dot(sunRateB, magDirB);
-  double dotmagRateSun = VectorOperations<double>::dot(magRateB, susDirB);
-  double rateFactor = 1 - pow(cosAngleSunMag, 2);
-  double rateParaSun = (dotmagRateSun + cosAngleSunMag * dotSunRateMag) / rateFactor;
-  double rateParaMag = (dotSunRateMag + cosAngleSunMag * dotmagRateSun) / rateFactor;
-
-  // Full rate or estimate
-  double estSatRate[3] = {0, 0, 0};
-  double estSatRateMag[3] = {0, 0, 0}, estSatRateSun[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(susDirB, rateParaSun, estSatRateSun, 3);
-  VectorOperations<double>::add(sunRateB, estSatRateSun, estSatRateSun, 3);
-  VectorOperations<double>::mulScalar(magDirB, rateParaMag, estSatRateMag, 3);
-  VectorOperations<double>::add(magRateB, estSatRateMag, estSatRateMag, 3);
-  VectorOperations<double>::add(estSatRateSun, estSatRateMag, estSatRate, 3);
-  VectorOperations<double>::mulScalar(estSatRate, 0.5, estSatRate, 3);
-
-  /* Only valid if angle between sun direction and magnetic field direction
-   * is sufficiently large */
-  double angleSunMag = acos(cosAngleSunMag);
-  if (angleSunMag < acsParameters->safeModeControllerParameters.sunMagAngleMin) {
-    return returnvalue::FAILED;
+  if (valid) {
+    VectorOperations<double>::mulScalar(
+        sunVector,
+        acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega),
+        commandParallel, 3);
+    omega = omega * sign<double>(omega);
   }
 
-  // Rate for Torque Calculation
-  double diffRate[3] = {0, 0, 0}; /* ADD TO MONITORING */
-  VectorOperations<double>::subtract(estSatRate, satRateRef, diffRate, 3);
+  VectorOperations<double>::cross(spinAxis, sunVector, commandAlign);
 
-  // Torque Align calculation
-  double kRateNoMekf = acsParameters->safeModeControllerParameters.k_rate_no_mekf;
-  double kAlignNoMekf = acsParameters->safeModeControllerParameters.k_align_no_mekf;
+  VectorOperations<double>::mulScalar(
+      commandAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, commandAlign, 3);
 
-  double cosAngleAlignErr = VectorOperations<double>::dot(sunDirRef, susDirB);
-  double crossSusSunRef[3] = {0, 0, 0};
-  VectorOperations<double>::cross(sunDirRef, susDirB, crossSusSunRef);
-  double sinAngleAlignErr = VectorOperations<double>::norm(crossSusSunRef, 3);
+  VectorOperations<double>::cross(sunvectorDerivative, sunVector, commandOrtho);
+  VectorOperations<double>::mulScalar(
+      commandOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, commandOrtho, 3);
 
-  double torqueAlign[3] = {0, 0, 0};
-  double angleAlignErr = acos(cosAngleAlignErr);
-  double torqueAlignFactor = kAlignNoMekf * angleAlignErr / sinAngleAlignErr;
-  VectorOperations<double>::mulScalar(crossSusSunRef, torqueAlignFactor, torqueAlign, 3);
+  // only spin up when the angle to the sun is less than a certain angle.
+  // note that we check the cosin, thus "<"
+  if (VectorOperations<double>::dot(spinAxis, sunVector) <
+      acsParameters->safeModeControllerParameters.cosineStartSpin) {
+    VectorOperations<double>::mulScalar(commandParallel, 0, commandParallel, 3);
+  }
 
-  // Torque Rate Calculations
-  double torqueRate[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(diffRate, -kRateNoMekf, torqueRate, 3);
-
-  // Final torque
-  double torqueB[3] = {0, 0, 0}, torqueAlignRate[3] = {0, 0, 0};
-  VectorOperations<double>::add(torqueRate, torqueAlign, torqueAlignRate, 3);
-  MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrix), torqueAlignRate,
-                                     torqueB, 3, 3, 1);
-
-  // Magnetic moment
-  double magMomB[3] = {0, 0, 0};
-  double crossMagFieldTorque[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
-  double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
-  VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
-
-  std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));
-  *outputAngle = angleAlignErr;
+  for (uint8_t i = 0; i < 3; i++) {
+    torqueCommand[i] = commandAlign[i] + commandOrtho[i] + commandParallel[i];
+  }
   return returnvalue::OK;
 }
+
+double SafeCtrl::estimateRotationAroundSun(const double *magneticFieldVector,
+                                           const double *magneticFieldVectorDerivative,
+                                           const double *sunVector, bool *updated) {
+  *updated = true;
+  double vector[3];
+  VectorOperations<double>::cross(magneticFieldVector, sunVector, vector);
+
+  float magLength = VectorOperations<double>::norm(magneticFieldVector, 3);
+  float length = VectorOperations<double>::norm(vector, 3);
+
+  // only check if angle between B and sun is large enough
+  if (length > (acsParameters->safeModeControllerParameters.sineCalculateOmegaSun * magLength)) {
+    float omega = VectorOperations<double>::dot(magneticFieldVectorDerivative, vector);
+    omega = omega / (length * length);
+    lastCalculatedOmega = omega;
+    return omega;
+  } else {
+    *updated = false;
+    return lastCalculatedOmega;
+  }
+}

mission/controller/acs/control/SafeCtrl.h

@@ -23,10 +23,14 @@ class SafeCtrl {
                          double *satRatRef,  // From Guidance (!)
                          double *outputAngle, double *outputMagMomB);
 
-  ReturnValue_t safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
-                           bool sunRateBValid, double *magFieldB, bool magFieldBValid,
-                           double *magRateB, bool magRateBValid, double *sunDirRef,
-                           double *satRateRef, double *outputAngle, double *outputMagMomB);
+  ReturnValue_t safeNoMekf(const double *magneticFieldVector,
+                           const double *magneticFieldVectorDerivative, const double *sunVector,
+                           const double *sunvectorDerivative, double omegaRef,
+                           double *torqueCommand, double *spinAxis);
+
+  double estimateRotationAroundSun(const double *magneticFieldVector,
+                                   const double *magneticFieldVectorDerivative,
+                                   const double *sunvector, bool *updated);
 
  protected:
  private:
@@ -35,6 +39,8 @@ class SafeCtrl {
 
   double magFieldBState[3];
   timeval magFieldBStateTime;
+
+  float lastCalculatedOmega = 0.0;
 };
 
 #endif /* ACS_CONTROL_SAFECTRL_H_ */