From 56642a11f7826429df6130bf584382b83f1386df Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 22 Mar 2023 08:59:29 +0100
Subject: [PATCH] frmt

---
 mission/controller/acs/control/SafeCtrl.cpp | 37 +++++++++++++--------
 1 file changed, 23 insertions(+), 14 deletions(-)

diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp
index 9336fa8a..969e4447 100644
--- a/mission/controller/acs/control/SafeCtrl.cpp
+++ b/mission/controller/acs/control/SafeCtrl.cpp
@@ -72,50 +72,59 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
 }
 
 // Will be the version in worst case scenario in event of no working MEKF
-ReturnValue_t SafeCtrl::safeNoMekf(const double *magneticFieldVector,
+ReturnValue_t SafeCtrl::safeNoMekf(const double *magFieldB,
                                    const double *magneticFieldVectorDerivative,
                                    const double *sunVector, const double *sunvectorDerivative,
-                                   double omegaRef, double *torqueCommand, double *spinAxis) {
+                                   double omegaRef, double *magMomB, double *spinAxis) {
   if (0) {
     return returnvalue::FAILED;
   }
 
-  double magneticFieldVectorT[3] = {0, 0, 0};
-  VectorOperations<double>::mulScalar(magneticFieldVector, 1e-6, magneticFieldVectorT, 3);
+  double magFieldBT[3] = {0, 0, 0};
+  VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
 
-  double commandParallel[3] = {0, 0, 0}, commandAlign[3] = {0, 0, 0}, commandOrtho[3] = {0, 0, 0};
+  double cmdParallel[3] = {0, 0, 0}, cmdAlign[3] = {0, 0, 0}, cmdOrtho[3] = {0, 0, 0},
+         torqueCmd[3] = {0, 0, 0};
 
   bool valid;
-  double omega = estimateRotationAroundSun(magneticFieldVector, magneticFieldVectorDerivative,
-                                           sunVector, &valid);
+  double omega =
+      estimateRotationAroundSun(magFieldB, magneticFieldVectorDerivative, sunVector, &valid);
 
   if (valid) {
     VectorOperations<double>::mulScalar(
         sunVector,
         acsParameters->safeModeControllerParameters.k_parallel_no_mekf * (omegaRef - omega),
-        commandParallel, 3);
+        cmdParallel, 3);
     omega = omega * sign<double>(omega);
   }
 
-  VectorOperations<double>::cross(spinAxis, sunVector, commandAlign);
+  VectorOperations<double>::cross(spinAxis, sunVector, cmdAlign);
 
   VectorOperations<double>::mulScalar(
-      commandAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, commandAlign, 3);
+      cmdAlign, acsParameters->safeModeControllerParameters.k_align_no_mekf, cmdAlign, 3);
 
-  VectorOperations<double>::cross(sunvectorDerivative, sunVector, commandOrtho);
+  VectorOperations<double>::cross(sunvectorDerivative, sunVector, cmdOrtho);
   VectorOperations<double>::mulScalar(
-      commandOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, commandOrtho, 3);
+      cmdOrtho, -acsParameters->safeModeControllerParameters.k_ortho_no_mekf, cmdOrtho, 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);
+    VectorOperations<double>::mulScalar(cmdParallel, 0, cmdParallel, 3);
   }
 
   for (uint8_t i = 0; i < 3; i++) {
-    torqueCommand[i] = commandAlign[i] + commandOrtho[i] + commandParallel[i];
+    torqueCmd[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i];
   }
+
+  // MagMom B (orthogonal torque)
+  double torqueMgt[3] = {0, 0, 0};
+  VectorOperations<double>::cross(magFieldBT, torqueCmd, torqueMgt);
+  double normMag = VectorOperations<double>::norm(magFieldB, 3);
+  VectorOperations<double>::mulScalar(torqueMgt, 1 / pow(normMag, 2), magMomB, 3);
+
+  *outputAngle = angleAlignErr;
   return returnvalue::OK;
 }