From e4936b1bedb3c0a08ceed13911cb0e8daa57c55c Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 19 Dec 2022 14:23:42 +0100
Subject: [PATCH] removed deprecated acs parameters and set sunMagAngleMin

---
 mission/controller/acs/AcsParameters.cpp | 26 ++++--------------------
 mission/controller/acs/AcsParameters.h   | 17 ++--------------
 2 files changed, 6 insertions(+), 37 deletions(-)

diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index 3205f7b2..276204d2 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -219,12 +219,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x23:
           parameterWrapper->set(susHandlingParameters.sus11coeffBeta);
           break;
-        case 0x24:
-          parameterWrapper->set(susHandlingParameters.filterAlpha);
-          break;
-        case 0x25:
-          parameterWrapper->set(susHandlingParameters.sunThresh);
-          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -259,30 +253,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
     case (0x6):  // RwHandlingParameters
       switch (parameterId & 0xFF) {
         case 0x0:
-          parameterWrapper->set(rwHandlingParameters.rw0orientationMatrix);
-          break;
-        case 0x1:
-          parameterWrapper->set(rwHandlingParameters.rw1orientationMatrix);
-          break;
-        case 0x2:
-          parameterWrapper->set(rwHandlingParameters.rw2orientationMatrix);
-          break;
-        case 0x3:
-          parameterWrapper->set(rwHandlingParameters.rw3orientationMatrix);
-          break;
-        case 0x4:
           parameterWrapper->set(rwHandlingParameters.inertiaWheel);
           break;
-        case 0x5:
+        case 0x1:
           parameterWrapper->set(rwHandlingParameters.maxTrq);
           break;
-        case 0x6:
+        case 0x2:
           parameterWrapper->set(rwHandlingParameters.stictionSpeed);
           break;
-        case 0x7:
+        case 0x3:
           parameterWrapper->set(rwHandlingParameters.stictionReleaseSpeed);
           break;
-        case 0x8:
+        case 0x4:
           parameterWrapper->set(rwHandlingParameters.stictionTorque);
           break;
         default:
diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index 3c88c9a1..895eaf9b 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -760,9 +760,6 @@ class AcsParameters : public HasParametersIF {
         {116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136,
          0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
          -0.000889232196185857, -0.00168429567131815}};
-
-    float filterAlpha;
-    float sunThresh;
   } susHandlingParameters;
 
   struct GyrHandlingParameters {
@@ -780,10 +777,6 @@ class AcsParameters : public HasParametersIF {
   } gyrHandlingParameters;
 
   struct RwHandlingParameters {
-    double rw0orientationMatrix[3][3];
-    double rw1orientationMatrix[3][3];
-    double rw2orientationMatrix[3][3];
-    double rw3orientationMatrix[3][3];
     double inertiaWheel = 0.000028198;
     double maxTrq = 0.0032;             // 3.2 [mNm]
     double stictionSpeed = 80;          // RPM
@@ -795,11 +788,6 @@ class AcsParameters : public HasParametersIF {
     double alignmentMatrix[3][4] = {{0.9205, 0.0000, -0.9205, 0.0000},
                                     {0.0000, -0.9205, 0.0000, 0.9205},
                                     {0.3907, 0.3907, 0.3907, 0.3907}};
-    //    double pseudoInverse[4][3] = {{0.4434, -0.2845, 0.3597},
-    //                                  {0.2136, -0.3317, 1.0123},
-    //                                  {-0.8672, -0.1406, 0.1778},
-    //                                  {0.6426, 0.4794, 1.3603}};
-    //    where does the first pseudo inverse come frome - matlab gives result below
     double pseudoInverse[4][3] = {
         {0.5432, 0, 0.6398}, {0, -0.5432, 0.6398}, {-0.5432, 0, 0.6398}, {0, 0.5432, 0.6398}};
     double without0[4][3] = {
@@ -818,10 +806,9 @@ class AcsParameters : public HasParametersIF {
     double k_align_mekf = 0.000056875;
 
     double k_rate_no_mekf = 0.00059437;
-    ;
     double k_align_no_mekf = 0.000056875;
-    ;
-    double sunMagAngleMin;  // ???
+
+    double sunMagAngleMin = 5 * M_PI / 180;
 
     double sunTargetDir[3] = {0, 0, 1};
     double satRateRef[3] = {0, 0, 0};