From f6a09543152de83e5eaf4354891b79458fdb8580 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 22 Mar 2023 08:59:13 +0100
Subject: [PATCH] added relevant parameters

---
 mission/controller/acs/AcsParameters.cpp | 24 ++++++++++++++++++------
 mission/controller/acs/AcsParameters.h   |  9 +++++++--
 2 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index 060e0aac..1d1502f2 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -321,29 +321,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
     case (0x8):  // SafeModeControllerParameters
       switch (parameterId) {
         case 0x0:
-          parameterWrapper->set(safeModeControllerParameters.k_rate_mekf);
+          parameterWrapper->set(safeModeControllerParameters.k_ortho_mekf);
           break;
         case 0x1:
           parameterWrapper->set(safeModeControllerParameters.k_align_mekf);
           break;
         case 0x2:
-          parameterWrapper->set(safeModeControllerParameters.k_rate_no_mekf);
+          parameterWrapper->set(safeModeControllerParameters.k_parallel_mekf);
           break;
         case 0x3:
-          parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf);
+          parameterWrapper->set(safeModeControllerParameters.k_ortho_no_mekf);
           break;
         case 0x4:
-          parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
+          parameterWrapper->set(safeModeControllerParameters.k_align_no_mekf);
           break;
         case 0x5:
-          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
+          parameterWrapper->set(safeModeControllerParameters.k_parallel_no_mekf);
           break;
         case 0x6:
-          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
+          parameterWrapper->set(safeModeControllerParameters.sunMagAngleMin);
           break;
         case 0x7:
+          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop);
+          break;
+        case 0x8:
+          parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir);
+          break;
+        case 0x9:
           parameterWrapper->setVector(safeModeControllerParameters.satRateRef);
           break;
+        case 0xA:
+          parameterWrapper->set(safeModeControllerParameters.cosineStartSpin);
+          break;
+        case 0xB:
+          parameterWrapper->set(safeModeControllerParameters.sineCalculateOmegaSun);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index 5b185ea4..3e956ae3 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -811,11 +811,13 @@ class AcsParameters : public HasParametersIF {
   } rwMatrices;
 
   struct SafeModeControllerParameters {
-    double k_rate_mekf = 0.00059437;
+    double k_ortho_mekf = 0.00059437;
     double k_align_mekf = 0.000056875;
+    double k_parallel_mekf = 0;
 
-    double k_rate_no_mekf = 0.00059437;
+    double k_ortho_no_mekf = 0.00059437;
     double k_align_no_mekf = 0.000056875;
+    double k_parallel_no_mekf = 0;
 
     double sunMagAngleMin = 5 * M_PI / 180;
 
@@ -823,6 +825,9 @@ class AcsParameters : public HasParametersIF {
     double sunTargetDir[3] = {0, 0, 1};
 
     double satRateRef[3] = {0, 0, 0};
+
+    float cosineStartSpin = 0.9;
+    float sineCalculateOmegaSun = 0.24;
   } safeModeControllerParameters;
 
   struct PointingLawParameters {