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: