From cc35db91cdd48e55e1498a4039fada076393f8a2 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 12 Oct 2022 17:14:33 +0200
Subject: [PATCH] added calibration of raw MGM values

---
 mission/controller/acs/AcsParameters.h      | 19 +++++++++++
 mission/controller/acs/SensorProcessing.cpp | 36 +++++++++++++++++----
 2 files changed, 49 insertions(+), 6 deletions(-)

diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index 29caa59e..6c72d9e5 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -36,6 +36,25 @@ class AcsParameters /*: public HasParametersIF*/ {
     float mgm2orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};
     float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
     float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
+
+    float mgm0hardIronOffset[3] = {19.89364, -29.94111, -31.07508};
+    float mgm1hardIronOffset[3] = {10.95500, -8.053403, -33.36383};
+    float mgm2hardIronOffset[3] = {15.72181, -26.87090, -62.19010};
+    float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0};
+    float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0};
+
+    float mgm0softIronInverse[3][3] = {{1420.727e-3, 9.352825e-3, -127.1979e-3},
+                                       {9.352825e-3, 1031.965e-3, -80.02734e-3},
+                                       {-127.1979e-3, -80.02734e-3, 934.8899e-3}};
+    float mgm1softIronInverse[3][3] = {{126.7325e-2, -4.146410e-2, -18.37963e-2},
+                                       {-4.146410e-2, 109.3310e-2, -5.246314e-2},
+                                       {-18.37963e-2, -5.246314e-2, 105.7300e-2}};
+    float mgm2softIronInverse[3][3] = {{143.0438e-2, 7.095763e-2, 15.67482e-2},
+                                       {7.095763e-2, 99.65167e-2, -6.958760e-2},
+                                       {15.67482e-2, -6.958760e-2, 94.50124e-2}};
+    float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+    float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+
   } mgmHandlingParameters;
 
   struct SusHandlingParameters {
diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index dc930fc8..998927f6 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -42,38 +42,62 @@ bool SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
     validMagField = false;
     return false;
   }
-  // Transforming Values to the Body Frame (actually it is the geometry frame atm)
+  float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
+        mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
+        mgm4ValueNoBias[3] = {0, 0, 0};
+  float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
+        mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
   float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
         mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
 
   bool validUnit[5] = {false, false, false, false, false};
   uint8_t validCount = 0;
   if (mgm0valid) {
-    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
+    VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
+                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
+                                      mgm0ValueCalib, 3, 3, 1);
+    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
                                       mgm0ValueBody, 3, 3, 1);
     validCount += 1;
     validUnit[0] = true;
   }
   if (mgm1valid) {
-    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
+    VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
+                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
+                                      mgm1ValueCalib, 3, 3, 1);
+    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
                                       mgm1ValueBody, 3, 3, 1);
     validCount += 1;
     validUnit[1] = true;
   }
   if (mgm2valid) {
-    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
+    VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
+                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
+                                      mgm2ValueCalib, 3, 3, 1);
+    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
                                       mgm2ValueBody, 3, 3, 1);
     validCount += 1;
     validUnit[2] = true;
   }
   if (mgm3valid) {
-    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
+    VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
+                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
+                                      mgm3ValueCalib, 3, 3, 1);
+    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
                                       mgm3ValueBody, 3, 3, 1);
     validCount += 1;
     validUnit[3] = true;
   }
   if (mgm4valid) {
-    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4Value,
+    VectorOperations<float>::subtract(mgm4Value, mgmParameters->mgm4hardIronOffset, mgm4ValueNoBias,
+                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
+                                      mgm4ValueCalib, 3, 3, 1);
+    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
                                       mgm4ValueBody, 3, 3, 1);
     validCount += 1;
     validUnit[4] = true;