Differential Rover: Differential drive module & library

ROMFS/px4fmu_common/init.d/rc.rover_differential_apps

@@ -15,7 +15,7 @@ ekf2 start &
 #
 # Start manual rover differential drive controller.
 #
-# differential_drive_control start
+differential_drive_control start
 
 
 #

boards/px4/fmu-v5x/default.px4board

@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
+CONFIG_MODULE_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y

msg/CMakeLists.txt

@@ -69,6 +69,7 @@ set(msg_files
 	DebugKeyValue.msg
 	DebugValue.msg
 	DebugVect.msg
+	DifferentialDriveSetpoint.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
 	Ekf2Timestamps.msg

msg/DifferentialDriveSetpoint.msg

@@ -0,0 +1,4 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 speed # [m/s] collective roll-off speed in body x-axis
+float32 yaw_rate # [rad/s] yaw rate

src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt

@@ -62,8 +62,6 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessTailsitterVTOL.hpp
 	ActuatorEffectivenessRoverAckermann.hpp
 	ActuatorEffectivenessRoverAckermann.cpp
-	ActuatorEffectivenessRoverDifferential.hpp
-	ActuatorEffectivenessRoverDifferential.cpp
 )
 
 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

src/modules/control_allocator/ControlAllocator.cpp

@@ -61,6 +61,7 @@ ControlAllocator::ControlAllocator() :
 	_actuator_servos_pub.advertise();
 	_actuator_servos_trim_pub.advertise();
 
+
 	for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
 		char buffer[17];
 		snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);
@@ -239,7 +240,7 @@ ControlAllocator::update_effectiveness_source()
 			break;
 
 		case EffectivenessSource::ROVER_DIFFERENTIAL:
-			tmp = new ActuatorEffectivenessRoverDifferential();
+			// actuator_motors message is published directly from the differential drive controller
 			break;
 
 		case EffectivenessSource::FIXED_WING:

src/modules/control_allocator/ControlAllocator.hpp

@@ -47,7 +47,6 @@
 #include <ActuatorEffectivenessTiltrotorVTOL.hpp>
 #include <ActuatorEffectivenessTailsitterVTOL.hpp>
 #include <ActuatorEffectivenessRoverAckermann.hpp>
-#include <ActuatorEffectivenessRoverDifferential.hpp>
 #include <ActuatorEffectivenessFixedWing.hpp>
 #include <ActuatorEffectivenessMCTilt.hpp>
 #include <ActuatorEffectivenessCustom.hpp>

src/modules/differential_drive_control/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   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)
+
+px4_add_module(
+	MODULE modules__differential_drive_control
+	MAIN differential_drive_control
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD
+	SRCS
+		DifferentialDriveControl.cpp
+		DifferentialDriveControl.hpp
+	DEPENDS
+		DifferentialDriveKinematics
+		px4_work_queue
+)

src/modules/differential_drive_control/DifferentialDriveControl.cpp

@@ -0,0 +1,200 @@
+/****************************************************************************
+ *
+ *   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 "DifferentialDriveControl.hpp"
+
+using namespace time_literals;
+
+namespace differential_drive_control
+{
+
+DifferentialDriveControl::DifferentialDriveControl() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	_outputs_pub.advertise();
+	_last_timestamp = hrt_absolute_time();
+	_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
+	_differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get());
+}
+
+int DifferentialDriveControl::task_spawn(int argc, char *argv[])
+{
+	DifferentialDriveControl *obj = new DifferentialDriveControl();
+
+	if (!obj) {
+		PX4_ERR("alloc failed");
+		return -1;
+	}
+
+	_object.store(obj);
+	_task_id = task_id_is_work_queue;
+
+	obj->start();
+
+	return 0;
+}
+
+void DifferentialDriveControl::start()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+}
+
+void DifferentialDriveControl::Run()
+{
+
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	_current_timestamp = hrt_absolute_time();
+
+	vehicle_control_mode_poll();
+
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		updateParams();
+
+		_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
+		_differential_drive_kinematics.setWheelRadius(_param_rdd_wheel_radius.get());
+	}
+
+	if (_vehicle_control_mode.flag_armed) {
+
+		if (_manual_control_setpoint_sub.updated()) {
+			manual_control_setpoint_s manual_control_setpoint{};
+			_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+			// Manual mode
+			if (_vehicle_control_mode.flag_control_manual_enabled) {
+				// directly get the input from the manual control setpoint (joystick)
+
+				_differential_drive_setpoint.timestamp = hrt_absolute_time();
+
+				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_max_speed.get();
+				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_max_angular_velocity.get();
+
+				_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
+			}
+		}
+
+		// if manual is not enabled, it will look for a differential drive setpoint topic (differential_drive_setpoint) and subscribe to it from a ROS2 mode over DDS
+		if (_differential_drive_setpoint_sub.updated()) {
+			_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint);
+
+			_velocity_control_inputs(0) = _differential_drive_setpoint.speed;
+			_velocity_control_inputs(1) = _differential_drive_setpoint.yaw_rate;
+
+		} else if (_differential_drive_setpoint.timestamp <= 100_ms) {
+			_velocity_control_inputs(0) = 0.0f;
+			_velocity_control_inputs(1) = 0.0f;
+		}
+
+	} else {
+		// if the system is in an error state, stop the vehicle
+		_velocity_control_inputs = {0.0f, 0.0f};
+	}
+
+	// publish data to actuator_motors (output module)
+	publishWheelControl();
+
+	_last_timestamp = _current_timestamp;
+
+}
+
+void DifferentialDriveControl::publishWheelControl()
+{
+	// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
+	_output_inverse = _differential_drive_kinematics.computeInverseKinematics(_velocity_control_inputs(0),
+			  _velocity_control_inputs(1));
+
+	// Superpose Linear and Angular velocity vector
+	float max_angular_wheel_speed = ((_param_rdd_max_speed.get() + (_param_rdd_max_angular_velocity.get() *
+					  _param_rdd_wheel_base.get() / 2.f)) / _param_rdd_wheel_radius.get());
+
+	// Check if max_angular_wheel_speed is zero
+	if (fabsf(max_angular_wheel_speed) < FLT_EPSILON) {
+		// Guard against division by zero
+		max_angular_wheel_speed = 0.001f;
+	}
+
+	_actuator_motors.timestamp = hrt_absolute_time();
+
+	// bitset which motors are configured to be reversible (3 -> ..00 0000 0011 this means that the first two motors are reversible)
+	_actuator_motors.reversible_flags = 3;
+	_actuator_motors.control[0] = _output_inverse(0) / max_angular_wheel_speed;
+	_actuator_motors.control[1] = _output_inverse(1) / max_angular_wheel_speed;
+
+	_outputs_pub.publish(_actuator_motors);
+}
+
+void DifferentialDriveControl::vehicle_control_mode_poll()
+{
+	if (_vehicle_control_mode_sub.updated()) {
+		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
+	}
+}
+
+int DifferentialDriveControl::print_status()
+{
+	PX4_INFO("Diffential Drive Controller running");
+	return 0;
+}
+
+int DifferentialDriveControl::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", "system");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[])
+{
+	return DifferentialDriveControl::main(argc, argv);
+}
+
+} // namespace differential_drive_control

src/modules/differential_drive_control/DifferentialDriveControl.hpp

@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   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/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+
+// Cruise mode
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/differential_drive_setpoint.h>
+
+// Standard library includes
+#include <math.h>
+
+// Local includes
+#include <DifferentialDriveKinematics.hpp>
+
+namespace differential_drive_control
+{
+
+class DifferentialDriveControl : public ModuleBase<DifferentialDriveControl>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	DifferentialDriveControl();
+	~DifferentialDriveControl() override = default;
+
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[])
+	{
+		return print_usage("unknown command");
+	}
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void start();
+
+private:
+
+	void Run() override;
+	void publishWheelControl();
+	void vehicle_control_mode_poll();
+
+	uORB::PublicationMulti<actuator_motors_s> _outputs_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
+
+
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	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 _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
+
+	differential_drive_setpoint_s _differential_drive_setpoint{};
+	vehicle_control_mode_s _vehicle_control_mode{};
+	actuator_motors_s _actuator_motors{};
+
+	DifferentialDriveKinematics _differential_drive_kinematics;
+
+	matrix::Vector2f _velocity_control_inputs{0.f, 0.f}; // [m/s] collective roll-off speed in body x-axis, [rad/s] yaw rate
+	matrix::Vector2f _output_inverse{0.0f, 0.0f}; // [rad/s] Right Motor, [rad/s] Left Motor
+
+	float _last_timestamp{0.f};
+	float _current_timestamp{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_MAX_SPEED>) _param_rdd_max_speed,
+		(ParamFloat<px4::params::RDD_MAX_ANG_VEL>) _param_rdd_max_angular_velocity,
+		(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
+		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius
+	)
+};
+
+} // namespace differential_drive_control

src/modules/differential_drive_control/DifferentialDriveKinematics/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(DifferentialDriveKinematics
+	DifferentialDriveKinematics.cpp
+	DifferentialDriveKinematics.hpp
+)
+
+target_compile_options(DifferentialDriveKinematics PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,23 +31,22 @@
  *
  ****************************************************************************/
 
-#pragma once
+#include "DifferentialDriveKinematics.hpp"
 
-#include "ActuatorEffectiveness.hpp"
-
-class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness
+void DifferentialDriveKinematics::setWheelBase(float wheel_base)
 {
-public:
-	ActuatorEffectivenessRoverDifferential() = default;
-	virtual ~ActuatorEffectivenessRoverDifferential() = default;
+	_wheel_base = wheel_base;
+}
 
-	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
+void DifferentialDriveKinematics::setWheelRadius(float wheel_radius)
+{
+	_wheel_radius = wheel_radius;
+}
 
-	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_vel_x, float yaw_rate)
+{
+	float motor_vel_right = linear_vel_x / _wheel_radius - _wheel_base / 2.f * yaw_rate / _wheel_radius;
+	float motor_vel_left = linear_vel_x / _wheel_radius + _wheel_base / 2.f * yaw_rate / _wheel_radius;
 
-	const char *name() const override { return "Rover (Differential)"; }
-private:
-	uint32_t _motors_mask{};
-};
+	return matrix::Vector2f(motor_vel_right, motor_vel_left);
+}

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   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 Computes the inverse kinematics for differential drive.
+	 *
+	 * @param linear_vel_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_vel_x, float yaw_rate);
+
+	/**
+	 * @brief Sets the wheel base of the robot.
+	 *
+	 * @param wheel_base The distance between the centers of the wheels.
+	 */
+	void setWheelBase(float wheel_base);
+
+	/**
+	 * @brief Sets the radius of the wheels of the robot.
+	 *
+	 * @param wheel_radius The radius of the wheels.
+	 */
+	void setWheelRadius(float wheel_radius);
+
+private:
+	float _wheel_base{0.f};
+	float _wheel_radius{0.f};
+};

src/modules/differential_drive_control/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp

@@ -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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "DifferentialDriveKinematics.hpp"
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+TEST(DifferentialDriveKinematicsTest, AllZeroCaseInverse)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setWheelRadius(1.f);
+	Vector2f rate_setpoint = {0.f, 0.f};
+	kinematics.setInput(rate_setpoint, true);
+	Vector2f wheel_output = kinematics.getOutput(true);
+	EXPECT_EQ(wheel_output, Vector2f());
+}
+
+TEST(DifferentialDriveKinematicsTest, InvalidCaseInverse)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(0.f);
+	kinematics.setWheelRadius(0.f);
+	Vector2f rate_setpoint = {0.f, 0.f};
+	kinematics.setInput(rate_setpoint, true);
+	Vector2f wheel_output = kinematics.getOutput(true);
+	EXPECT_EQ(wheel_output, Vector2f());
+}
+
+TEST(DifferentialDriveKinematicsTest, UnitCaseInverse)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setWheelRadius(1.f);
+	Vector2f rate_setpoint = {1.f, 1.f};
+	kinematics.setInput(rate_setpoint, true);
+	Vector2f wheel_output = kinematics.getOutput(true);
+	Vector2f expected_output = {1.5f, 0.5f};
+	EXPECT_EQ(wheel_output, expected_output);
+}
+
+TEST(DifferentialDriveKinematicsTest, UnitCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setWheelRadius(1.f);
+	Vector2f wheel_input = {1.f, 1.f};
+	kinematics.setInput(wheel_input, false);
+	Vector2f rate_output = kinematics.getOutput(false);
+	Vector2f expected_output = {1.f, 0.f};
+	EXPECT_EQ(rate_output, expected_output);
+}

src/modules/differential_drive_control/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL
+	bool "differential_drive_control"
+	default y
+	---help---
+		Enable support for differential_drive_control

src/modules/differential_drive_control/params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,29 +31,54 @@
  *
  ****************************************************************************/
 
-#include "ActuatorEffectivenessRoverDifferential.hpp"
-#include <ControlAllocation/ControlAllocation.hpp>
+/**
+ * Wheel Base
+ *
+ * Distance from the center of the right wheel to the center of the left wheel
+ *
+ * @unit m
+ * @min 0.001
+ * @max 100
+ * @increment 0.001
+ * @decimal 3
+ * @group Rover Differential Drive
+ */
+PARAM_DEFINE_FLOAT(RDD_WHEEL_BASE, 0.5f);
 
-using namespace matrix;
+/**
+ * Wheel radius
+ *
+ * Size of the wheel, half the diameter of the wheel
+ *
+ * @unit m
+ * @min 0.001
+ * @max 100
+ * @increment 0.001
+ * @decimal 3
+ * @group Rover Differential Drive
+ */
+PARAM_DEFINE_FLOAT(RDD_WHEEL_RADIUS, 0.1f);
 
-bool
-ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration,
-		EffectivenessUpdateReason external_update)
-{
-	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
-		return false;
-	}
-
-	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f});
-	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f});
-	_motors_mask = (1u << 0) | (1u << 1);
-	return true;
-}
-
-void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
-{
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
-}
+/**
+ * Max Speed
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 100
+ * @increment 0.01
+ * @decimal 2
+ * @group Rover Differential Drive
+ */
+PARAM_DEFINE_FLOAT(RDD_MAX_SPEED, 0.5f);
 
+/**
+ * Max Angular Velocity
+ *
+ * @unit rad/s
+ * @min 0.0
+ * @max 100
+ * @increment 0.01
+ * @decimal 2
+ * @group Rover Differential Drive
+ */
+PARAM_DEFINE_FLOAT(RDD_MAX_ANG_VEL, 0.3f);