diff --git a/CHANGELOG.md b/CHANGELOG.md
index d01f4290..417f6522 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -17,8 +17,19 @@ change warranting a new major release:
 
 # [unreleased]
 
+## Added
+
+- Function for the ACS controller to command MTQ and RWs called by all subroutines
+- RwHandler now handles commanding of RW speeds via RwSpeedActuationSet
+- Tracing supports which allows checking whether threads are running as usual.
+
 ## Changed
 
+- Remove 2 TCS threads.
+- Move low level polling into ACS PST, move high level device handlers into TCS system task.
+- 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
 - Remove 2 TCS threads. Move low level polling into ACS PST, move high level device handlers into
   TCS system task.
 - Further reduce number of threads:
@@ -27,10 +38,6 @@ change warranting a new major release:
   3. Group all other components into PUS medium priority task
   4. Add SCEX device handler to PL task, remove dedicated thread
 
-## Added
-
-- Tracing supports which allows checking whether threads are running as usual.
-
 ## Removed
 
 - lwgps dependency not compiled anymore, is not used
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 d8df8e9a..7e4cafeb 100644
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@@ -241,7 +241,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,
@@ -294,7 +293,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
     }
   }
   // SUS: 16 ms
-
   bool addSus0 = true;
   bool addSus1 = true;
   bool addSus2 = true;
@@ -442,7 +440,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);
@@ -465,7 +462,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);
@@ -488,7 +484,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);
@@ -511,7 +506,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);
@@ -523,6 +517,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);
@@ -533,7 +528,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);
@@ -556,7 +550,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);
@@ -640,6 +633,129 @@ 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);
+  }
+
+  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,
@@ -685,6 +801,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;
 }
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 051fa8f0..220c477e 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -119,10 +119,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) {
@@ -140,8 +140,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);
@@ -183,18 +183,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(cmdDipolUnits[0], cmdDipolUnits[1], cmdDipolUnits[2],
-  //    torqueDuration);
-  //  }
+
+  //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
+  //                   acsParameters.rwHandlingParameters.rampTime);
 }
 
 void AcsController::performDetumble() {
@@ -211,8 +208,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) <
@@ -231,10 +228,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) {
@@ -242,18 +235,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);
-  //  }
+
+  //  commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
+  //                   acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
+  //                   acsParameters.rwHandlingParameters.rampTime);
 }
 
 void AcsController::performPointingCtrl() {
@@ -276,7 +266,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:
@@ -396,44 +386,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);
-  //  }
+
+  //  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 f5240b74..c0e719f2 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"
 #include "mission/trace.h"
@@ -74,11 +75,20 @@ 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;
 
-  /* 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);
diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index 13642fe9..44bed15f 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -278,6 +278,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x4:
           parameterWrapper->set(rwHandlingParameters.stictionTorque);
           break;
+        case 0x5:
+          parameterWrapper->set(rwHandlingParameters.rampTime);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -584,6 +587,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 {
diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp
index 4e0052e0..b902593f 100644
--- a/mission/controller/acs/ActuatorCmd.cpp
+++ b/mission/controller/acs/ActuatorCmd.cpp
@@ -38,27 +38,33 @@ 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);
+  VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, 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 +75,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:
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index 0e75c06d..93e9dae8 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -183,18 +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;
-        }
-      } 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 732bb9fe..9d4a7255 100644
--- a/mission/devices/RwHandler.cpp
+++ b/mission/devices/RwHandler.cpp
@@ -7,13 +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) {
+      tmDataset(this),
+      rwSpeedActuationSet(*this),
+      rwIdx(rwIdx) {
   if (comCookie == nullptr) {
     sif::error << "RwHandler: Invalid com cookie" << std::endl;
   }
@@ -42,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;
@@ -52,7 +58,7 @@ 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::CLEAR_RESET_STATUS;
       break;
     case InternalState::CLEAR_RESET_STATUS:
       *id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
@@ -97,16 +103,37 @@ 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);
+
+      {
+        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 = 0;
+        uint16_t rampTime = 0;
+        rwSpeedActuationSet.getRwSpeed(speed, rampTime);
+        sif::debug << "Actuating RW " << static_cast<int>(rwIdx) << " 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): {
@@ -243,6 +270,9 @@ 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}));
 
   localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>({0}));
@@ -295,17 +325,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;
@@ -314,23 +342,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 f9f66bd5..ee18960d 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;
 
 /**
@@ -34,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);
 
@@ -93,10 +95,21 @@ class RwHandler : public DeviceHandlerBase {
   RwDefinitions::StatusSet statusSet;
   RwDefinitions::LastResetSatus lastResetStatusSet;
   RwDefinitions::TmDataset tmDataset;
+  RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet;
 
   uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
+  uint8_t rwIdx;
 
-  enum class InternalState { GET_RESET_STATUS, CLEAR_RESET_STATUS, READ_TEMPERATURE, GET_RW_SATUS };
+  PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>({0});
+  PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>({10});
+
+  enum class InternalState {
+    GET_RESET_STATUS,
+    CLEAR_RESET_STATUS,
+    READ_TEMPERATURE,
+    SET_SPEED,
+    GET_RW_SATUS
+  };
 
   InternalState internalState = InternalState::GET_RESET_STATUS;
 
@@ -114,13 +127,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
diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h
index 2923b41b..b3cb0fe4 100644
--- a/mission/devices/devicedefinitions/RwDefinitions.h
+++ b/mission/devices/devicedefinitions/RwDefinitions.h
@@ -51,7 +51,10 @@ enum PoolIds : lp_id_t {
   SPI_BYTES_WRITTEN,
   SPI_BYTES_READ,
   SPI_REG_OVERRUN_ERRORS,
-  SPI_TOTAL_ERRORS
+  SPI_TOTAL_ERRORS,
+
+  RW_SPEED,
+  RAMP_TIME,
 };
 
 enum States : uint8_t { STATE_ERROR, IDLE, COASTING, RUNNING_SPEED_STABLE, RUNNING_SPEED_CHANGING };
@@ -75,10 +78,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,
+  SPEED_CMD_SET = 10,
+};
 
 static const size_t SIZE_GET_RESET_STATUS = 5;
 static const size_t SIZE_CLEAR_RESET_STATUS = 4;
@@ -106,9 +112,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 +132,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 +153,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 +204,36 @@ class TmDataset : public StaticLocalDataSet<TM_SET_ENTRIES> {
       lp_var_t<uint32_t>(sid.objectId, PoolIds::SPI_TOTAL_ERRORS, this);
 };
 
+class RwSpeedActuationSet : public StaticLocalDataSet<2> {
+  friend class RwHandler;
+
+ public:
+  RwSpeedActuationSet(HasLocalDataPoolIF& owner)
+      : StaticLocalDataSet(&owner, RwDefinitions::SetIds::SPEED_CMD_SET) {}
+  RwSpeedActuationSet(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::SPEED_CMD_SET)) {}
+
+  void setRwSpeed(int32_t rwSpeed_, uint16_t rampTime_) {
+    if (rwSpeed.value != rwSpeed_) {
+      rwSpeed = rwSpeed_;
+    }
+    if (rampTime.value != rampTime_) {
+      rampTime = rampTime_;
+    }
+  }
+
+  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
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */
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_;
   }
 
diff --git a/tmtc b/tmtc
index 20e107c7..1e143ea6 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 20e107c7ae373e7f36fca68957dbc36f4dd76f3b
+Subproject commit 1e143ea6faa608baf3118512416f5a495dbd606c