diff --git a/msg/DifferentialDriveSetpoint.msg b/msg/DifferentialDriveSetpoint.msg
index fddcc24365..f7e4c58400 100644
--- a/msg/DifferentialDriveSetpoint.msg
+++ b/msg/DifferentialDriveSetpoint.msg
@@ -1,6 +1,8 @@
 uint64 timestamp # time since system start (microseconds)
 
 float32 speed # [m/s] collective roll-off speed in body x-axis
+bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward
 float32 yaw_rate # [rad/s] yaw rate
+bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward
 
 # TOPICS differential_drive_setpoint differential_drive_control_output
diff --git a/src/modules/differential_drive/CMakeLists.txt b/src/modules/differential_drive/CMakeLists.txt
index b562a8fc2c..2e0e3583b9 100644
--- a/src/modules/differential_drive/CMakeLists.txt
+++ b/src/modules/differential_drive/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,8 +31,9 @@
 #
 ############################################################################
 
-add_subdirectory(DifferentialDriveKinematics)
+add_subdirectory(DifferentialDriveControl)
 add_subdirectory(DifferentialDriveGuidance)
+add_subdirectory(DifferentialDriveKinematics)
 
 px4_add_module(
 	MODULE modules__differential_drive
@@ -41,9 +42,9 @@ px4_add_module(
 		DifferentialDrive.cpp
 		DifferentialDrive.hpp
 	DEPENDS
-		DifferentialDriveKinematics
+		DifferentialDriveControl
 		DifferentialDriveGuidance
-		pid
+		DifferentialDriveKinematics
 		px4_work_queue
 		modules__control_allocator # for parameter CA_R_REV
 	MODULE_CONFIG
diff --git a/src/modules/differential_drive/DifferentialDrive.cpp b/src/modules/differential_drive/DifferentialDrive.cpp
index 24e754d4e1..99f462a782 100644
--- a/src/modules/differential_drive/DifferentialDrive.cpp
+++ b/src/modules/differential_drive/DifferentialDrive.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,16 +33,11 @@
 
 #include "DifferentialDrive.hpp"
 
-using namespace time_literals;
-using namespace matrix;
-
 DifferentialDrive::DifferentialDrive() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	updateParams();
-	pid_init(&_angular_velocity_pid, PID_MODE_DERIVATIV_NONE, 0.001f);
-	pid_init(&_speed_pid, PID_MODE_DERIVATIV_NONE, 0.001f);
 }
 
 bool DifferentialDrive::init()
@@ -55,28 +50,15 @@ void DifferentialDrive::updateParams()
 {
 	ModuleParams::updateParams();
 
-	pid_set_parameters(&_angular_velocity_pid,
-			   _param_rdd_p_gain_angular_velocity.get(),  // Proportional gain
-			   _param_rdd_i_gain_angular_velocity.get(),  // Integral gain
-			   0,  // Derivative gain
-			   20,  // Integral limit
-			   200);  // Output limit
-
-	pid_set_parameters(&_speed_pid,
-			   _param_rdd_p_gain_speed.get(),  // Proportional gain
-			   _param_rdd_i_gain_speed.get(),  // Integral gain
-			   0,  // Derivative gain
-			   2,  // Integral limit
-			   200);  // Output limit
-
-	_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
-	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
-
 	_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
+
+	_max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get();
+	_differential_drive_guidance.setMaxSpeed(_max_speed);
 	_differential_drive_kinematics.setMaxSpeed(_max_speed);
-	_differential_guidance_controller.setMaxSpeed(_max_speed);
+
+	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
+	_differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity);
 	_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
-	_differential_guidance_controller.setMaxAngularVelocity(_max_angular_velocity);
 }
 
 void DifferentialDrive::Run()
@@ -87,13 +69,12 @@ void DifferentialDrive::Run()
 	}
 
 	hrt_abstime now = hrt_absolute_time();
-	const double dt = static_cast<double>(math::min((now - _time_stamp_last), kTimeoutUs)) / 1e6;
+	const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
 	_time_stamp_last = now;
 
 	if (_parameter_update_sub.updated()) {
 		parameter_update_s parameter_update;
 		_parameter_update_sub.copy(&parameter_update);
-
 		updateParams();
 	}
 
@@ -101,34 +82,12 @@ void DifferentialDrive::Run()
 		vehicle_control_mode_s vehicle_control_mode{};
 
 		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
-			_armed = vehicle_control_mode.flag_armed;
+			_differential_drive_kinematics.setArmed(vehicle_control_mode.flag_armed);
 			_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
 			_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
 		}
 	}
 
-	if (_vehicle_attitude_sub.updated()) {
-		vehicle_attitude_s vehicle_attitude{};
-
-		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
-			_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
-			_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
-		}
-	}
-
-	if (_vehicle_angular_velocity_sub.updated()) {
-		_vehicle_angular_velocity_sub.copy(&_vehicle_angular_velocity);
-	}
-
-	if (_vehicle_local_position_sub.updated()) {
-		vehicle_local_position_s vehicle_local_position{};
-
-		if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
-			Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-			_velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-		}
-	}
-
 	if (_manual_driving) {
 		// Manual mode
 		// directly produce setpoints from the manual control setpoint (joystick)
@@ -136,66 +95,26 @@ void DifferentialDrive::Run()
 			manual_control_setpoint_s manual_control_setpoint{};
 
 			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
-				_differential_drive_setpoint.timestamp = now;
-				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
-				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
-									_max_angular_velocity;
-				_differential_drive_control_output_pub.publish(_differential_drive_setpoint);
+				differential_drive_setpoint_s setpoint{};
+				setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed;
+				setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity;
+				setpoint.timestamp = now;
+				_differential_drive_setpoint_pub.publish(setpoint);
 			}
 		}
 
 	} else if (_mission_driving) {
 		// Mission mode
 		// directly receive setpoints from the guidance library
-		matrix::Vector2f guidance_output =
-			_differential_guidance_controller.computeGuidance(
-				_vehicle_yaw,
-				_vehicle_angular_velocity.xyz[2],
-				dt
-			);
-
-		_differential_drive_setpoint.timestamp = now;
-		_differential_drive_setpoint.speed = guidance_output(0);
-		_differential_drive_setpoint.yaw_rate = guidance_output(1);
-		_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
+		_differential_drive_guidance.computeGuidance(
+			_differential_drive_control.getVehicleYaw(),
+			_differential_drive_control.getVehicleBodyYawRate(),
+			dt
+		);
 	}
 
-	// check if the topic is updated and update the setpoint
-	if (_differential_drive_control_output_sub.updated()) {
-		_differential_drive_control_output_sub.copy(&_differential_drive_setpoint);
-
-		_speed_pid_output = 0;
-		_angular_velocity_pid_output = 0;
-	}
-
-	if (_differential_drive_setpoint_sub.updated()) {
-		_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint);
-
-		_speed_pid_output = pid_calculate(&_speed_pid, _differential_drive_setpoint.speed, _velocity_in_body_frame(0), 0, dt);
-		_angular_velocity_pid_output = pid_calculate(&_angular_velocity_pid, _differential_drive_setpoint.yaw_rate,
-					       _vehicle_angular_velocity.xyz[2], 0, dt);
-	}
-
-	// get the normalized wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
-	Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
-					_differential_drive_setpoint.speed + _speed_pid_output,
-					_differential_drive_setpoint.yaw_rate + _angular_velocity_pid_output);
-
-	// Check if max_angular_wheel_speed is zero
-	const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
-	const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
-
-	if (!_armed || setpoint_timeout || !valid_max_speed) {
-		wheel_speeds = {}; // stop
-	}
-
-	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
-
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
-	wheel_speeds.copyTo(actuator_motors.control);
-	actuator_motors.timestamp = now;
-	_actuator_motors_pub.publish(actuator_motors);
+	_differential_drive_control.control(dt);
+	_differential_drive_kinematics.allocate();
 }
 
 int DifferentialDrive::task_spawn(int argc, char *argv[])
diff --git a/src/modules/differential_drive/DifferentialDrive.hpp b/src/modules/differential_drive/DifferentialDrive.hpp
index 9090e27237..6c5805596e 100644
--- a/src/modules/differential_drive/DifferentialDrive.hpp
+++ b/src/modules/differential_drive/DifferentialDrive.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,45 +33,24 @@
 
 #pragma once
 
-// PX4 includes
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
-// uORB includes
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_angular_velocity.h>
-#include <uORB/topics/vehicle_local_position.h>
-
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_attitude.h>
-
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/differential_drive_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
-// Standard library includes
-#include <math.h>
-#include <lib/pid/pid.h>
-
-// Local includes
-#include <DifferentialDriveKinematics.hpp>
-#include <DifferentialDriveGuidance.hpp>
+#include "DifferentialDriveControl/DifferentialDriveControl.hpp"
+#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp"
+#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp"
 
 using namespace time_literals;
 
-static constexpr uint64_t kTimeoutUs = 5000_ms; // Maximal time in microseconds before a loop or data times out
-
 class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModuleParams,
 	public px4::ScheduledWorkItem
 {
@@ -95,55 +74,28 @@ protected:
 
 private:
 	void Run() override;
-
-	uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)};
-	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 
-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
-	uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
 	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
 
-	differential_drive_setpoint_s _differential_drive_setpoint{};
-	vehicle_angular_velocity_s _vehicle_angular_velocity{};
-	bool _armed = false;
 	bool _manual_driving = false;
 	bool _mission_driving = false;
-
 	hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
 
-	DifferentialDriveKinematics _differential_drive_kinematics;
-	DifferentialDriveGuidance _differential_guidance_controller{this};
+	DifferentialDriveGuidance _differential_drive_guidance{this};
+	DifferentialDriveControl _differential_drive_control{this};
+	DifferentialDriveKinematics _differential_drive_kinematics{this};
 
 	float _max_speed{0.f};
 	float _max_angular_velocity{0.f};
 
-	matrix::Quatf _vehicle_attitude_quaternion{};
-	float _vehicle_yaw{0.f};
-	Vector3f _velocity_in_body_frame{0.f, 0.f, 0.f};
-
-	PID_t _angular_velocity_pid; ///< The PID controller for yaw rate.
-	PID_t _speed_pid; ///< The PID controller for velocity.
-
-	float _speed_pid_output{0.f};
-	float _angular_velocity_pid_output{0.f};
-
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
-		(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
-		(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
-		(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity,
-		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
 		(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
-		(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
+		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
 		(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
-		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_wheel_speed,
+		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius
 	)
 };
diff --git a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt
new file mode 100644
index 0000000000..4610fa6c3d
--- /dev/null
+++ b/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(DifferentialDriveControl
+	DifferentialDriveControl.cpp
+)
+
+target_link_libraries(DifferentialDriveControl PUBLIC pid)
+target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp
new file mode 100644
index 0000000000..728c5e666a
--- /dev/null
+++ b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "DifferentialDriveControl.hpp"
+
+using namespace matrix;
+
+DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+void DifferentialDriveControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_pid_angular_velocity,
+			   _param_rdd_p_gain_angular_velocity.get(), // Proportional gain
+			   _param_rdd_i_gain_angular_velocity.get(), // Integral gain
+			   0, // Derivative gain
+			   20, // Integral limit
+			   200); // Output limit
+
+	pid_set_parameters(&_pid_speed,
+			   _param_rdd_p_gain_speed.get(), // Proportional gain
+			   _param_rdd_i_gain_speed.get(), // Integral gain
+			   0, // Derivative gain
+			   2, // Integral limit
+			   200); // Output limit
+}
+
+void DifferentialDriveControl::control(float dt)
+{
+	if (_vehicle_angular_velocity_sub.updated()) {
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+
+		if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
+			_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
+		}
+	}
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+
+		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+			_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
+			_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
+		}
+	}
+
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+
+		if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
+			Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+			Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+			_vehicle_forward_speed = velocity_in_body_frame(0);
+		}
+	}
+
+	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
+
+	// PID to reach setpoint using control_output
+	differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint;
+
+	if (_differential_drive_setpoint.closed_loop_speed_control) {
+		differential_drive_control_output.speed +=
+			pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt);
+	}
+
+	if (_differential_drive_setpoint.closed_loop_yaw_rate_control) {
+		differential_drive_control_output.yaw_rate +=
+			pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
+	}
+
+	differential_drive_control_output.timestamp = hrt_absolute_time();
+	_differential_drive_control_output_pub.publish(differential_drive_control_output);
+}
diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp
new file mode 100644
index 0000000000..8d3b7e1c21
--- /dev/null
+++ b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file DifferentialDriveControl.hpp
+ *
+ * Controller for heading rate and forward speed.
+ */
+
+#pragma once
+
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/differential_drive_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+class DifferentialDriveControl : public ModuleParams
+{
+public:
+	DifferentialDriveControl(ModuleParams *parent);
+	~DifferentialDriveControl() = default;
+
+	void control(float dt);
+	float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
+	float getVehicleYaw() const { return _vehicle_yaw; }
+
+protected:
+	void updateParams() override;
+
+private:
+	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
+
+	differential_drive_setpoint_s _differential_drive_setpoint{};
+
+	matrix::Quatf _vehicle_attitude_quaternion{};
+	float _vehicle_yaw{0.f};
+
+	// States
+	float _vehicle_body_yaw_rate{0.f};
+	float _vehicle_forward_speed{0.f};
+
+	PID_t _pid_angular_velocity; ///< The PID controller for yaw rate.
+	PID_t _pid_speed; ///< The PID controller for velocity.
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
+		(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
+		(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
+		(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity
+	)
+};
diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt
index 39be618c00..fa833c50ae 100644
--- a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt
+++ b/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -33,10 +33,6 @@
 
 px4_add_library(DifferentialDriveGuidance
 	DifferentialDriveGuidance.cpp
-	DifferentialDriveGuidance.hpp
 )
 
-target_compile_options(DifferentialDriveGuidance PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-# px4_add_unit_gtest(SRC DifferentialDriveGuidanceTest.cpp LINKLIBS DifferentialDriveGuidance)
diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
index 6352548d91..2dfc54af42 100644
--- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
+++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
 #include "DifferentialDriveGuidance.hpp"
 
 #include <mathlib/math/Limits.hpp>
@@ -11,7 +44,7 @@ DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : Mod
 	pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
 }
 
-matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, float angular_velocity, float dt)
+void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt)
 {
 	if (_position_setpoint_triplet_sub.updated()) {
 		_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
@@ -31,7 +64,7 @@ matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, f
 
 	float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
 				current_waypoint(1));
-	float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw);
+	float heading_error = matrix::wrap_pi(desired_heading - yaw);
 
 	const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
 				   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
@@ -54,7 +87,6 @@ matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, f
 		_currentState = GuidanceState::GOAL_REACHED;
 	}
 
-	matrix::Vector2f output;
 	float desired_speed = 0.f;
 
 	switch (_currentState) {
@@ -79,10 +111,12 @@ matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, f
 	// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
 	float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, dt);
 
-	output(0) = math::constrain(desired_speed, -_max_speed, _max_speed);
-	output(1) = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
-
-	return output;
+	differential_drive_setpoint_s output{};
+	output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
+	output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity);
+	output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true;
+	output.timestamp = hrt_absolute_time();
+	_differential_drive_setpoint_pub.publish(output);
 }
 
 void DifferentialDriveGuidance::updateParams()
diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
index 07a32ca485..4a07376983 100644
--- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
+++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,7 +33,6 @@
 
 #pragma once
 
-#include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 
 #include <matrix/matrix/math.hpp>
@@ -48,6 +47,7 @@
 
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/differential_drive_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
 
@@ -74,10 +74,6 @@ public:
 	 * @param parent The parent ModuleParams object.
 	 */
 	DifferentialDriveGuidance(ModuleParams *parent);
-
-	/**
-	 * @brief Default destructor.
-	 */
 	~DifferentialDriveGuidance() = default;
 
 	/**
@@ -89,10 +85,8 @@ public:
 	 * @param body_velocity The velocity of the vehicle in m/s.
 	 * @param angular_velocity The angular velocity of the vehicle in rad/s.
 	 * @param dt The time step in seconds.
-	 * @return A 2D vector containing the computed guidance (speed in m/s, yaw rate in rad/s).
 	 */
-	matrix::Vector2f computeGuidance(float vehicle_yaw,
-					 float angular_velocity, float dt);
+	void computeGuidance(float yaw, float angular_velocity, float dt);
 
 	/**
 	 * @brief Set the maximum speed for the vehicle.
@@ -116,10 +110,11 @@ protected:
 	void updateParams() override;
 
 private:
-
 	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
+
 	position_setpoint_triplet_s _position_setpoint_triplet{};
 	vehicle_global_position_s _vehicle_global_position{};
 
@@ -143,5 +138,4 @@ private:
 		(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
 		(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
 	)
-
 };
diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt
index c352d7c597..c556db2b8f 100644
--- a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt
+++ b/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -37,4 +37,4 @@ px4_add_library(DifferentialDriveKinematics
 
 target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
+px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp
index 6c28a0b38f..42fcbea56b 100644
--- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp
+++ b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2023-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,8 +36,38 @@
 #include <mathlib/mathlib.h>
 
 using namespace matrix;
+using namespace time_literals;
 
-matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
+DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent)
+{}
+
+void DifferentialDriveKinematics::allocate()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_differential_drive_control_output_sub.updated()) {
+		_differential_drive_control_output_sub.copy(&_differential_drive_control_output);
+	}
+
+	const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now;
+
+	Vector2f wheel_speeds =
+		computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate);
+
+	if (!_armed || setpoint_timeout) {
+		wheel_speeds = {}; // stop
+	}
+
+	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
+
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
+	wheel_speeds.copyTo(actuator_motors.control);
+	actuator_motors.timestamp = now;
+	_actuator_motors_pub.publish(actuator_motors);
+}
+
+matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
 {
 	if (_max_speed < FLT_EPSILON) {
 		return Vector2f();
@@ -54,7 +84,7 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin
 
 	if (combined_velocity > _max_speed) {
 		float excess_velocity = fabsf(combined_velocity - _max_speed);
-		float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
+		const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
 		gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
 	}
 
diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp
index 193a7d05fb..524a5520a7 100644
--- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp
+++ b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,6 +34,11 @@
 #pragma once
 
 #include <matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/differential_drive_setpoint.h>
 
 /**
  * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
@@ -41,10 +46,10 @@
  * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
  * given linear velocity and yaw rate.
  */
-class DifferentialDriveKinematics
+class DifferentialDriveKinematics : public ModuleParams
 {
 public:
-	DifferentialDriveKinematics() = default;
+	DifferentialDriveKinematics(ModuleParams *parent);
 	~DifferentialDriveKinematics() = default;
 
 	/**
@@ -68,6 +73,10 @@ public:
 	 */
 	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
 
+	void setArmed(const bool armed) { _armed = armed; };
+
+	void allocate();
+
 	/**
 	 * @brief Computes the inverse kinematics for differential drive.
 	 *
@@ -75,10 +84,20 @@ public:
 	 * @param yaw_rate Yaw rate of the robot.
 	 * @return matrix::Vector2f Motor velocities for the right and left motors.
 	 */
-	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
+	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const;
 
 private:
+	uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)};
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+
+	differential_drive_setpoint_s _differential_drive_control_output{};
+	bool _armed = false;
+
 	float _wheel_base{0.f};
 	float _max_speed{0.f};
 	float _max_angular_velocity{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
 };
diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp
index 6eee43c41e..b3c4935c1d 100644
--- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp
+++ b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2023-2024 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,9 +37,14 @@
 
 using namespace matrix;
 
-TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
+class DifferentialDriveKinematicsTest : public ::testing::Test
+{
+public:
+	DifferentialDriveKinematics kinematics{nullptr};
+};
+
+TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -49,9 +54,8 @@ TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
 }
 
 
-TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
+TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(0.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -61,9 +65,8 @@ TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
 }
 
 
-TEST(DifferentialDriveKinematicsTest, UnitCase)
+TEST_F(DifferentialDriveKinematicsTest, UnitCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(10.f);
 	kinematics.setMaxAngularVelocity(10.f);
@@ -73,9 +76,8 @@ TEST(DifferentialDriveKinematicsTest, UnitCase)
 }
 
 
-TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
+TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -85,9 +87,8 @@ TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
 }
 
 
-TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
+TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -96,9 +97,8 @@ TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
 }
 
-TEST(DifferentialDriveKinematicsTest, RandomCase)
+TEST_F(DifferentialDriveKinematicsTest, RandomCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(2.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -107,9 +107,8 @@ TEST(DifferentialDriveKinematicsTest, RandomCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
 }
 
-TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
+TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -118,9 +117,8 @@ TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
 }
 
-TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
+TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -129,9 +127,8 @@ TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
 }
 
-TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
+TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(FLT_MIN);
 	kinematics.setMaxSpeed(FLT_MIN);
 	kinematics.setMaxAngularVelocity(FLT_MIN);
@@ -140,9 +137,8 @@ TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
 }
 
-TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -151,9 +147,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
 }
 
-TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(1.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
@@ -162,9 +157,8 @@ TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
 	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
 }
 
-TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
+TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase)
 {
-	DifferentialDriveKinematics kinematics;
 	kinematics.setWheelBase(2.f);
 	kinematics.setMaxSpeed(1.f);
 	kinematics.setMaxAngularVelocity(1.f);
diff --git a/src/modules/differential_drive/module.yaml b/src/modules/differential_drive/module.yaml
index e2567a1c2c..50ab9bb837 100644
--- a/src/modules/differential_drive/module.yaml
+++ b/src/modules/differential_drive/module.yaml
@@ -43,7 +43,7 @@ parameters:
         increment: 0.01
         decimal: 2
         default: 1
-      RDD_WHL_SPEED:
+      RDD_WHEEL_SPEED:
         description:
           short: Maximum wheel speed
         type: float
diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml
index 2da2b99d00..255c8e0b54 100644
--- a/src/modules/uxrce_dds_client/dds_topics.yaml
+++ b/src/modules/uxrce_dds_client/dds_topics.yaml
@@ -127,9 +127,6 @@ subscriptions:
   - topic: /fmu/in/vehicle_rates_setpoint
     type: px4_msgs::msg::VehicleRatesSetpoint
 
-  - topic: /fmu/in/differential_drive_control_output
-    type: px4_msgs::msg::DifferentialDriveSetpoint
-
   - topic: /fmu/in/differential_drive_setpoint
     type: px4_msgs::msg::DifferentialDriveSetpoint