From 5a5d00e4e50bf81bcb2932386850724bad31e966 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 17:11:23 +0100
Subject: [PATCH 01/23] dataPool for setting RW speeds

---
 mission/devices/RwHandler.cpp                 |  2 +
 mission/devices/RwHandler.h                   |  2 +
 .../devices/devicedefinitions/RwDefinitions.h | 54 +++++++++++++++----
 3 files changed, 47 insertions(+), 11 deletions(-)

diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp
index 732bb9fe..d859f9d0 100644
--- a/mission/devices/RwHandler.cpp
+++ b/mission/devices/RwHandler.cpp
@@ -243,6 +243,8 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
 
 ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                  LocalDataPoolManager& poolManager) {
+  localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed);
+
   localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
 
   localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>({0}));
diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h
index f9f66bd5..43deb579 100644
--- a/mission/devices/RwHandler.h
+++ b/mission/devices/RwHandler.h
@@ -96,6 +96,8 @@ class RwHandler : public DeviceHandlerBase {
 
   uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
 
+  PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>(0, false);
+
   enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS };
 
   InternalState internalState = InternalState::GET_RESET_STATUS;
diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h
index 2923b41b..4016b23c 100644
--- a/mission/devices/devicedefinitions/RwDefinitions.h
+++ b/mission/devices/devicedefinitions/RwDefinitions.h
@@ -51,7 +51,9 @@ enum PoolIds : lp_id_t {
   SPI_BYTES_WRITTEN,
   SPI_BYTES_READ,
   SPI_REG_OVERRUN_ERRORS,
-  SPI_TOTAL_ERRORS
+  SPI_TOTAL_ERRORS,
+
+  RW_SPEED,
 };
 
 enum States : uint8_t { STATE_ERROR, IDLE, COASTING, RUNNING_SPEED_STABLE, RUNNING_SPEED_CHANGING };
@@ -75,10 +77,13 @@ static const DeviceCommandId_t SET_SPEED = 6;
 static const DeviceCommandId_t GET_TEMPERATURE = 8;
 static const DeviceCommandId_t GET_TM = 9;
 
-static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE;
-static const uint32_t STATUS_SET_ID = GET_RW_STATUS;
-static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS;
-static const uint32_t TM_SET_ID = GET_TM;
+enum SetIds : uint32_t {
+  TEMPERATURE_SET_ID = GET_TEMPERATURE,
+  STATUS_SET_ID = GET_RW_STATUS,
+  LAST_RESET_ID = GET_LAST_RESET_STATUS,
+  TM_SET_ID = GET_TM,
+  RW_SPEED = 10,
+};
 
 static const size_t SIZE_GET_RESET_STATUS = 5;
 static const size_t SIZE_CLEAR_RESET_STATUS = 4;
@@ -106,9 +111,11 @@ static const uint8_t TM_SET_ENTRIES = 24;
  */
 class StatusSet : public StaticLocalDataSet<STATUS_SET_ENTRIES> {
  public:
-  StatusSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, STATUS_SET_ID) {}
+  StatusSet(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, RwDefinitions::SetIds::STATUS_SET_ID) {}
 
-  StatusSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, STATUS_SET_ID)) {}
+  StatusSet(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::STATUS_SET_ID)) {}
 
   lp_var_t<int32_t> temperatureCelcius =
       lp_var_t<int32_t>(sid.objectId, PoolIds::TEMPERATURE_C, this);
@@ -124,9 +131,11 @@ class StatusSet : public StaticLocalDataSet<STATUS_SET_ENTRIES> {
  */
 class LastResetSatus : public StaticLocalDataSet<LAST_RESET_ENTRIES> {
  public:
-  LastResetSatus(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, LAST_RESET_ID) {}
+  LastResetSatus(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, RwDefinitions::SetIds::LAST_RESET_ID) {}
 
-  LastResetSatus(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, LAST_RESET_ID)) {}
+  LastResetSatus(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::LAST_RESET_ID)) {}
 
   // If a reset occurs, the status code will be cached into this variable
   lp_var_t<uint8_t> lastNonClearedResetStatus =
@@ -143,9 +152,11 @@ class LastResetSatus : public StaticLocalDataSet<LAST_RESET_ENTRIES> {
  */
 class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
  public:
-  TmDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TM_SET_ID) {}
+  TmDataset(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, RwDefinitions::SetIds::TM_SET_ID) {}
 
-  TmDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TM_SET_ID)) {}
+  TmDataset(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::TM_SET_ID)) {}
 
   lp_var_t<uint8_t> lastResetStatus =
       lp_var_t<uint8_t>(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this);
@@ -192,6 +203,27 @@ class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
       lp_var_t<uint32_t>(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this);
 };
 
+class RwSpeedActuationSet : StaticLocalDataSet<4> {
+  friend class ::RwHandler;
+
+ public:
+  RwSpeedActuationSet(HasLocalDataPoolIF& owner)
+      : StaticLocalDataSet(&owner, RwDefinitions::SetIds::RW_SPEED) {}
+  RwSpeedActuationSet(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::RW_SPEED)) {}
+
+  void setRwSpeed(int32_t rwSpeed_) {
+    if (rwSpeed.value != rwSpeed_) {
+    }
+    rwSpeed = rwSpeed_;
+  }
+
+  void getRwSpeed(int32_t& rwSpeed_) { rwSpeed_ = rwSpeed.value; }
+
+ private:
+  lp_var_t<int32_t> rwSpeed = lp_var_t<int32_t>(sid.objectId, RW_SPEED, this);
+};
+
 }  // namespace RwDefinitions
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */

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 02/23] 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);

From 51629da4a068e1aad1364692a339048e64accc49 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 9 Feb 2023 17:55:20 +0100
Subject: [PATCH 03/23] naming fixes

---
 mission/devices/devicedefinitions/RwDefinitions.h | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h
index 4016b23c..a4e8e96d 100644
--- a/mission/devices/devicedefinitions/RwDefinitions.h
+++ b/mission/devices/devicedefinitions/RwDefinitions.h
@@ -82,7 +82,7 @@ enum SetIds : uint32_t {
   STATUS_SET_ID = GET_RW_STATUS,
   LAST_RESET_ID = GET_LAST_RESET_STATUS,
   TM_SET_ID = GET_TM,
-  RW_SPEED = 10,
+  SPEED_CMD_SET = 10,
 };
 
 static const size_t SIZE_GET_RESET_STATUS = 5;
@@ -204,13 +204,13 @@ class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
 };
 
 class RwSpeedActuationSet : StaticLocalDataSet<4> {
-  friend class ::RwHandler;
+  friend class RwHandler;
 
  public:
   RwSpeedActuationSet(HasLocalDataPoolIF& owner)
-      : StaticLocalDataSet(&owner, RwDefinitions::SetIds::RW_SPEED) {}
+      : StaticLocalDataSet(&owner, RwDefinitions::SetIds::SPEED_CMD_SET) {}
   RwSpeedActuationSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::RW_SPEED)) {}
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::SPEED_CMD_SET)) {}
 
   void setRwSpeed(int32_t rwSpeed_) {
     if (rwSpeed.value != rwSpeed_) {
@@ -221,7 +221,8 @@ class RwSpeedActuationSet : StaticLocalDataSet<4> {
   void getRwSpeed(int32_t& rwSpeed_) { rwSpeed_ = rwSpeed.value; }
 
  private:
-  lp_var_t<int32_t> rwSpeed = lp_var_t<int32_t>(sid.objectId, RW_SPEED, this);
+  lp_var_t<int32_t> rwSpeed =
+      lp_var_t<int32_t>(sid.objectId, RwDefinitions::PoolIds::RW_SPEED, this);
 };
 
 }  // namespace RwDefinitions

From 9dfd8491d26aee8b619818c3883da58e104afdb6 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 10:25:13 +0100
Subject: [PATCH 04/23] added rampTime and torqueDuration to AcsParameters

---
 mission/controller/acs/AcsParameters.cpp | 5 +++++
 mission/controller/acs/AcsParameters.h   | 3 +++
 2 files changed, 8 insertions(+)

diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index 13642fe9..30264963 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -278,6 +278,8 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x4:
           parameterWrapper->set(rwHandlingParameters.stictionTorque);
           break;
+        case 0x5:
+          parameterWrapper->set(rwHandlingParameters.rampTime);
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -584,6 +586,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x5:
           parameterWrapper->set(magnetorquesParameter.DipolMax);
           break;
+        case 0x6:
+          parameterWrapper->set(magnetorquesParameter.torqueDuration);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index f1c0fb63..35e316f1 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -792,6 +792,8 @@ class AcsParameters : public HasParametersIF {
     double stictionSpeed = 100;         // 80;          // RPM
     double stictionReleaseSpeed = 120;  // RPM
     double stictionTorque = 0.0006;
+
+    uint16_t rampTime = 10;
   } rwHandlingParameters;
 
   struct RwMatrices {
@@ -910,6 +912,7 @@ class AcsParameters : public HasParametersIF {
     double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
     double DipolMax = 0.2;  // [Am^2]
 
+    uint16_t torqueDuration = 300;  // [ms]
   } magnetorquesParameter;
 
   struct DetumbleParameter {

From 9041a3376c18cae282a1afcfbcd39c3ef3b09c47 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 10:56:50 +0100
Subject: [PATCH 05/23] ActuatorCmd now output their solutions as integers as
 expected by the sensors

---
 mission/controller/acs/ActuatorCmd.cpp | 27 +++++++++++++++++---------
 mission/controller/acs/ActuatorCmd.h   |  6 +++---
 2 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp
index 4e0052e0..cbe24e5b 100644
--- a/mission/controller/acs/ActuatorCmd.cpp
+++ b/mission/controller/acs/ActuatorCmd.cpp
@@ -38,27 +38,32 @@ void ActuatorCmd::scalingTorqueRws(const double *rwTrq, double *rwTrqScaled) {
   }
 }
 
-void ActuatorCmd::cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1,
-                                const int32_t *speedRw2, const int32_t *speedRw3,
-                                const double *rwTorque, double *rwCmdSpeed) {
+void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
+                                const int32_t speedRw2, const int32_t speedRw3,
+                                const double *rwTorque, int32_t *rwCmdSpeed) {
   using namespace Math;
 
   // Calculating the commanded speed in RPM for every reaction wheel
-  double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
+  int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
   double deltaSpeed[4] = {0, 0, 0, 0};
   double commandTime = acsParameters.onBoardParams.sampleTime,
          inertiaWheel = acsParameters.rwHandlingParameters.inertiaWheel;
   double radToRpm = 60 / (2 * PI);  // factor for conversion to RPM
   //	 W_RW = Torque_RW / I_RW * delta t [rad/s]
   double factor = commandTime / inertiaWheel * radToRpm;
+  int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
   VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
-  VectorOperations<double>::add(speedRws, deltaSpeed, rwCmdSpeed, 4);
+  for (int i = 0; i < 4; i++) {
+    deltaSpeedInt[i] = std::round(deltaSpeed[i]);
+  }
+  VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
 }
 
-void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator) {
+void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) {
   // Convert to actuator frame
+  double dipolMomentActuatorDouble[3] = {0, 0, 0};
   MatrixOperations<double>::multiply(*acsParameters.magnetorquesParameter.inverseAlignment,
-                                     dipolMoment, dipolMomentActuator, 3, 3, 1);
+                                     dipolMoment, dipolMomentActuatorDouble, 3, 3, 1);
   // Scaling along largest element if dipol exceeds maximum
   double maxDipol = acsParameters.magnetorquesParameter.DipolMax;
   double maxValue = 0;
@@ -69,8 +74,12 @@ void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, double *dipolMomentActu
   }
   if (maxValue > maxDipol) {
     double scalingFactor = maxDipol / maxValue;
-    VectorOperations<double>::mulScalar(dipolMomentActuator, scalingFactor, dipolMomentActuator, 3);
+    VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
+                                        dipolMomentActuatorDouble, 3);
   }
   // scale dipole from 1 Am^2 to 1e^-4 Am^2
-  VectorOperations<double>::mulScalar(dipolMomentActuator, 1e4, dipolMomentActuator, 3);
+  VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
+  for (int i = 0; i < 3; i++) {
+    dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
+  }
 }
diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h
index 04e2b61f..969bd782 100644
--- a/mission/controller/acs/ActuatorCmd.h
+++ b/mission/controller/acs/ActuatorCmd.h
@@ -28,8 +28,8 @@ class ActuatorCmd {
    * 			rwCmdSpeed			output revolutions per minute for every
    * reaction wheel
    */
-  void cmdSpeedToRws(const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
-                     const int32_t *speedRw3, const double *rwTorque, double *rwCmdSpeed);
+  void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
+                     const int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed);
 
   /*
    * @brief:	cmdDipolMtq()		gives the commanded dipol moment for the magnetorques
@@ -37,7 +37,7 @@ class ActuatorCmd {
    * @param:	dipolMoment 		given dipol moment in spacecraft frame
    * 			dipolMomentActuator	resulting dipol moment in actuator reference frame
    */
-  void cmdDipolMtq(const double *dipolMoment, double *dipolMomentActuator);
+  void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator);
 
  protected:
  private:

From c2079dcbbad994877c3dc3a638c25ea325699f0d Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 11:26:46 +0100
Subject: [PATCH 06/23] added rampTime and made dataSet public

---
 .../devices/devicedefinitions/RwDefinitions.h   | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h
index a4e8e96d..b3cb0fe4 100644
--- a/mission/devices/devicedefinitions/RwDefinitions.h
+++ b/mission/devices/devicedefinitions/RwDefinitions.h
@@ -54,6 +54,7 @@ enum PoolIds : lp_id_t {
   SPI_TOTAL_ERRORS,
 
   RW_SPEED,
+  RAMP_TIME,
 };
 
 enum States : uint8_t { STATE_ERROR, IDLE, COASTING, RUNNING_SPEED_STABLE, RUNNING_SPEED_CHANGING };
@@ -203,7 +204,7 @@ class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
       lp_var_t<uint32_t>(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this);
 };
 
-class RwSpeedActuationSet : StaticLocalDataSet<4> {
+class RwSpeedActuationSet : public StaticLocalDataSet<2> {
   friend class RwHandler;
 
  public:
@@ -212,17 +213,25 @@ class RwSpeedActuationSet : StaticLocalDataSet<4> {
   RwSpeedActuationSet(object_id_t objectId)
       : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::SPEED_CMD_SET)) {}
 
-  void setRwSpeed(int32_t rwSpeed_) {
+  void setRwSpeed(int32_t rwSpeed_, uint16_t rampTime_) {
     if (rwSpeed.value != rwSpeed_) {
+      rwSpeed = rwSpeed_;
+    }
+    if (rampTime.value != rampTime_) {
+      rampTime = rampTime_;
     }
-    rwSpeed = rwSpeed_;
   }
 
-  void getRwSpeed(int32_t& rwSpeed_) { rwSpeed_ = rwSpeed.value; }
+  void getRwSpeed(int32_t& rwSpeed_, uint16_t& rampTime_) {
+    rwSpeed_ = rwSpeed.value;
+    rampTime_ = rampTime.value;
+  }
 
  private:
   lp_var_t<int32_t> rwSpeed =
       lp_var_t<int32_t>(sid.objectId, RwDefinitions::PoolIds::RW_SPEED, this);
+  lp_var_t<uint16_t> rampTime =
+      lp_var_t<uint16_t>(sid.objectId, RwDefinitions::PoolIds::RAMP_TIME, this);
 };
 
 }  // namespace RwDefinitions

From 01d6060111ee96cedc8e40e3368aae3414a38562 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 11:27:02 +0100
Subject: [PATCH 07/23] added ramp time

---
 mission/devices/RwHandler.cpp | 1 +
 mission/devices/RwHandler.h   | 1 +
 2 files changed, 2 insertions(+)

diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp
index d859f9d0..e9383c4a 100644
--- a/mission/devices/RwHandler.cpp
+++ b/mission/devices/RwHandler.cpp
@@ -244,6 +244,7 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
 ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                  LocalDataPoolManager& poolManager) {
   localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed);
+  localDataPoolMap.emplace(RwDefinitions::RAMP_TIME, &rampTime);
 
   localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
 
diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h
index 43deb579..17f2b396 100644
--- a/mission/devices/RwHandler.h
+++ b/mission/devices/RwHandler.h
@@ -97,6 +97,7 @@ class RwHandler : public DeviceHandlerBase {
   uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
 
   PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>(0, false);
+  PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>(10, false);
 
   enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS };
 

From ea32d87c25955912be952335696348cb003b659a Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 11:27:25 +0100
Subject: [PATCH 08/23] function now handles actuator cmd

---
 mission/controller/AcsController.cpp | 163 ++++++++++-----------------
 mission/controller/AcsController.h   |   4 +
 2 files changed, 66 insertions(+), 101 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index bee21d71..29bd06b6 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -116,10 +116,10 @@ void AcsController::performSafe() {
   navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
                      &mekfData, &validMekf);
 
-  // Give desired satellite rate and sun direction to align
+  // give desired satellite rate and sun direction to align
   double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
   guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
-  //	IF MEKF is working
+  // if MEKF is working
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
   bool magMomMtqValid = false;
   if (validMekf == returnvalue::OK) {
@@ -137,8 +137,8 @@ void AcsController::performSafe() {
         sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
   }
 
-  double dipolCmdUnits[3] = {0, 0, 0};
-  actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
+  int16_t cmdDipolMtqs[3] = {0, 0, 0};
+  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
 
   {
     PoolReadGuard pg(&ctrlValData);
@@ -180,33 +180,15 @@ void AcsController::performSafe() {
       actuatorCmdData.rwTargetTorque.setValid(false);
       std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
       actuatorCmdData.rwTargetSpeed.setValid(false);
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t));
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
       actuatorCmdData.mtqTargetDipole.setValid(true);
       actuatorCmdData.setValidity(true, false);
     }
   }
-  //  {
-  //    PoolReadGuard pg(&dipoleSet);
-  //    MutexGuard mg(torquer::lazyLock());
-  //    torquer::NEW_ACTUATION_FLAG = true;
-  //    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);
-  //  }
+
+  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
+                   acsParameters.rwHandlingParameters.rampTime);
 }
 
 void AcsController::performDetumble() {
@@ -223,8 +205,8 @@ void AcsController::performDetumble() {
   detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
                    mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
                    mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
-  double dipolCmdUnits[3] = {0, 0, 0};
-  actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
+  int16_t cmdDipolMtqs[3] = {0, 0, 0};
+  actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
 
   if (mekfData.satRotRateMekf.isValid() &&
       VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
@@ -243,10 +225,6 @@ void AcsController::performDetumble() {
     triggerEvent(acs::SAFE_RATE_RECOVERY);
   }
 
-  int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
-  for (int i = 0; i < 3; ++i) {
-    cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]);
-  }
   {
     PoolReadGuard pg(&actuatorCmdData);
     if (pg.getReadResult() == returnvalue::OK) {
@@ -254,34 +232,15 @@ void AcsController::performDetumble() {
       actuatorCmdData.rwTargetTorque.setValid(false);
       std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
       actuatorCmdData.rwTargetSpeed.setValid(false);
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
       actuatorCmdData.mtqTargetDipole.setValid(true);
       actuatorCmdData.setValidity(true, false);
     }
   }
-  //  {
-  //	  PoolReadGuard pg(&dipoleSet);
-  //	  MutexGuard mg(torquer::lazyLock());
-  //	  torquer::NEW_ACTUATION_FLAG = true;
-  //	  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);
-  //  }
+
+  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
+                   acsParameters.rwHandlingParameters.rampTime);
 }
 
 void AcsController::performPointingCtrl() {
@@ -304,7 +263,7 @@ void AcsController::performPointingCtrl() {
   guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   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};
-  double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
+  double mgtDpDes[3] = {0, 0, 0};
 
   switch (submode) {
     case acs::PTG_IDLE:
@@ -424,60 +383,62 @@ 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)};
+    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);
   }
 
-  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
-  actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value),
-                            &(sensorValues.rw2Set.currSpeed.value),
-                            &(sensorValues.rw3Set.currSpeed.value),
-                            &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws);
-  actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
-
-  int16_t cmdDipolUnitsInt[3] = {0, 0, 0};
-  for (int i = 0; i < 3; ++i) {
-    cmdDipolUnitsInt[i] = std::round(dipolUnits[i]);
-  }
-  int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0};
-  for (int i = 0; i < 4; ++i) {
-    cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]);
-  }
+  int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
+  actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
+                            sensorValues.rw2Set.currSpeed.value,
+                            sensorValues.rw3Set.currSpeed.value,
+                            sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
+  int16_t cmdDipolMtqs[3] = {0, 0, 0};
+  actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
 
   {
     PoolReadGuard pg(&actuatorCmdData);
     if (pg.getReadResult() == returnvalue::OK) {
       std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
-      std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t));
-      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
+      std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
+      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
       actuatorCmdData.setValidity(true, true);
     }
   }
-  //  {
-  //	  PoolReadGuard pg(&dipoleSet);
-  //	  MutexGuard mg(torquer::lazyLock());
-  //	  torquer::NEW_ACTUATION_FLAG = true;
-  //	  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]);
-  //  }
+
+  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+                   acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
+                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
+                   acsParameters.rwHandlingParameters.rampTime);
+}
+
+ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
+                                              uint16_t dipoleTorqueDuration, int32_t rw1Speed,
+                                              int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed,
+                                              uint16_t rampTime) {
+  {
+    PoolReadGuard pg(&dipoleSet);
+    MutexGuard mg(torquer::lazyLock());
+    torquer::NEW_ACTUATION_FLAG = true;
+    dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
+  }
+  {
+    PoolReadGuard pg(&rw1SpeedSet);
+    rw1SpeedSet.setRwSpeed(rw1Speed, rampTime);
+  }
+  {
+    PoolReadGuard pg(&rw2SpeedSet);
+    rw2SpeedSet.setRwSpeed(rw2Speed, rampTime);
+  }
+  {
+    PoolReadGuard pg(&rw3SpeedSet);
+    rw3SpeedSet.setRwSpeed(rw3Speed, rampTime);
+  }
+  {
+    PoolReadGuard pg(&rw4SpeedSet);
+    rw4SpeedSet.setRwSpeed(rw4Speed, rampTime);
+  }
+  return returnvalue::OK;
 }
 
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index d427bd5b..c79e4426 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -70,6 +70,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void modeChanged(Mode_t mode, Submode_t submode);
   void announceMode(bool recursive);
 
+  ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
+                                 uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
+                                 int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
+
   /* ACS Sensor Values */
   ACS::SensorValues sensorValues;
 

From 109560feb263f6078fb31db12fde99d9d091ae55 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 11:30:52 +0100
Subject: [PATCH 09/23] disabled actCmd from acsCtrl again for now

---
 mission/controller/AcsController.cpp | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 29bd06b6..7ed66045 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -186,9 +186,9 @@ void AcsController::performSafe() {
     }
   }
 
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
-                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
-                   acsParameters.rwHandlingParameters.rampTime);
+  //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
+  //                   acsParameters.rwHandlingParameters.rampTime);
 }
 
 void AcsController::performDetumble() {
@@ -238,9 +238,9 @@ void AcsController::performDetumble() {
     }
   }
 
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
-                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
-                   acsParameters.rwHandlingParameters.rampTime);
+  //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
+  //                   acsParameters.rwHandlingParameters.rampTime);
 }
 
 void AcsController::performPointingCtrl() {
@@ -406,10 +406,10 @@ void AcsController::performPointingCtrl() {
     }
   }
 
-  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
-                   acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
-                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
-                   acsParameters.rwHandlingParameters.rampTime);
+  //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  //                   acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
+  //                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
+  //                   acsParameters.rwHandlingParameters.rampTime);
 }
 
 ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,

From bc4c6c3a5466e66fac8a02e2f2c1245cdd2acd35 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 12:07:45 +0100
Subject: [PATCH 10/23] changelog

---
 CHANGELOG.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index d92d6113..1bbc760d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -20,11 +20,13 @@ change warranting a new major release:
 ## Added
 
 - First version of a TCS controller heater control loop.
+- Function for the ACS controller to command MTQ and RWs called by all subroutines
 
 ## Changed
 
 - Reworked dummy handling for the TCS controller.
 - Generator scripts now generate files for hosted and for Q7S build.
+- ActCmds now returns command vectors as integers as required by the actuators
 
 # [v1.26.2] 2023-02-08
 

From 33684f2ef7eeaba440382d3a5a51dfcd61d5724c Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 12:48:17 +0100
Subject: [PATCH 11/23] fixed imtq setDipoles check

---
 mission/devices/devicedefinitions/imtqHandlerDefinitions.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h
index 2abf1c21..c3bc3d36 100644
--- a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h
+++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h
@@ -492,14 +492,14 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
   void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_,
                   uint16_t currentTorqueDurationMs_) {
     if (xDipole.value != xDipole_) {
+      xDipole = xDipole_;
     }
-    xDipole = xDipole_;
     if (yDipole.value != yDipole_) {
+      yDipole = yDipole_;
     }
-    yDipole = yDipole_;
     if (zDipole.value != zDipole_) {
+      zDipole = zDipole_;
     }
-    zDipole = zDipole_;
     currentTorqueDurationMs = currentTorqueDurationMs_;
   }
 

From 08df102f3684b880c295a64b374793e84eef87e0 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 13:16:50 +0100
Subject: [PATCH 12/23] rwHandler rwSpeedActuationSet handling

---
 mission/devices/RwHandler.cpp | 62 +++++++++++++++++++++++------------
 mission/devices/RwHandler.h   | 12 ++++---
 2 files changed, 49 insertions(+), 25 deletions(-)

diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp
index e9383c4a..7a8b52f3 100644
--- a/mission/devices/RwHandler.cpp
+++ b/mission/devices/RwHandler.cpp
@@ -13,7 +13,8 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki
       enableGpio(enableGpio),
       statusSet(this),
       lastResetStatusSet(this),
-      tmDataset(this) {
+      tmDataset(this),
+      rwSpeedActuationSet(*this) {
   if (comCookie == nullptr) {
     sif::error << "RwHandler: Invalid com cookie" << std::endl;
   }
@@ -97,16 +98,36 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
       return returnvalue::OK;
     }
     case (RwDefinitions::SET_SPEED): {
-      if (commandDataLen != 6) {
+      if (commandData != nullptr && commandDataLen != 6) {
         sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
                    << " invalid length" << std::endl;
         return SET_SPEED_COMMAND_INVALID_LENGTH;
       }
-      result = checkSpeedAndRampTime(commandData, commandDataLen);
+      // Commands override anything which was set in the software
+      if (commandData != nullptr) {
+        rwSpeedActuationSet.setValidityBufferGeneration(false);
+        result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen,
+                                                 SerializeIF::Endianness::NETWORK);
+        rwSpeedActuationSet.setValidityBufferGeneration(true);
+        if (result != returnvalue::OK) {
+          return result;
+        }
+      } else {
+        // Read set rw speed value from local pool
+        PoolReadGuard pg(&rwSpeedActuationSet);
+      }
+      if (ACTUATION_WIRETAPPING) {
+        int32_t speed;
+        uint16_t rampTime;
+        rwSpeedActuationSet.getRwSpeed(speed, rampTime);
+        sif::debug << "Actuating RW with speed = " << speed << " and rampTime = " << rampTime
+                   << std::endl;
+      }
+      result = checkSpeedAndRampTime();
       if (result != returnvalue::OK) {
         return result;
       }
-      prepareSetSpeedCmd(commandData, commandDataLen);
+      result = prepareSetSpeedCmd();
       return result;
     }
     case (RwDefinitions::GET_TEMPERATURE): {
@@ -298,17 +319,15 @@ void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
   rawPacketLen = 3;
 }
 
-ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) {
-  int32_t speed =
-      *commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3);
-
+ReturnValue_t RwHandler::checkSpeedAndRampTime() {
+  int32_t speed = 0;
+  uint16_t rampTime = 0;
+  rwSpeedActuationSet.getRwSpeed(speed, rampTime);
   if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
     sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
     return INVALID_SPEED;
   }
 
-  uint16_t rampTime = (*(commandData + 4) << 8) | *(commandData + 5);
-
   if (rampTime < 10 || rampTime > 20000) {
     sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl;
     return INVALID_RAMP_TIME;
@@ -317,23 +336,24 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
   return returnvalue::OK;
 }
 
-void RwHandler::prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen) {
+ReturnValue_t RwHandler::prepareSetSpeedCmd() {
   commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);
-
-  /** Speed (0.1 RPM) */
-  commandBuffer[1] = *(commandData + 3);
-  commandBuffer[2] = *(commandData + 2);
-  commandBuffer[3] = *(commandData + 1);
-  commandBuffer[4] = *commandData;
-  /** Ramp time (ms) */
-  commandBuffer[5] = *(commandData + 5);
-  commandBuffer[6] = *(commandData + 4);
+  uint8_t* serPtr = commandBuffer + 1;
+  size_t serSize = 1;
+  rwSpeedActuationSet.setValidityBufferGeneration(false);
+  ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
+                                                       SerializeIF::Endianness::LITTLE);
+  rwSpeedActuationSet.setValidityBufferGeneration(true);
+  if (result != returnvalue::OK) {
+    return result;
+  }
 
   uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
   commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
-  commandBuffer[8] = static_cast<uint8_t>(crc >> 8 & 0xFF);
+  commandBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
   rawPacket = commandBuffer;
   rawPacketLen = 9;
+  return result;
 }
 
 void RwHandler::handleResetStatusReply(const uint8_t* packet) {
diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h
index 17f2b396..af59caa1 100644
--- a/mission/devices/RwHandler.h
+++ b/mission/devices/RwHandler.h
@@ -9,6 +9,8 @@
 #include "events/subsystemIdRanges.h"
 #include "returnvalues/classIds.h"
 
+static constexpr bool ACTUATION_WIRETAPPING = false;
+
 class GpioIF;
 
 /**
@@ -93,6 +95,7 @@ class RwHandler : public DeviceHandlerBase {
   RwDefinitions::StatusSet statusSet;
   RwDefinitions::LastResetSatus lastResetStatusSet;
   RwDefinitions::TmDataset tmDataset;
+  RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet;
 
   uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
 
@@ -117,13 +120,14 @@ class RwHandler : public DeviceHandlerBase {
    *          range.
    * @return  returnvalue::OK if successful, otherwise error code.
    */
-  ReturnValue_t checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t checkSpeedAndRampTime();
 
   /**
-   * @brief   This function prepares the set speed command from the commandData received with
-   *          an action message.
+   * @brief   This function prepares the set speed command from the dataSet received with
+   *          an action message or set in the software.
+   * @return  returnvalue::OK if successful, otherwise error code.
    */
-  void prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareSetSpeedCmd();
 
   /**
    * @brief   This function writes the last reset status retrieved with the get last reset status

From 285bd7116b2a8b08472e6ccfcc39dbce2362b678 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 13:17:52 +0100
Subject: [PATCH 13/23] bump changelog

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 1bbc760d..6f1896d6 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -21,6 +21,7 @@ change warranting a new major release:
 
 - First version of a TCS controller heater control loop.
 - Function for the ACS controller to command MTQ and RWs called by all subroutines
+- RwHandler now handles commanding of RW speeds via RwSpeedActuationSet
 
 ## Changed
 

From 35172185f9da2b55ea0e85018bf3a5543579519b Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 10 Feb 2023 13:29:13 +0100
Subject: [PATCH 14/23] fix

---
 mission/controller/acs/AcsParameters.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index 30264963..44bed15f 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -280,6 +280,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           break;
         case 0x5:
           parameterWrapper->set(rwHandlingParameters.rampTime);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

From f4f6d1ed819f69a0291092584029fb6956a1467a Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Feb 2023 10:33:08 +0100
Subject: [PATCH 15/23] added InternalState SET_SPEED to enable setting RW
 Speed from ACS Ctrl

---
 mission/devices/RwHandler.cpp |  7 ++++++-
 mission/devices/RwHandler.h   | 10 ++++++++--
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp
index 7a8b52f3..88857b68 100644
--- a/mission/devices/RwHandler.cpp
+++ b/mission/devices/RwHandler.cpp
@@ -53,7 +53,11 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
       break;
     case InternalState::GET_RW_SATUS:
       *id = RwDefinitions::GET_RW_STATUS;
-      internalState = InternalState::GET_RESET_STATUS;
+      internalState = InternalState::SET_SPEED;
+      break;
+    case InternalState::SET_SPEED:
+      *id = RwDefinitions::SET_SPEED;
+      internalState = InternalState::CLEAR_RESET_STATUS;
       break;
     case InternalState::CLEAR_RESET_STATUS:
       *id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
@@ -98,6 +102,7 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
       return returnvalue::OK;
     }
     case (RwDefinitions::SET_SPEED): {
+      sif::debug << "hello" << std::endl;
       if (commandData != nullptr && commandDataLen != 6) {
         sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
                    << " invalid length" << std::endl;
diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h
index af59caa1..148a6359 100644
--- a/mission/devices/RwHandler.h
+++ b/mission/devices/RwHandler.h
@@ -9,7 +9,7 @@
 #include "events/subsystemIdRanges.h"
 #include "returnvalues/classIds.h"
 
-static constexpr bool ACTUATION_WIRETAPPING = false;
+static constexpr bool ACTUATION_WIRETAPPING = true;
 
 class GpioIF;
 
@@ -102,7 +102,13 @@ class RwHandler : public DeviceHandlerBase {
   PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>(0, false);
   PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>(10, false);
 
-  enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS };
+  enum class InternalState {
+    GET_RESET_STATUS,
+    CLEAR_RESET_STATUS,
+    READ_TEMPERATURE,
+    SET_SPEED,
+    GET_RW_SATUS
+  };
 
   InternalState internalState = InternalState::GET_RESET_STATUS;
 

From 53f31ca5a4df78887aaf3e22090159eb35d8509c Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Feb 2023 10:42:05 +0100
Subject: [PATCH 16/23] lol

---
 .../pollingSequenceFactory.cpp                | 123 ++++++++++++++++++
 1 file changed, 123 insertions(+)

diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
index f9b69e45..6b184f58 100644
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@@ -639,6 +639,129 @@ ReturnValue_t pst::pstAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) {
   }
 
   if (cfg.scheduleRws) {
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
+
+    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
+  }
+
+  if (cfg.scheduleRws) {
+    // this is the torquing cycle
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::PERFORM_OPERATION);
+
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::SEND_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::GET_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+
     thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
     thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD,

From 14c43c49dca652396a161dc083cb99acd6ed1ba8 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Feb 2023 10:58:12 +0100
Subject: [PATCH 17/23] scale rwCmdSpeed to appropriate range

---
 mission/controller/acs/ActuatorCmd.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp
index cbe24e5b..b902593f 100644
--- a/mission/controller/acs/ActuatorCmd.cpp
+++ b/mission/controller/acs/ActuatorCmd.cpp
@@ -57,6 +57,7 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
     deltaSpeedInt[i] = std::round(deltaSpeed[i]);
   }
   VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
+  VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
 }
 
 void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator) {

From 1d20c3b472e8e1d7e617d54204465a0c41302ebe Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Feb 2023 11:04:43 +0100
Subject: [PATCH 18/23] changelog

---
 CHANGELOG.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 1dbd2f9c..57209a3f 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -26,6 +26,8 @@ change warranting a new major release:
 ## Changed
 
 - ActCmds now returns command vectors as integers as required by the actuators
+  and scales them to the appropriate range
+- All RwHandler are now polled five times per ACS cycle
 
 # [v1.27.0] 2023-02-13
 

From 068b31a3d6bac46dd3feaac02670af10c89250de Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Feb 2023 11:07:09 +0100
Subject: [PATCH 19/23] changelog

---
 CHANGELOG.md | 1 -
 1 file changed, 1 deletion(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 57209a3f..d3d64dce 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -19,7 +19,6 @@ change warranting a new major release:
 
 ## Added
 
-- First version of a TCS controller heater control loop.
 - Function for the ACS controller to command MTQ and RWs called by all subroutines
 - RwHandler now handles commanding of RW speeds via RwSpeedActuationSet
 

From 99913594fda899dbc1b8f5a374502008d2afa66c Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Feb 2023 16:10:58 +0100
Subject: [PATCH 20/23] rfrmt

---
 .../pollingsequence/pollingSequenceFactory.cpp        | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
index 31606768..2944856e 100644
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@@ -240,7 +240,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
                             DeviceHandlerIF::GET_READ);
     }
-
     if (enableBside) {
       // B side
       thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
@@ -293,7 +292,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
     }
   }
   // SUS: 16 ms
-
   bool addSus0 = true;
   bool addSus1 = true;
   bool addSus2 = true;
@@ -441,7 +439,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::SUS_5_N_LOC_XFYMZB_PT_ZB,
                             length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
     }
-
     if (addSus6) {
       thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF, length * 0,
                             DeviceHandlerIF::PERFORM_OPERATION);
@@ -464,7 +461,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::SUS_6_R_LOC_XFYBZM_PT_XF,
                             length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
     }
-
     if (addSus7) {
       thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB, length * 0,
                             DeviceHandlerIF::PERFORM_OPERATION);
@@ -487,7 +483,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::SUS_7_R_LOC_XBYBZM_PT_XB,
                             length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
     }
-
     if (addSus8) {
       thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB, length * 0,
                             DeviceHandlerIF::PERFORM_OPERATION);
@@ -510,7 +505,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::SUS_8_R_LOC_XBYBZB_PT_YB,
                             length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
     }
-
     if (addSus9) {
       thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
                             DeviceHandlerIF::PERFORM_OPERATION);
@@ -522,6 +516,7 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF, length * 0,
                             DeviceHandlerIF::GET_READ);
+
       thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
                             length * config::acs::SCHED_BLOCK_1_PERIOD,
                             DeviceHandlerIF::SEND_WRITE);
@@ -532,7 +527,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::SUS_9_R_LOC_XBYBZB_PT_YF,
                             length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
     }
-
     if (addSus10) {
       thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, length * 0,
                             DeviceHandlerIF::PERFORM_OPERATION);
@@ -555,7 +549,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::SUS_10_N_LOC_XMYBZF_PT_ZF,
                             length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ);
     }
-
     if (addSus11) {
       thisSequence->addSlot(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, length * 0,
                             DeviceHandlerIF::PERFORM_OPERATION);
@@ -807,6 +800,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
     thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD,
                           DeviceHandlerIF::GET_READ);
   }
+
   thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0);
+
   return returnvalue::OK;
 }

From 1de832509a7d6383abe509145d2107346dbcf4dd Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Feb 2023 13:40:07 +0100
Subject: [PATCH 21/23] do a commit

---
 mission/devices/ImtqHandler.cpp | 4 ++++
 mission/devices/RwHandler.cpp   | 5 ++++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index 0e75c06d..b370b7d9 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -192,6 +192,10 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
         if (result != returnvalue::OK) {
           return result;
         }
+        result = dipoleSet.commit();
+        if (result != returnvalue::OK) {
+          sif::error << "ImtqHandler::buildCommandFromCommand: commit failed" << std::endl;
+        }
       } else {
         // Read set dipole values from local pool
         PoolReadGuard pg(&dipoleSet);
diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp
index 88857b68..5c6914be 100644
--- a/mission/devices/RwHandler.cpp
+++ b/mission/devices/RwHandler.cpp
@@ -102,7 +102,6 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
       return returnvalue::OK;
     }
     case (RwDefinitions::SET_SPEED): {
-      sif::debug << "hello" << std::endl;
       if (commandData != nullptr && commandDataLen != 6) {
         sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
                    << " invalid length" << std::endl;
@@ -117,6 +116,10 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
         if (result != returnvalue::OK) {
           return result;
         }
+        result = rwSpeedActuationSet.commit();
+        if (result != returnvalue::OK) {
+          sif::error << "RwHandler::buildCommandFromCommand: commit failed" << std::endl;
+        }
       } else {
         // Read set rw speed value from local pool
         PoolReadGuard pg(&rwSpeedActuationSet);

From e07492c85515126257dbeccdf6a7eaf230a301cf Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Feb 2023 14:10:37 +0100
Subject: [PATCH 22/23] bump tmtc

---
 tmtc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tmtc b/tmtc
index 9b7471e9..20e107c7 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 9b7471e9097edd410995ba0c76125b626440d9be
+Subproject commit 20e107c7ae373e7f36fca68957dbc36f4dd76f3b

From 166fd77f7b6a17486d9fc43813183ab63e76cb88 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Feb 2023 18:41:43 +0100
Subject: [PATCH 23/23] stupid RW

---
 bsp_q7s/core/ObjectFactory.cpp                |   4 +-
 .../pollingSequenceFactory.cpp                | 148 +++++++++---------
 mission/devices/ImtqHandler.cpp               |  26 ++-
 mission/devices/RwHandler.cpp                 |  48 +++---
 mission/devices/RwHandler.h                   |   9 +-
 5 files changed, 116 insertions(+), 119 deletions(-)

diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp
index 2bf36743..2631a628 100644
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@@ -683,8 +683,8 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF,
                                    RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED,
                                    &rwSpiCallback::spiCallback, nullptr);
     auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF,
-                                    rwGpioIds[idx]);
-    rwCookies[idx]->setCallbackArgs(rws[idx]);
+                                    rwGpioIds[idx], idx);
+    rwCookies[idx]->setCallbackArgs(rwHandler);
 #if OBSW_TEST_RW == 1
     rws[idx]->setStartUpImmediately();
 #endif
diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
index e404cdf7..7e4cafeb 100644
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@@ -633,80 +633,80 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
   }
 
   if (cfg.scheduleRws) {
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
-
-    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_WRITE);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_WRITE);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_WRITE);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::SEND_READ);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::SEND_READ);
+    //
+    //    thisSequence->addSlot(objects::RW1, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW2, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW3, length * 0, DeviceHandlerIF::GET_READ);
+    //    thisSequence->addSlot(objects::RW4, length * 0, DeviceHandlerIF::GET_READ);
   }
 
   if (cfg.scheduleRws) {
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index b370b7d9..93e9dae8 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -183,22 +183,20 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
         return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
       }
       ReturnValue_t result;
-      // Commands override anything which was set in the software
-      if (commandData != nullptr) {
-        dipoleSet.setValidityBufferGeneration(false);
-        result =
-            dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
-        dipoleSet.setValidityBufferGeneration(true);
-        if (result != returnvalue::OK) {
-          return result;
-        }
-        result = dipoleSet.commit();
-        if (result != returnvalue::OK) {
-          sif::error << "ImtqHandler::buildCommandFromCommand: commit failed" << std::endl;
-        }
-      } else {
+      {
         // Read set dipole values from local pool
         PoolReadGuard pg(&dipoleSet);
+
+        // Commands override anything which was set in the software
+        if (commandData != nullptr) {
+          dipoleSet.setValidityBufferGeneration(false);
+          result = dipoleSet.deSerialize(&commandData, &commandDataLen,
+                                         SerializeIF::Endianness::NETWORK);
+          dipoleSet.setValidityBufferGeneration(true);
+          if (result != returnvalue::OK) {
+            return result;
+          }
+        }
       }
       if (ACTUATION_WIRETAPPING) {
         sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp
index 5c6914be..9d4a7255 100644
--- a/mission/devices/RwHandler.cpp
+++ b/mission/devices/RwHandler.cpp
@@ -7,14 +7,15 @@
 #include "OBSWConfig.h"
 
 RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
-                     GpioIF* gpioComIF, gpioId_t enableGpio)
+                     GpioIF* gpioComIF, gpioId_t enableGpio, uint8_t rwIdx)
     : DeviceHandlerBase(objectId, comIF, comCookie),
       gpioComIF(gpioComIF),
       enableGpio(enableGpio),
       statusSet(this),
       lastResetStatusSet(this),
       tmDataset(this),
-      rwSpeedActuationSet(*this) {
+      rwSpeedActuationSet(*this),
+      rwIdx(rwIdx) {
   if (comCookie == nullptr) {
     sif::error << "RwHandler: Invalid com cookie" << std::endl;
   }
@@ -43,6 +44,10 @@ void RwHandler::doShutDown() {
 
 ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
   switch (internalState) {
+    case InternalState::SET_SPEED:
+      *id = RwDefinitions::SET_SPEED;
+      internalState = InternalState::GET_RESET_STATUS;
+      break;
     case InternalState::GET_RESET_STATUS:
       *id = RwDefinitions::GET_LAST_RESET_STATUS;
       internalState = InternalState::READ_TEMPERATURE;
@@ -53,10 +58,6 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
       break;
     case InternalState::GET_RW_SATUS:
       *id = RwDefinitions::GET_RW_STATUS;
-      internalState = InternalState::SET_SPEED;
-      break;
-    case InternalState::SET_SPEED:
-      *id = RwDefinitions::SET_SPEED;
       internalState = InternalState::CLEAR_RESET_STATUS;
       break;
     case InternalState::CLEAR_RESET_STATUS:
@@ -107,29 +108,26 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
                    << " invalid length" << std::endl;
         return SET_SPEED_COMMAND_INVALID_LENGTH;
       }
-      // Commands override anything which was set in the software
-      if (commandData != nullptr) {
-        rwSpeedActuationSet.setValidityBufferGeneration(false);
-        result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen,
-                                                 SerializeIF::Endianness::NETWORK);
-        rwSpeedActuationSet.setValidityBufferGeneration(true);
-        if (result != returnvalue::OK) {
-          return result;
-        }
-        result = rwSpeedActuationSet.commit();
-        if (result != returnvalue::OK) {
-          sif::error << "RwHandler::buildCommandFromCommand: commit failed" << std::endl;
-        }
-      } else {
-        // Read set rw speed value from local pool
+
+      {
         PoolReadGuard pg(&rwSpeedActuationSet);
+        // Commands override anything which was set in the software
+        if (commandData != nullptr) {
+          rwSpeedActuationSet.setValidityBufferGeneration(false);
+          result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen,
+                                                   SerializeIF::Endianness::NETWORK);
+          rwSpeedActuationSet.setValidityBufferGeneration(true);
+          if (result != returnvalue::OK) {
+            return result;
+          }
+        }
       }
       if (ACTUATION_WIRETAPPING) {
-        int32_t speed;
-        uint16_t rampTime;
+        int32_t speed = 0;
+        uint16_t rampTime = 0;
         rwSpeedActuationSet.getRwSpeed(speed, rampTime);
-        sif::debug << "Actuating RW with speed = " << speed << " and rampTime = " << rampTime
-                   << std::endl;
+        sif::debug << "Actuating RW " << static_cast<int>(rwIdx) << " with speed = " << speed
+                   << " and rampTime = " << rampTime << std::endl;
       }
       result = checkSpeedAndRampTime();
       if (result != returnvalue::OK) {
diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h
index 148a6359..ee18960d 100644
--- a/mission/devices/RwHandler.h
+++ b/mission/devices/RwHandler.h
@@ -9,7 +9,7 @@
 #include "events/subsystemIdRanges.h"
 #include "returnvalues/classIds.h"
 
-static constexpr bool ACTUATION_WIRETAPPING = true;
+static constexpr bool ACTUATION_WIRETAPPING = false;
 
 class GpioIF;
 
@@ -36,7 +36,7 @@ class RwHandler : public DeviceHandlerBase {
    * 						to high to enable the device.
    */
   RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, GpioIF* gpioComIF,
-            gpioId_t enableGpio);
+            gpioId_t enableGpio, uint8_t rwIdx);
 
   void setDebugMode(bool enable);
 
@@ -98,9 +98,10 @@ class RwHandler : public DeviceHandlerBase {
   RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet;
 
   uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
+  uint8_t rwIdx;
 
-  PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>(0, false);
-  PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>(10, false);
+  PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>({0});
+  PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>({10});
 
   enum class InternalState {
     GET_RESET_STATUS,