From a3eaace4a55504e3a6dc13b933031e3e1b13963b Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 17:31:26 +0100
Subject: [PATCH] added RwSpeedActuationSets to AcsController

---
 mission/controller/AcsController.cpp | 51 ++++++++++++++++++++++++++--
 mission/controller/AcsController.h   |  8 ++++-
 2 files changed, 56 insertions(+), 3 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 1b4f54a9..bee21d71 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -189,8 +189,23 @@ void AcsController::performSafe() {
   //    PoolReadGuard pg(&dipoleSet);
   //    MutexGuard mg(torquer::lazyLock());
   //    torquer::NEW_ACTUATION_FLAG = true;
-  //    dipoleSet.setDipoles(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
-  //    torqueDuration);
+  //    dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], 0);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw1SpeedSet);
+  //    rw1SpeedSet.setRwSpeed(0);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw2SpeedSet);
+  //    rw2SpeedSet.setRwSpeed(0);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw3SpeedSet);
+  //    rw3SpeedSet.setRwSpeed(0);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw4SpeedSet);
+  //    rw4SpeedSet.setRwSpeed(0);
   //  }
 }
 
@@ -251,6 +266,22 @@ void AcsController::performDetumble() {
   //	  dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
   // torqueDuration);
   //  }
+  //  {
+  //    PoolReadGuard pg(&rw1SpeedSet);
+  //    rw1SpeedSet.setRwSpeed(0);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw2SpeedSet);
+  //    rw2SpeedSet.setRwSpeed(0);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw3SpeedSet);
+  //    rw3SpeedSet.setRwSpeed(0);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw4SpeedSet);
+  //    rw4SpeedSet.setRwSpeed(0);
+  //  }
 }
 
 void AcsController::performPointingCtrl() {
@@ -431,6 +462,22 @@ void AcsController::performPointingCtrl() {
   //	  dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2],
   // torqueDuration);
   //  }
+  //  {
+  //    PoolReadGuard pg(&rw1SpeedSet);
+  //    rw1SpeedSet.setRwSpeed(cmdRwSpeedInt[0]);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw2SpeedSet);
+  //    rw2SpeedSet.setRwSpeed(cmdRwSpeedInt[1]);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw3SpeedSet);
+  //    rw3SpeedSet.setRwSpeed(cmdRwSpeedInt[2]);
+  //  }
+  //  {
+  //    PoolReadGuard pg(&rw4SpeedSet);
+  //    rw4SpeedSet.setRwSpeed(cmdRwSpeedInt[3]);
+  //  }
 }
 
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 1e017099..d427bd5b 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -17,6 +17,7 @@
 #include "eive/objects.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
+#include "mission/devices/devicedefinitions/RwDefinitions.h"
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
 #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
 
@@ -72,8 +73,13 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   /* ACS Sensor Values */
   ACS::SensorValues sensorValues;
 
-  /* ACS Datasets */
+  /* ACS Actuation Datasets */
   IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
+  RwDefinitions::RwSpeedActuationSet rw1SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW1);
+  RwDefinitions::RwSpeedActuationSet rw2SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW2);
+  RwDefinitions::RwSpeedActuationSet rw3SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW3);
+  RwDefinitions::RwSpeedActuationSet rw4SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW4);
+  /* ACS Datasets */
   // MGMs
   acsctrl::MgmDataRaw mgmDataRaw;
   PoolEntry<float> mgm0VecRaw = PoolEntry<float>(3);