From 75ade4d8f8b047fc3142845622aad181a9a3a189 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 17 Feb 2023 09:21:30 +0100
Subject: [PATCH] event for multiple invalid rw now triggert after 5 failures

---
 mission/controller/AcsController.cpp | 13 ++++++++-----
 mission/controller/AcsController.h   |  1 +
 2 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 352f4167..74fce754 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -15,6 +15,7 @@ AcsController::AcsController(object_id_t objectId)
       detumble(&acsParameters),
       ptgCtrl(&acsParameters),
       detumbleCounter{0},
+      multipleRwUnavailableCounter{0},
       parameterHelper(this),
       mgmDataRaw(this),
       mgmDataProcessed(this),
@@ -265,8 +266,13 @@ void AcsController::performPointingCtrl() {
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
   ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
-    triggerEvent(acs::MULTIPLE_RW_INVALID);
+    multipleRwUnavailableCounter++;
+    if (multipleRwUnavailableCounter > 4) {
+      triggerEvent(acs::MULTIPLE_RW_INVALID);
+    }
     return;
+  } else {
+    multipleRwUnavailableCounter = 0;
   }
   double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0};
   double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
@@ -389,10 +395,7 @@ void AcsController::performPointingCtrl() {
   }
 
   if (enableAntiStiction) {
-    bool rwAvailable[4] = {true, true, true, true};  // WHICH INPUT SENSOR SET?
-    int32_t rwSpeed[4] = {sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
-                          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value};
-    ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled);
+    ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
   }
 
   int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index c0e719f2..265fe930 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -50,6 +50,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PtgCtrl ptgCtrl;
 
   uint8_t detumbleCounter;
+  uint8_t multipleRwUnavailableCounter;
 
   ParameterHelper parameterHelper;