diff --git a/lib/APPConvoyFollower/src/App.cpp b/lib/APPConvoyFollower/src/App.cpp
index 76253f32..eb480a5f 100644
--- a/lib/APPConvoyFollower/src/App.cpp
+++ b/lib/APPConvoyFollower/src/App.cpp
@@ -42,6 +42,7 @@
 #include <DifferentialDrive.h>
 #include <Odometry.h>
 #include <Logging.h>
+#include <Util.h>
 
 /******************************************************************************
  * Compiler Switches
@@ -189,7 +190,8 @@ void App::handleRemoteCommand(const Command& cmd)
         break;
 
     case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED:
-        rsp.maxMotorSpeed = Board::getInstance().getSettings().getMaxSpeed();
+        rsp.maxMotorSpeed =
+            Util::stepsPerSecondToMillimetersPerSecond(Board::getInstance().getSettings().getMaxSpeed());
         break;
 
     case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS:
@@ -252,6 +254,9 @@ void App::reportVehicleData()
     uint8_t            averageCounts = 0U;
     uint8_t            leftCounts    = 0U;
     uint8_t            rightCounts   = 0U;
+    int16_t            leftSpeed     = speedometer.getLinearSpeedLeft();
+    int16_t            rightSpeed    = speedometer.getLinearSpeedRight();
+    int16_t            centerSpeed   = speedometer.getLinearSpeedCenter();
 
     proximitySensors.read();
     leftCounts  = proximitySensors.countsFrontWithLeftLeds();
@@ -265,9 +270,9 @@ void App::reportVehicleData()
     payload.xPos        = xPos;
     payload.yPos        = yPos;
     payload.orientation = odometry.getOrientation();
-    payload.left        = speedometer.getLinearSpeedLeft();
-    payload.right       = speedometer.getLinearSpeedRight();
-    payload.center      = speedometer.getLinearSpeedCenter();
+    payload.left        = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed);
+    payload.right       = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed);
+    payload.center      = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed);
     payload.proximity   = static_cast<SMPChannelPayload::Range>(averageCounts);
 
     /* Ignoring return value, as error handling is not available. */
@@ -359,7 +364,9 @@ void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_
     if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize))
     {
         const SpeedData* motorSpeedData = reinterpret_cast<const SpeedData*>(payload);
-        DrivingState::getInstance().setTargetSpeeds(motorSpeedData->left, motorSpeedData->right);
+        int16_t          leftSpeed      = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->left);
+        int16_t          rightSpeed     = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->right);
+        DrivingState::getInstance().setTargetSpeeds(leftSpeed, rightSpeed);
     }
 }
 
diff --git a/lib/APPConvoyFollower/src/SerialMuxChannels.h b/lib/APPConvoyFollower/src/SerialMuxChannels.h
index c79b9754..29838c30 100644
--- a/lib/APPConvoyFollower/src/SerialMuxChannels.h
+++ b/lib/APPConvoyFollower/src/SerialMuxChannels.h
@@ -169,16 +169,16 @@ typedef struct _CommandResponse
     /** Response Payload. */
     union
     {
-        int16_t maxMotorSpeed; /**< Max speed [steps/s]. */
+        int32_t maxMotorSpeed; /**< Max speed [mm/s]. */
     };
 } __attribute__((packed)) CommandResponse;
 
 /** Struct of the "Speed" channel payload. */
 typedef struct _SpeedData
 {
-    int16_t left;   /**< Left motor speed [steps/s] */
-    int16_t right;  /**< Right motor speed [steps/s] */
-    int16_t center; /**< Center motor speed [steps/s] */
+    int32_t left;   /**< Left motor speed [mm/s] */
+    int32_t right;  /**< Right motor speed [mm/s] */
+    int32_t center; /**< Center motor speed [mm/s] */
 } __attribute__((packed)) SpeedData;
 
 /** Struct of the "Current Vehicle Data" channel payload. */
@@ -187,9 +187,9 @@ typedef struct _VehicleData
     int32_t                  xPos;        /**< X position [mm]. */
     int32_t                  yPos;        /**< Y position [mm]. */
     int32_t                  orientation; /**< Orientation [mrad]. */
-    int16_t                  left;        /**< Left motor speed [steps/s]. */
-    int16_t                  right;       /**< Right motor speed [steps/s]. */
-    int16_t                  center;      /**< Center speed [steps/s]. */
+    int32_t                  left;        /**< Left motor speed [mm/s]. */
+    int32_t                  right;       /**< Right motor speed [mm/s]. */
+    int32_t                  center;      /**< Center speed [mm/s]. */
     SMPChannelPayload::Range proximity;   /**< Range at which object is found [range]. */
 } __attribute__((packed)) VehicleData;
 
diff --git a/lib/APPConvoyLeader/src/App.cpp b/lib/APPConvoyLeader/src/App.cpp
index b4ad13aa..a09da528 100644
--- a/lib/APPConvoyLeader/src/App.cpp
+++ b/lib/APPConvoyLeader/src/App.cpp
@@ -42,6 +42,7 @@
 #include <DifferentialDrive.h>
 #include <Odometry.h>
 #include <Logging.h>
+#include <Util.h>
 
 /******************************************************************************
  * Compiler Switches
@@ -182,7 +183,8 @@ void App::handleRemoteCommand(const Command& cmd)
     switch (cmd.commandId)
     {
     case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED:
-        rsp.maxMotorSpeed = ParameterSets::getInstance().getParameterSet().topSpeed;
+        rsp.maxMotorSpeed =
+            Util::stepsPerSecondToMillimetersPerSecond(ParameterSets::getInstance().getParameterSet().topSpeed);
         break;
 
     case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS:
@@ -245,6 +247,9 @@ void App::reportVehicleData()
     uint8_t            averageCounts = 0U;
     uint8_t            leftCounts    = 0U;
     uint8_t            rightCounts   = 0U;
+    int16_t            leftSpeed     = speedometer.getLinearSpeedLeft();
+    int16_t            rightSpeed    = speedometer.getLinearSpeedRight();
+    int16_t            centerSpeed   = speedometer.getLinearSpeedCenter();
 
     proximitySensors.read();
     leftCounts  = proximitySensors.countsFrontWithLeftLeds();
@@ -258,9 +263,9 @@ void App::reportVehicleData()
     payload.xPos        = xPos;
     payload.yPos        = yPos;
     payload.orientation = odometry.getOrientation();
-    payload.left        = speedometer.getLinearSpeedLeft();
-    payload.right       = speedometer.getLinearSpeedRight();
-    payload.center      = speedometer.getLinearSpeedCenter();
+    payload.left        = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed);
+    payload.right       = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed);
+    payload.center      = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed);
     payload.proximity   = static_cast<SMPChannelPayload::Range>(averageCounts);
 
     /* Ignoring return value, as error handling is not available. */
@@ -331,7 +336,8 @@ void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_
     if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize))
     {
         const SpeedData* motorSpeedData = reinterpret_cast<const SpeedData*>(payload);
-        DrivingState::getInstance().setTopSpeed(motorSpeedData->center);
+        int16_t          topSpeed       = Util::millimetersPerSecondToStepsPerSecond(motorSpeedData->center);
+        DrivingState::getInstance().setTopSpeed(topSpeed);
     }
 }
 
diff --git a/lib/APPConvoyLeader/src/SerialMuxChannels.h b/lib/APPConvoyLeader/src/SerialMuxChannels.h
index a4440d77..396586e4 100644
--- a/lib/APPConvoyLeader/src/SerialMuxChannels.h
+++ b/lib/APPConvoyLeader/src/SerialMuxChannels.h
@@ -163,16 +163,16 @@ typedef struct _CommandResponse
     /** Response Payload. */
     union
     {
-        int16_t maxMotorSpeed; /**< Max speed [steps/s]. */
+        int32_t maxMotorSpeed; /**< Max speed [mm/s]. */
     };
 } __attribute__((packed)) CommandResponse;
 
 /** Struct of the "Speed" channel payload. */
 typedef struct _SpeedData
 {
-    int16_t left;   /**< Left motor speed [steps/s] */
-    int16_t right;  /**< Right motor speed [steps/s] */
-    int16_t center; /**< Center motor speed [steps/s] */
+    int32_t left;   /**< Left motor speed [mm/s] */
+    int32_t right;  /**< Right motor speed [mm/s] */
+    int32_t center; /**< Center motor speed [mm/s] */
 } __attribute__((packed)) SpeedData;
 
 /** Struct of the "Current Vehicle Data" channel payload. */
@@ -181,9 +181,9 @@ typedef struct _VehicleData
     int32_t                  xPos;        /**< X position [mm]. */
     int32_t                  yPos;        /**< Y position [mm]. */
     int32_t                  orientation; /**< Orientation [mrad]. */
-    int16_t                  left;        /**< Left motor speed [steps/s]. */
-    int16_t                  right;       /**< Right motor speed [steps/s]. */
-    int16_t                  center;      /**< Center speed [steps/s]. */
+    int32_t                  left;        /**< Left motor speed [mm/s]. */
+    int32_t                  right;       /**< Right motor speed [mm/s]. */
+    int32_t                  center;      /**< Center speed [mm/s]. */
     SMPChannelPayload::Range proximity;   /**< Range at which object is found [range]. */
 } __attribute__((packed)) VehicleData;
 
diff --git a/lib/APPRemoteControl/src/App.cpp b/lib/APPRemoteControl/src/App.cpp
index 76253f32..28675f2d 100644
--- a/lib/APPRemoteControl/src/App.cpp
+++ b/lib/APPRemoteControl/src/App.cpp
@@ -42,6 +42,7 @@
 #include <DifferentialDrive.h>
 #include <Odometry.h>
 #include <Logging.h>
+#include <Util.h>
 
 /******************************************************************************
  * Compiler Switches
@@ -189,7 +190,8 @@ void App::handleRemoteCommand(const Command& cmd)
         break;
 
     case SMPChannelPayload::CmdId::CMD_ID_GET_MAX_SPEED:
-        rsp.maxMotorSpeed = Board::getInstance().getSettings().getMaxSpeed();
+        rsp.maxMotorSpeed =
+            Util::stepsPerSecondToMillimetersPerSecond(Board::getInstance().getSettings().getMaxSpeed());
         break;
 
     case SMPChannelPayload::CmdId::CMD_ID_SET_INIT_POS:
@@ -252,6 +254,9 @@ void App::reportVehicleData()
     uint8_t            averageCounts = 0U;
     uint8_t            leftCounts    = 0U;
     uint8_t            rightCounts   = 0U;
+    int16_t            leftSpeed     = speedometer.getLinearSpeedLeft();
+    int16_t            rightSpeed    = speedometer.getLinearSpeedRight();
+    int16_t            centerSpeed   = speedometer.getLinearSpeedCenter();
 
     proximitySensors.read();
     leftCounts  = proximitySensors.countsFrontWithLeftLeds();
@@ -265,9 +270,9 @@ void App::reportVehicleData()
     payload.xPos        = xPos;
     payload.yPos        = yPos;
     payload.orientation = odometry.getOrientation();
-    payload.left        = speedometer.getLinearSpeedLeft();
-    payload.right       = speedometer.getLinearSpeedRight();
-    payload.center      = speedometer.getLinearSpeedCenter();
+    payload.left        = Util::stepsPerSecondToMillimetersPerSecond(leftSpeed);
+    payload.right       = Util::stepsPerSecondToMillimetersPerSecond(rightSpeed);
+    payload.center      = Util::stepsPerSecondToMillimetersPerSecond(centerSpeed);
     payload.proximity   = static_cast<SMPChannelPayload::Range>(averageCounts);
 
     /* Ignoring return value, as error handling is not available. */
diff --git a/lib/APPRemoteControl/src/SerialMuxChannels.h b/lib/APPRemoteControl/src/SerialMuxChannels.h
index c79b9754..29838c30 100644
--- a/lib/APPRemoteControl/src/SerialMuxChannels.h
+++ b/lib/APPRemoteControl/src/SerialMuxChannels.h
@@ -169,16 +169,16 @@ typedef struct _CommandResponse
     /** Response Payload. */
     union
     {
-        int16_t maxMotorSpeed; /**< Max speed [steps/s]. */
+        int32_t maxMotorSpeed; /**< Max speed [mm/s]. */
     };
 } __attribute__((packed)) CommandResponse;
 
 /** Struct of the "Speed" channel payload. */
 typedef struct _SpeedData
 {
-    int16_t left;   /**< Left motor speed [steps/s] */
-    int16_t right;  /**< Right motor speed [steps/s] */
-    int16_t center; /**< Center motor speed [steps/s] */
+    int32_t left;   /**< Left motor speed [mm/s] */
+    int32_t right;  /**< Right motor speed [mm/s] */
+    int32_t center; /**< Center motor speed [mm/s] */
 } __attribute__((packed)) SpeedData;
 
 /** Struct of the "Current Vehicle Data" channel payload. */
@@ -187,9 +187,9 @@ typedef struct _VehicleData
     int32_t                  xPos;        /**< X position [mm]. */
     int32_t                  yPos;        /**< Y position [mm]. */
     int32_t                  orientation; /**< Orientation [mrad]. */
-    int16_t                  left;        /**< Left motor speed [steps/s]. */
-    int16_t                  right;       /**< Right motor speed [steps/s]. */
-    int16_t                  center;      /**< Center speed [steps/s]. */
+    int32_t                  left;        /**< Left motor speed [mm/s]. */
+    int32_t                  right;       /**< Right motor speed [mm/s]. */
+    int32_t                  center;      /**< Center speed [mm/s]. */
     SMPChannelPayload::Range proximity;   /**< Range at which object is found [range]. */
 } __attribute__((packed)) VehicleData;