Rename module differential_drive_control -> differential_drive

src/modules/differential_drive/CMakeLists.txt

@@ -0,0 +1,51 @@
+############################################################################
+#
+#   Copyright (c) 2023 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.
+#
+############################################################################
+
+add_subdirectory(DifferentialDriveKinematics)
+add_subdirectory(DifferentialDriveGuidance)
+
+px4_add_module(
+	MODULE modules__differential_drive
+	MAIN differential_drive
+	SRCS
+		DifferentialDrive.cpp
+		DifferentialDrive.hpp
+	DEPENDS
+		DifferentialDriveKinematics
+		DifferentialDriveGuidance
+		pid
+		px4_work_queue
+		modules__control_allocator # for parameter CA_R_REV
+	MODULE_CONFIG
+		module.yaml
+)

src/modules/differential_drive/DifferentialDrive.cpp

@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "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()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+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());
+	_differential_drive_kinematics.setMaxSpeed(_max_speed);
+	_differential_guidance_controller.setMaxSpeed(_max_speed);
+	_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
+	_differential_guidance_controller.setMaxAngularVelocity(_max_angular_velocity);
+}
+
+void DifferentialDrive::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+	const double dt = static_cast<double>(math::min((now - _time_stamp_last), kTimeoutUs)) / 1e6;
+	_time_stamp_last = now;
+
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
+
+		updateParams();
+	}
+
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode{};
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = 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)
+		if (_manual_control_setpoint_sub.updated()) {
+			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);
+			}
+		}
+
+	} 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);
+	}
+
+	// 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);
+}
+
+int DifferentialDrive::task_spawn(int argc, char *argv[])
+{
+	DifferentialDrive *instance = new DifferentialDrive();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int DifferentialDrive::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int DifferentialDrive::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Rover Differential Drive controller.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("differential_drive", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int differential_drive_main(int argc, char *argv[])
+{
+	return DifferentialDrive::main(argc, argv);
+}

src/modules/differential_drive/DifferentialDrive.hpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#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>
+
+// Standard library includes
+#include <math.h>
+#include <lib/pid/pid.h>
+
+// Local includes
+#include <DifferentialDriveKinematics.hpp>
+#include <DifferentialDriveGuidance.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
+{
+public:
+	DifferentialDrive();
+	~DifferentialDrive() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+protected:
+	void updateParams() override;
+
+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};
+
+	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_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
+	)
+};

src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2023 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(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)

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp

@@ -0,0 +1,102 @@
+#include "DifferentialDriveGuidance.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+
+	pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, float angular_velocity, float dt)
+{
+	if (_position_setpoint_triplet_sub.updated()) {
+		_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
+	}
+
+	if (_vehicle_global_position_sub.updated()) {
+		_vehicle_global_position_sub.copy(&_vehicle_global_position);
+	}
+
+	matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon);
+	matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon);
+	matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon);
+
+	const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1),
+					  current_waypoint(0),
+					  current_waypoint(1));
+
+	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);
+
+	const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
+				   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
+
+	_forwards_velocity_smoothing.updateDurations(max_velocity);
+	_forwards_velocity_smoothing.updateTraj(dt);
+
+	if (_next_waypoint != next_waypoint) {
+		if (fabsf(heading_error) < 0.1f) {
+			_currentState = GuidanceState::DRIVING;
+
+		} else {
+			_currentState = GuidanceState::TURNING;
+		}
+
+	}
+
+	// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
+	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
+		_currentState = GuidanceState::GOAL_REACHED;
+	}
+
+	matrix::Vector2f output;
+	float desired_speed = 0.f;
+
+	switch (_currentState) {
+	case GuidanceState::TURNING:
+		desired_speed = 0.f;
+		break;
+
+	case GuidanceState::DRIVING:
+		desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.4f,
+				_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
+		break;
+
+	case GuidanceState::GOAL_REACHED:
+		// temporary till I find a better way to stop the vehicle
+		desired_speed = 0.f;
+		heading_error = 0.f;
+		angular_velocity = 0.f;
+		_desired_angular_velocity = 0.f;
+		break;
+	}
+
+	// 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;
+}
+
+void DifferentialDriveGuidance::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_heading_p_controller,
+			   _param_rdd_p_gain_heading.get(),  // Proportional gain
+			   0,  // Integral gain
+			   0,  // Derivative gain
+			   0,  // Integral limit
+			   200);  // Output limit
+
+	_forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get());
+	_forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get());
+	_forwards_velocity_smoothing.setMaxVel(_max_speed);
+}

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+// #include <matrix/matrix/Matrix.hpp>
+
+#include <lib/motion_planning/PositionSmoothing.hpp>
+#include <lib/motion_planning/VelocitySmoothing.hpp>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+#include <lib/pid/pid.h>
+
+
+/**
+ * @brief Enum class for the different states of guidance.
+ */
+enum class GuidanceState {
+	TURNING, ///< The vehicle is currently turning.
+	DRIVING, ///< The vehicle is currently driving straight.
+	GOAL_REACHED ///< The vehicle has reached its goal.
+};
+
+/**
+ * @brief Class for differential drive guidance.
+ */
+class DifferentialDriveGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for DifferentialDriveGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	DifferentialDriveGuidance(ModuleParams *parent);
+
+	/**
+	 * @brief Default destructor.
+	 */
+	~DifferentialDriveGuidance() = default;
+
+	/**
+	 * @brief Compute guidance for the vehicle.
+	 * @param global_pos The global position of the vehicle in degrees.
+	 * @param current_waypoint The current waypoint the vehicle is heading towards in degrees.
+	 * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees.
+	 * @param vehicle_yaw The yaw orientation of the vehicle in radians.
+	 * @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);
+
+	/**
+	 * @brief Set the maximum speed for the vehicle.
+	 * @param max_speed The maximum speed in m/s.
+	 * @return The set maximum speed in m/s.
+	 */
+	float setMaxSpeed(float max_speed) { return _max_speed = max_speed; }
+
+
+	/**
+	 * @brief Set the maximum angular velocity for the vehicle.
+	 * @param max_angular_velocity The maximum angular velocity in rad/s.
+	 * @return The set maximum angular velocity in rad/s.
+	 */
+	float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; }
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	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)};
+
+	position_setpoint_triplet_s _position_setpoint_triplet{};
+	vehicle_global_position_s _vehicle_global_position{};
+
+	GuidanceState _currentState; ///< The current state of guidance.
+
+	float _desired_angular_velocity; ///< The desired angular velocity.
+
+	float _max_speed; ///< The maximum speed.
+	float _max_angular_velocity; ///< The maximum angular velocity.
+
+	matrix::Vector2d _next_waypoint; ///< The next waypoint.
+
+	VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion.
+	PositionSmoothing _position_smoothing; ///< The position smoothing.
+
+	PID_t _heading_p_controller; ///< The PID controller for yaw rate.
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_P_HEADING>) _param_rdd_p_gain_heading,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
+		(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
+	)
+
+};

src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2023 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(DifferentialDriveKinematics
+	DifferentialDriveKinematics.cpp
+)
+
+target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)

src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 "DifferentialDriveKinematics.hpp"
+
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
+{
+	if (_max_speed < FLT_EPSILON) {
+		return Vector2f();
+	}
+
+	linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
+	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
+
+	const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
+	float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
+
+	// Compute an initial gain
+	float gain = 1.0f;
+
+	if (combined_velocity > _max_speed) {
+		float excess_velocity = fabsf(combined_velocity - _max_speed);
+		float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
+		gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
+	}
+
+	// Apply the gain
+	linear_velocity_x *= gain;
+
+	// Calculate the left and right wheel speeds
+	return Vector2f(linear_velocity_x - rotational_velocity,
+			linear_velocity_x + rotational_velocity) / _max_speed;
+}

src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+/**
+ * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
+ *
+ * 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
+{
+public:
+	DifferentialDriveKinematics() = default;
+	~DifferentialDriveKinematics() = default;
+
+	/**
+	 * @brief Sets the wheel base of the robot.
+	 *
+	 * @param wheel_base The distance between the centers of the wheels.
+	 */
+	void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
+
+	/**
+	 * @brief Sets the maximum speed of the robot.
+	 *
+	 * @param max_speed The maximum speed of the robot.
+	 */
+	void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
+
+	/**
+	 * @brief Sets the maximum angular speed of the robot.
+	 *
+	 * @param max_angular_speed The maximum angular speed of the robot.
+	 */
+	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
+
+	/**
+	 * @brief Computes the inverse kinematics for differential drive.
+	 *
+	 * @param linear_velocity_x Linear velocity along the x-axis.
+	 * @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);
+
+private:
+	float _wheel_base{0.f};
+	float _max_speed{0.f};
+	float _max_angular_velocity{0.f};
+};

src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp

@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 <gtest/gtest.h>
+#include "DifferentialDriveKinematics.hpp"
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with zero linear velocity and zero yaw rate (stationary vehicle)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f());
+}
+
+
+TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(0.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with invalid parameters (zero wheel base and wheel radius)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f());
+}
+
+
+TEST(DifferentialDriveKinematicsTest, UnitCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with unit values for linear velocity and yaw rate
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f));
+}
+
+
+TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
+}
+
+
+TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
+}
+
+TEST(DifferentialDriveKinematicsTest, RandomCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
+}
+
+TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test rotating in place (zero linear velocity, non-zero yaw rate)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
+}
+
+TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test moving straight (non-zero linear velocity, zero yaw rate)
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(FLT_MIN);
+	kinematics.setMaxSpeed(FLT_MIN);
+	kinematics.setMaxAngularVelocity(FLT_MIN);
+
+	// Test with minimum possible input values
+	EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
+}

src/modules/differential_drive/Kconfig

@@ -0,0 +1,6 @@
+menuconfig MODULES_DIFFERENTIAL_DRIVE
+	bool "differential_drive"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Enable support for control of differential drive rovers

src/modules/differential_drive/module.yaml

@@ -0,0 +1,122 @@
+module_name: Differential Drive
+
+parameters:
+  - group: Rover Differential Drive
+    definitions:
+      RDD_WHEEL_BASE:
+        description:
+          short: Wheel base
+          long: Distance from the center of the right wheel to the center of the left wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+      RDD_WHEEL_RADIUS:
+        description:
+          short: Wheel radius
+          long: Size of the wheel, half the diameter of the wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.1
+      RDD_SPEED_SCALE:
+        description:
+          short: Manual speed scale
+        type: float
+        min: 0
+        max: 1.0
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_ANG_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1.0
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_WHL_SPEED:
+        description:
+          short: Maximum wheel speed
+        type: float
+        unit: rad/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.3
+      RDD_P_HEADING:
+        description:
+          short: Proportional gain for heading controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_P_SPEED:
+        description:
+          short: Proportional gain for speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_I_SPEED:
+        description:
+          short: Integral gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.0
+      RDD_P_ANG_VEL:
+        description:
+          short: Proportional gain for angular velocity controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_I_ANG_VEL:
+        description:
+          short: Integral gain for angular velocity controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.0
+      RDD_MAX_JERK:
+        description:
+          short: Maximum jerk
+          long: Limit for forwards acc/deceleration change.
+        type: float
+        unit: m/s^3
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5
+      RDD_MAX_ACCEL:
+        description:
+          short: Maximum acceleration
+          long: Maximum acceleration is used to limit the acceleration of the rover
+        type: float
+        unit: m/s^2
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5