nearly there

mission/controller/AcsController.cpp

@@ -174,7 +174,8 @@ void AcsController::performSafe() {
                                sunTargetDir, satRateSafe, &errAng, magMomMtq);
   } else {
     result = safeCtrl.safeNoMekf(
-        now, susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
+        mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
+        susDataProcessed.susVecTot.value, susDataProcessed.susVecTot.isValid(),
         susDataProcessed.susVecTotDerivative.value, susDataProcessed.susVecTotDerivative.isValid(),
         mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(),
         mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(),

mission/controller/acs/control/SafeCtrl.cpp

@@ -13,6 +13,21 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
 
 SafeCtrl::~SafeCtrl() {}
 
+ReturnValue_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
+                                         const bool satRotRateValid, const bool sunDirValid) {
+  if (not magFieldValid) {
+    return SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
+  } else if (mekfValid) {
+    return SAFECTRL_USE_MEKF;
+  } else if (satRotRateValid and sunDirValid) {
+    return SAFECTRL_USE_NONMEKF;
+  } else if (satRotRateValid and not sunDirValid) {
+    return SAFECTRL_USE_DAMPING;
+  } else {
+    return SAFECTRL_NO_SENSORS_FOR_CONTROL;
+  }
+}
+
 ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
                                  double *magFieldModel, bool magFieldModelValid,
                                  double *sunDirModel, bool sunDirModelValid, double *satRateMekf,
@@ -71,81 +86,56 @@ 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
+void SafeCtrl::safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
-ReturnValue_t SafeCtrl::safeNoMekf(const double *magFieldB,
+                          const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
-                                   const double *magneticFieldVectorDerivative,
+                          double &errorAngle) {
-                                   const double *sunVector, const double *sunvectorDerivative,
+  // convert magFieldB from uT to T
-                                   double omegaRef, double *magMomB, double *spinAxis) {
-  if (0) {
-    return returnvalue::FAILED;
-  }
-
   double magFieldBT[3] = {0, 0, 0};
   VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
-  double cmdParallel[3] = {0, 0, 0}, cmdAlign[3] = {0, 0, 0}, cmdOrtho[3] = {0, 0, 0},
+  // calculate angle alpha between sunDirRef and sunDir
-         torqueCmd[3] = {0, 0, 0};
+  double dotSun = VectorOperations<double>::dot(sunDirRefB, sunDirB);
+  errorAngle = acos(dotSun);
 
-  bool valid;
+  // split rotational rate into parallel and orthogonal parts
-  double omega =
+  double satRotRateParallelB[3] = {0, 0, 0}, satRotRateOrthogonalB[3] = {0, 0, 0};
-      estimateRotationAroundSun(magFieldB, magneticFieldVectorDerivative, sunVector, &valid);
+  double parallelLength = VectorOperations<double>::dot(satRotRateB, sunDirB) *
+                          pow(VectorOperations<double>::norm(sunDirB, 3), -2);
+  VectorOperations<double>::mulScalar(sunDirB, parallelLength, satRotRateParallelB, 3);
+  VectorOperations<double>::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3);
 
-  if (valid) {
+  // calculate torque for parallel rotational rate
+  double cmdParallel[3] = {0, 0, 0};
+  if (errorAngle < (double)acsParameters->safeModeControllerParameters.angleStartSpin) {
+    VectorOperations<double>::subtract(satRotRateRefB, satRotRateParallelB, cmdParallel, 3);
     VectorOperations<double>::mulScalar(
-        sunVector,
+        cmdParallel, acsParameters->safeModeControllerParameters.k_parallel_mekf, cmdParallel, 3);
-        acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega),
-        cmdParallel, 3);
-    omega = omega * sign<double>(omega);
   }
 
-  VectorOperations<double>::cross(spinAxis, sunVector, cmdAlign);
+  // calculate torque for orthogonal rotational rate
+  double cmdOrtho[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(satRotRateOrthogonalB,
+                                      -acsParameters->safeModeControllerParameters.k_ortho_mekf,
+                                      cmdOrtho, 3);
 
-  VectorOperations<double>::mulScalar(
+  // calculate torque for alignment
-      cmdAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, cmdAlign, 3);
+  double cmdAlign[3] = {0, 0, 0}, crossAlign[3] = {0, 0, 0},
-
+         alignFactor[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  VectorOperations<double>::cross(sunvectorDerivative, sunVector, cmdOrtho);
+  MatrixOperations<double>::multiplyScalar(*acsParameters->inertiaEIVE.inertiaMatrix,
-  VectorOperations<double>::mulScalar(
+                                           acsParameters->safeModeControllerParameters.k_align_mekf,
-      cmdOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, cmdOrtho, 3);
+                                           *alignFactor, 3, 3);
-
+  VectorOperations<double>::cross(sunDirRefB, sunDirB, crossAlign);
-  // only spin up when the angle to the sun is less than a certain angle.
+  MatrixOperations<double>::multiply(*alignFactor, *crossAlign, *cmdAlign, 3, 3, 1);
-  // note that we check the cosin, thus "<"
-  if (VectorOperations<double>::dot(spinAxis, sunVector) <
-      acsParameters->safeModeControllerParameters.cosineStartSpin) {
-    VectorOperations<double>::mulScalar(cmdParallel, 0, cmdParallel, 3);
-  }
 
+  // sum of all torques
+  double cmdTorque[3] = {0, 0, 0};
   for (uint8_t i = 0; i < 3; i++) {
-    torqueCmd[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
+    cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
   }
 
   // MagMom B (orthogonal torque)
   double torqueMgt[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldBT, torqueCmd, torqueMgt);
+  VectorOperations<double>::cross(magFieldBT, cmdTorque, torqueMgt);
   double normMag = VectorOperations<double>::norm(magFieldB, 3);
-  VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), magMomB, 3);
+  VectorOperations<double>::mulScalar(torqueMgt, pow(normMag, -2), magMomB, 3);
-
-  *outputAngle = angleAlignErr;
-  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

@@ -1,21 +1,29 @@
 #ifndef SAFECTRL_H_
 #define SAFECTRL_H_
 
+#include <eive/resultClassIds.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/SensorValues.h>
 #include <stdio.h>
 #include <string.h>
 #include <time.h>
 
-#include "../AcsParameters.h"
-#include "../SensorValues.h"
-#include "eive/resultClassIds.h"
-
 class SafeCtrl {
  public:
   SafeCtrl(AcsParameters *acsParameters_);
   virtual ~SafeCtrl();
 
-  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_SAFE;
+  static constexpr uint8_t IF_SAFE_ID = CLASS_ID::ACS_SAFE;
-  static const ReturnValue_t SAFECTRL_MEKF_INPUT_INVALID = MAKE_RETURN_CODE(0x01);
+  static constexpr ReturnValue_t SAFECTRL_NO_MAG_FIELD_FOR_CONTROL =
+      returnvalue::makeCode(IF_SAFE_ID, 2);
+  static constexpr ReturnValue_t SAFECTRL_USE_MEKF = returnvalue::makeCode(IF_SAFE_ID, 3);
+  static constexpr ReturnValue_t SAFECTRL_USE_NONMEKF = returnvalue::makeCode(IF_SAFE_ID, 4);
+  static constexpr ReturnValue_t SAFECTRL_USE_DAMPING = returnvalue::makeCode(IF_SAFE_ID, 5);
+  static constexpr ReturnValue_t SAFECTRL_NO_SENSORS_FOR_CONTROL =
+      returnvalue::makeCode(IF_SAFE_ID, 6);
+
+  ReturnValue_t safeCtrlStrategy(const bool magFieldValid, const ReturnValue_t mekfValid,
+                                 const bool satRotRateValid, const bool sunDirValid);
 
   ReturnValue_t safeMekf(timeval now, double *quatBJ, bool quatBJValid, double *magFieldModel,
                          bool magFieldModelValid, double *sunDirModel, bool sunDirModelValid,
@@ -23,24 +31,13 @@ class SafeCtrl {
                          double *satRatRef,  // From Guidance (!)
                          double *outputAngle, double *outputMagMomB);
 
-  ReturnValue_t safeNoMekf(const double *magneticFieldVector,
+  void safeNoMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB,
-                           const double *magneticFieldVectorDerivative, const double *sunVector,
+                  const double *sunDirRefB, const double *satRotRateRefB, double *magMomB,
-                           const double *sunvectorDerivative, double omegaRef,
+                  double &errorAngle);
-                           double *torqueCommand, double *spinAxis);
-
-  double estimateRotationAroundSun(const double *magneticFieldVector,
-                                   const double *magneticFieldVectorDerivative,
-                                   const double *sunvector, bool *updated);
 
  protected:
  private:
   AcsParameters *acsParameters;
-  double gainMatrixInertia[3][3];
-
-  double magFieldBState[3];
-  timeval magFieldBStateTime;
-
-  float lastCalculatedOmega = 0.0;
 };
 
 #endif /* ACS_CONTROL_SAFECTRL_H_ */