From 4a86d4ba4b95cd510f30a18531aaa70cfde24a1a Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 15 Nov 2023 16:54:54 +0100
Subject: [PATCH] changed fdir to new variant

---
 mission/acs/defs.h                       |  6 +++---
 mission/controller/AcsController.cpp     | 15 +++++++++++++++
 mission/controller/AcsController.h       |  2 +-
 mission/controller/acs/AcsParameters.cpp |  2 +-
 mission/controller/acs/AcsParameters.h   |  2 +-
 mission/system/acs/StrFdir.cpp           |  2 +-
 6 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/mission/acs/defs.h b/mission/acs/defs.h
index 253c6aca..2f82ca9f 100644
--- a/mission/acs/defs.h
+++ b/mission/acs/defs.h
@@ -67,9 +67,9 @@ static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
 static constexpr Event MEKF_RECOVERY = MAKE_EVENT(4, severity::INFO);
 //! [EXPORT] : [COMMENT] MEKF performed an automatic reset after detection of nonfinite values.
 static constexpr Event MEKF_AUTOMATIC_RESET = MAKE_EVENT(5, severity::INFO);
-//! [EXPORT] : [COMMENT] MEKF was not able to compute a solution during any pointing ACS mode for a
-//! prolonged time.
-static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIGH);
+//! [EXPORT] : [COMMENT] For a prolonged time, no attitude information was available for the
+//! Pointing Controller. Falling back to Safe Mode.
+static constexpr Event PTG_CTRL_NO_ATTITUDE_INFORMATION = MAKE_EVENT(6, severity::HIGH);
 //! [EXPORT] : [COMMENT] The ACS safe mode controller was not able to compute a solution and has
 //! failed.
 //! P1: Missing information about magnetic field, P2: Missing information about rotational rate
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 8b484fb4..47c5d77e 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -369,6 +369,21 @@ void AcsController::performDetumble() {
 }
 
 void AcsController::performPointingCtrl() {
+  acs::ControlModeStrategy ptgCtrlStrat =
+      ptgCtrl.pointingCtrlStrategy(magFieldValid, mekfValid, satRotRateValid, sunDirValid,
+                                   fusedRateTotalValid, mekfEnabled, gyrEnabled, dampingEnabled);
+
+  if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
+    ptgCtrlLostCounter++;
+    if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
+      triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
+      ptgCtrlLostCounter = 0;
+    }
+    commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
+                     cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
+                     acsParameters.rwHandlingParameters.rampTime);
+  }
+
   uint8_t enableAntiStiction = true;
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 1b184b80..f67595ee 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -72,7 +72,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   uint8_t detumbleCounter = 0;
   uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = false;
-  uint16_t mekfInvalidCounter = 0;
+  uint16_t ptgCtrlLostCounter = 0;
   bool safeCtrlFailureFlag = false;
   uint8_t safeCtrlFailureCounter = 0;
   uint8_t resetMekfCount = 0;
diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index 7b867a46..551f7cd3 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -24,7 +24,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(onBoardParams.sampleTime);
           break;
         case 0x1:
-          parameterWrapper->set(onBoardParams.mekfViolationTimer);
+          parameterWrapper->set(onBoardParams.ptgCtrlLostTimer);
           break;
         case 0x2:
           parameterWrapper->set(onBoardParams.fusedRateSafeDuringEclipse);
diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index f62928a0..cbebb52a 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -18,7 +18,7 @@ class AcsParameters : public HasParametersIF {
 
   struct OnBoardParams {
     double sampleTime = 0.4;  // [s]
-    uint16_t mekfViolationTimer = 750;
+    uint16_t ptgCtrlLostTimer = 750;
     uint8_t fusedRateSafeDuringEclipse = true;
   } onBoardParams;
 
diff --git a/mission/system/acs/StrFdir.cpp b/mission/system/acs/StrFdir.cpp
index 97a4162a..83bd27fa 100644
--- a/mission/system/acs/StrFdir.cpp
+++ b/mission/system/acs/StrFdir.cpp
@@ -6,7 +6,7 @@ StrFdir::StrFdir(object_id_t strObject)
     : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {}
 
 ReturnValue_t StrFdir::eventReceived(EventMessage* event) {
-  if (event->getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
+  if (event->getEvent() == acs::PTG_CTRL_NO_ATTITUDE_INFORMATION) {
     setFaulty(event->getEvent());
     return returnvalue::OK;
   }