added mecanum apps and defaults

After smoothing the linera velocity setpoint, the EKF has trouble initializing, becuase the acceleration is too smooth, to combat this issue, there is a 1 second delay when initializing the mission mode

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_r1_rover_mecanum

@@ -0,0 +1,41 @@
+#!/bin/sh
+# @name Aion Robotics R1 Rover
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_mecanum_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel front
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel front
+param set-default SIM_GZ_WH_MIN2 0
+param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_DIS2 100
+
+param set-default SIM_GZ_WH_FUNC3 103 # right wheel back
+param set-default SIM_GZ_WH_MIN3 0
+param set-default SIM_GZ_WH_MAX3 200
+param set-default SIM_GZ_WH_DIS3 100
+
+param set-default SIM_GZ_WH_FUNC4 104 # left wheel back
+param set-default SIM_GZ_WH_MIN4 0
+param set-default SIM_GZ_WH_MAX4 200
+param set-default SIM_GZ_WH_DIS4 100
+
+# param set-default SIM_GZ_WH_REV 1 # reverse right wheel

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -82,6 +82,7 @@ px4_add_romfs_files(
 	4008_gz_advanced_plane
 	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
+	4011_gz_r1_rover_mecanum
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -53,6 +53,8 @@ px4_add_romfs_files(
 	rc.rover_defaults
 	rc.rover_differential_apps
 	rc.rover_differential_defaults
+	rc.rover_mecanum_apps
+	rc.rover_mecanum_defaults
 	rc.uuv_apps
 	rc.uuv_defaults
 	rc.vehicle_setup

ROMFS/px4fmu_common/init.d/rc.rover_differential_apps

@@ -5,7 +5,7 @@
 ekf2 start &
 
 # Start rover differential drive controller.
-differential_drive_control start
+differential_drive start
 
 # Start Land Detector.
 land_detector start rover

ROMFS/px4fmu_common/init.d/rc.rover_mecanum_apps

@@ -0,0 +1,11 @@
+#!/bin/sh
+# Standard apps for a mecanum drive rover.
+
+# Start the attitude and position estimator.
+ekf2 start &
+
+# Start rover mecanum drive controller.
+mecanum_drive start
+
+# Start Land Detector.
+land_detector start rover

ROMFS/px4fmu_common/init.d/rc.rover_mecanum_defaults

@@ -0,0 +1,11 @@
+#!/bin/sh
+# Mecanum rover parameters.
+
+set VEHICLE_TYPE rover_mecanum
+
+param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
+
+param set-default CA_AIRFRAME 13 # Rover (Mecanum)
+param set-default CA_R_REV 15 # Right and left motors reversible
+
+param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -41,6 +41,15 @@ then
 	. ${R}etc/init.d/rc.rover_differential_apps
 fi
 
+#
+# Mecanum Rover setup.
+#
+if [ $VEHICLE_TYPE = rover_mecanum ]
+then
+	# Start mecanum drive rover apps.
+	. ${R}etc/init.d/rc.rover_mecanum_apps
+fi
+
 #
 # VTOL setup.
 #

boards/px4/fmu-v5x/default.px4board

@@ -53,7 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y

boards/px4/sitl/default.px4board

@@ -12,7 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_VERBOSE_STATUS=y
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
@@ -40,6 +40,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_MECANUM_DRIVE=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_PAYLOAD_DELIVERER=y

msg/CMakeLists.txt

@@ -137,6 +137,7 @@ set(msg_files
 	ManualControlSwitches.msg
 	MavlinkLog.msg
 	MavlinkTunnel.msg
+	MecanumDriveSetpoint.msg
 	MessageFormatRequest.msg
 	MessageFormatResponse.msg
 	Mission.msg

msg/DifferentialDriveSetpoint.msg

@@ -1,4 +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

msg/MecanumDriveSetpoint.msg

@@ -0,0 +1,8 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32[2] speed # [m/s] collective roll-off speed in body x- and y-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 mecanum_drive_setpoint mecanum_drive_control_output

src/lib/mixer_module/mixer_module.cpp

@@ -437,12 +437,25 @@ bool MixingOutput::update()
 	bool all_disabled = true;
 	_reversible_mask = 0;
 
+	// printf("max num outputs: %d\n", _max_num_outputs);
+
 	for (int i = 0; i < _max_num_outputs; ++i) {
+
+		// printf(" _functions[i]: %p\n", (void *)_functions[i]);
+		// printf("_function_assignment: %d\n", (int)_function_assignment[i]);
+
 		if (_functions[i]) {
+
+			// printf("valid function\n");
+			// printf("armed or prearmed allowprearmcontrol %d\n", _functions[i]->allowPrearmControl());
+			// printf("prearmed %d\n", _armed.prearmed);
+			// printf("armed %d\n", _armed.armed);
+			// printf("nan\n");
 			all_disabled = false;
 
 			if (_armed.armed || (_armed.prearmed && _functions[i]->allowPrearmControl())) {
 				outputs[i] = _functions[i]->value(_function_assignment[i]);
+				printf("outputs[i]: %f\n", (double)outputs[i]);
 
 			} else {
 				outputs[i] = NAN;
@@ -471,6 +484,9 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat
 {
 	bool stop_motors = !_throttle_armed && !_actuator_test.inTestMode();
 
+	// printf("outputs from limitAndUpdateOutputs: %f, %f, %f, %f\n", (double)outputs[0], (double)outputs[1],
+	//        (double)outputs[2], (double)outputs[3]);
+
 	if (_armed.lockdown || _armed.manual_lockdown) {
 		// overwrite outputs in case of lockdown with disarmed values
 		for (size_t i = 0; i < _max_num_outputs; i++) {

src/modules/control_allocator/module.yaml

@@ -1140,3 +1140,17 @@ mixer:
                     parameters:
                       - label: 'Throttle spoolup time'
                         name: COM_SPOOLUP_TIME
+
+            13: # Rover (Mecanum)
+                title: 'Rover (Mecanum)'
+                actuators:
+                  - actuator_type: 'motor'
+                    instances:
+                      - name: 'Right Motor Front'
+                        position: [ 1, 1., 0 ]
+                      - name: 'Left Motor Front'
+                        position: [ 1, -1., 0 ]
+                      - name: 'Right Motor Back'
+                        position: [ -1, 1., 0 ]
+                      - name: 'Left Motor Back'
+                        position: [ -1, -1., 0 ]

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,17 +31,22 @@
 #
 ############################################################################
 
+add_subdirectory(DifferentialDriveControl)
+add_subdirectory(DifferentialDriveGuidance)
 add_subdirectory(DifferentialDriveKinematics)
 
 px4_add_module(
-	MODULE modules__differential_drive_control
+	MODULE modules__differential_drive
-	MAIN differential_drive_control
+	MAIN differential_drive
 	SRCS
-		DifferentialDriveControl.cpp
+		DifferentialDrive.cpp
-		DifferentialDriveControl.hpp
+		DifferentialDrive.hpp
 	DEPENDS
+		DifferentialDriveControl
+		DifferentialDriveGuidance
 		DifferentialDriveKinematics
 		px4_work_queue
+		modules__control_allocator # for parameter CA_R_REV
 	MODULE_CONFIG
 		module.yaml
 )

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
@@ -31,38 +31,37 @@
  *
  ****************************************************************************/
 
-#include "DifferentialDriveControl.hpp"
+#include "DifferentialDrive.hpp"
 
-using namespace time_literals;
+DifferentialDrive::DifferentialDrive() :
-using namespace matrix;
-namespace differential_drive_control
-{
-
-DifferentialDriveControl::DifferentialDriveControl() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	updateParams();
 }
 
-bool DifferentialDriveControl::init()
+bool DifferentialDrive::init()
 {
 	ScheduleOnInterval(10_ms); // 100 Hz
 	return true;
 }
 
-void DifferentialDriveControl::updateParams()
+void DifferentialDrive::updateParams()
 {
 	ModuleParams::updateParams();
-	_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);
+
+	_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);
 }
 
-void DifferentialDriveControl::Run()
+void DifferentialDrive::Run()
 {
 	if (should_exit()) {
 		ScheduleClear();
@@ -70,20 +69,30 @@ void DifferentialDriveControl::Run()
 	}
 
 	hrt_abstime now = hrt_absolute_time();
+	const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
+	_time_stamp_last = now;
 
 	if (_parameter_update_sub.updated()) {
-		parameter_update_s pupdate;
+		parameter_update_s parameter_update;
-		_parameter_update_sub.copy(&pupdate);
+		_parameter_update_sub.copy(&parameter_update);
-
 		updateParams();
 	}
 
 	if (_vehicle_control_mode_sub.updated()) {
-		vehicle_control_mode_s vehicle_control_mode;
+		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; // change this when more modes are supported
+			_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
+			_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
+		}
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
 		}
 	}
 
@@ -94,43 +103,42 @@ void DifferentialDriveControl::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_s setpoint{};
-				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
+				setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed;
-				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
+				setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity;
-									_max_angular_velocity;
+
-				_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
+				// if acro mode, we activate the yaw rate control
+				if (_acro_driving) {
+					setpoint.closed_loop_speed_control = false;
+					setpoint.closed_loop_yaw_rate_control = true;
+
+				} else {
+					setpoint.closed_loop_speed_control = false;
+					setpoint.closed_loop_yaw_rate_control = false;
+				}
+
+				setpoint.timestamp = now;
+				_differential_drive_setpoint_pub.publish(setpoint);
 			}
 		}
+
+	} else if (_mission_driving) {
+		// Mission mode
+		// directly receive setpoints from the guidance library
+		_differential_drive_guidance.computeGuidance(
+			_differential_drive_control.getVehicleYaw(),
+			_differential_drive_control.getVehicleBodyYawRate(),
+			dt
+		);
 	}
 
-	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
+	_differential_drive_control.control(dt);
-
+	_differential_drive_kinematics.allocate();
-	// publish data to actuator_motors (output module)
-	// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
-	Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
-					_differential_drive_setpoint.speed,
-					_differential_drive_setpoint.yaw_rate);
-
-	// 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 DifferentialDriveControl::task_spawn(int argc, char *argv[])
+int DifferentialDrive::task_spawn(int argc, char *argv[])
 {
-	DifferentialDriveControl *instance = new DifferentialDriveControl();
+	DifferentialDrive *instance = new DifferentialDrive();
 
 	if (instance) {
 		_object.store(instance);
@@ -151,12 +159,12 @@ int DifferentialDriveControl::task_spawn(int argc, char *argv[])
 	return PX4_ERROR;
 }
 
-int DifferentialDriveControl::custom_command(int argc, char *argv[])
+int DifferentialDrive::custom_command(int argc, char *argv[])
 {
 	return print_usage("unknown command");
 }
 
-int DifferentialDriveControl::print_usage(const char *reason)
+int DifferentialDrive::print_usage(const char *reason)
 {
 	if (reason) {
 		PX4_ERR("%s\n", reason);
@@ -168,15 +176,13 @@ int DifferentialDriveControl::print_usage(const char *reason)
 Rover Differential Drive controller.
 )DESCR_STR");
 
-	PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller");
+	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_control_main(int argc, char *argv[])
+extern "C" __EXPORT int differential_drive_main(int argc, char *argv[])
 {
-	return DifferentialDriveControl::main(argc, argv);
+	return DifferentialDrive::main(argc, argv);
 }
-
-} // namespace differential_drive_control

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,41 +33,31 @@
 
 #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>
 #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>
+#include <uORB/topics/vehicle_status.h>
 
-// Standard library includes
+#include "DifferentialDriveControl/DifferentialDriveControl.hpp"
-#include <math.h>
+#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp"
+#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp"
 
-// Local includes
+using namespace time_literals;
-#include <DifferentialDriveKinematics.hpp>
 
-namespace differential_drive_control
+class DifferentialDrive : public ModuleBase<DifferentialDrive>, public ModuleParams,
-{
-
-class DifferentialDriveControl : public ModuleBase<DifferentialDriveControl>, public ModuleParams,
 	public px4::ScheduledWorkItem
 {
 public:
-	DifferentialDriveControl();
+	DifferentialDrive();
-	~DifferentialDriveControl() override = default;
+	~DifferentialDrive() override = default;
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -85,32 +75,30 @@ protected:
 
 private:
 	void Run() override;
-
-	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_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
 
-	differential_drive_setpoint_s _differential_drive_setpoint{};
-	DifferentialDriveKinematics _differential_drive_kinematics{};
-
-	bool _armed = false;
 	bool _manual_driving = false;
+	bool _mission_driving = false;
+	bool _acro_driving = false;
+	hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
+
+	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};
 
 	DEFINE_PARAMETERS(
-		(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,
+		(ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_wheel_speed,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius
 	)
 };
-
-} // namespace differential_drive_control

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})

src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   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;
+using namespace time_literals;
+
+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);
+		}
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			_spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s;
+		}
+	}
+
+	_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);
+	}
+
+
+	if (!_spooled_up) {
+		differential_drive_control_output.speed = 0.0f;
+		differential_drive_control_output.yaw_rate = 0.0f;
+	}
+
+	differential_drive_control_output.timestamp = hrt_absolute_time();
+	_differential_drive_control_output_pub.publish(differential_drive_control_output);
+}

src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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>
+#include <uORB/topics/vehicle_status.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::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	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.
+
+	bool _spooled_up{false};
+
+	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::COM_SPOOLUP_TIME>) _param_com_spoolup_time
+	)
+};

src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(DifferentialDriveGuidance
+	DifferentialDriveGuidance.cpp
+)
+
+target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp

@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   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>
+
+using namespace matrix;
+
+DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+
+	pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+void DifferentialDriveGuidance::computeGuidance(float 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 - yaw);
+
+	if (_current_waypoint != current_waypoint) {
+		_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;
+	}
+
+	float desired_speed = 0.f;
+
+	switch (_currentState) {
+	case GuidanceState::TURNING:
+		desired_speed = 0.f;
+
+		if (fabsf(heading_error) < 0.05f) {
+			_currentState = GuidanceState::DRIVING;
+		}
+
+		break;
+
+	case GuidanceState::DRIVING: {
+			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);
+			desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
+					_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) + heading_error;
+
+	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);
+
+	_current_waypoint = current_waypoint;
+}
+
+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,140 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#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 <lib/motion_planning/PositionSmoothing.hpp>
+#include <lib/motion_planning/VelocitySmoothing.hpp>
+
+#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>
+
+#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);
+	~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.
+	 */
+	void computeGuidance(float 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)};
+
+	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{};
+
+	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 _current_waypoint; ///< The current 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

@@ -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)

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);
 	}
 
@@ -65,4 +95,3 @@ matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float lin
 	return Vector2f(linear_velocity_x - rotational_velocity,
 			linear_velocity_x + rotational_velocity) / _max_speed;
 }
-

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
+	)
 };

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);

src/modules/differential_drive/Kconfig

@@ -1,5 +1,5 @@
-menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL
+menuconfig MODULES_DIFFERENTIAL_DRIVE
-	bool "differential_drive_control"
+	bool "differential_drive"
 	default n
 	depends on MODULES_CONTROL_ALLOCATOR
 	---help---

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
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_ANG_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_WHEEL_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
+      RDD_P_SPEED:
+        description:
+          short: Proportional gain for speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      RDD_I_SPEED:
+        description:
+          short: Integral gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 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
+      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
+      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

src/modules/differential_drive_control/module.yaml

@@ -1,55 +0,0 @@
-module_name: Differential Drive Control
-
-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: 10

src/modules/logger/logged_topics.cpp

@@ -57,6 +57,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("commander_state");
 	add_topic("config_overrides");
 	add_topic("cpuload");
+	add_optional_topic("differential_drive_control_output", 100);
 	add_optional_topic("differential_drive_setpoint", 100);
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");

src/modules/mecanum_drive/.vscode/c_cpp_properties.json

@@ -0,0 +1,20 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/humble/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

src/modules/mecanum_drive/.vscode/settings.json

@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}

src/modules/mecanum_drive/CMakeLists.txt

@@ -0,0 +1,52 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(MecanumDriveControl)
+# add_subdirectory(MecanumDriveGuidance)
+add_subdirectory(MecanumDriveKinematics)
+
+px4_add_module(
+	MODULE modules__mecanum_drive
+	MAIN mecanum_drive
+	SRCS
+		MecanumDrive.cpp
+		MecanumDrive.hpp
+	DEPENDS
+		MecanumDriveControl
+		# MecanumDriveGuidance
+		MecanumDriveKinematics
+		px4_work_queue
+		modules__control_allocator # for parameter CA_R_REV
+	MODULE_CONFIG
+		module.yaml
+)

src/modules/mecanum_drive/Kconfig

@@ -0,0 +1,6 @@
+menuconfig MODULES_MECANUM_DRIVE
+	bool "mecanum_drive"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Enable support for control of mecanum drive rovers

src/modules/mecanum_drive/MecanumDrive.cpp

@@ -0,0 +1,195 @@
+/****************************************************************************
+ *
+ *   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 "MecanumDrive.hpp"
+
+MecanumDrive::MecanumDrive() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+}
+
+bool MecanumDrive::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void MecanumDrive::updateParams()
+{
+	ModuleParams::updateParams();
+
+	_mecanum_drive_kinematics.setWheelBase(_param_md_wheel_base.get(), _param_md_wheel_base.get());
+	_mecanum_drive_kinematics.setWheelRadius(_param_md_wheel_radius.get());
+
+	_max_speed = _param_md_wheel_speed.get() * _param_md_wheel_radius.get();
+	// _mecanum_drive_guidance.setMaxSpeed(_max_speed);
+	_mecanum_drive_kinematics.setMaxSpeed(_max_speed, _max_speed);
+
+	_max_angular_velocity = _max_speed / (_param_md_wheel_base.get() / 2.f);
+	// _mecanum_drive_guidance.setMaxAngularVelocity(_max_angular_velocity);
+	_mecanum_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
+}
+
+void MecanumDrive::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+	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();
+	}
+
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode{};
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_mecanum_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_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
+		}
+	}
+
+	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)) {
+				mecanum_drive_setpoint_s setpoint{};
+				setpoint.yaw_rate = manual_control_setpoint.throttle * math::max(0.f, _param_md_speed_scale.get()) * _max_speed;
+				setpoint.speed[1] = manual_control_setpoint.yaw * math::max(0.f, _param_md_speed_scale.get()) * _max_speed;
+				setpoint.speed[0] = manual_control_setpoint.roll * _param_md_ang_velocity_scale.get() * _max_angular_velocity;
+				// setpoint.speed[0] = 0;
+				// setpoint.speed[1] = 0.0;
+				// setpoint.yaw_rate = 0;
+
+				// if acro mode, we activate the yaw rate control
+				if (_acro_driving) {
+					setpoint.closed_loop_speed_control = false;
+					setpoint.closed_loop_yaw_rate_control = true;
+
+				} else {
+					setpoint.closed_loop_speed_control = false;
+					setpoint.closed_loop_yaw_rate_control = false;
+				}
+
+				setpoint.timestamp = now;
+				_mecanum_drive_setpoint_pub.publish(setpoint);
+			}
+		}
+
+	}
+
+	// else if (_mission_driving) {
+	// 	Mission mode
+	// 	directly receive setpoints from the guidance library
+	// 	_mecanum_drive_guidance.computeGuidance(
+	// 		_mecanum_drive_control.getVehicleYaw(),
+	// 		_mecanum_drive_control.getVehicleBodyYawRate(),
+	// 		dt
+	// 	);
+	// }
+
+	_mecanum_drive_control.control(dt);
+	_mecanum_drive_kinematics.allocate();
+}
+
+int MecanumDrive::task_spawn(int argc, char *argv[])
+{
+	MecanumDrive *instance = new MecanumDrive();
+
+	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 MecanumDrive::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int MecanumDrive::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Rover Mecanum Drive controller.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("mecanum_drive", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int mecanum_drive_main(int argc, char *argv[])
+{
+	return MecanumDrive::main(argc, argv);
+}

src/modules/mecanum_drive/MecanumDrive.hpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#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>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/mecanum_drive_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include "MecanumDriveControl/MecanumDriveControl.hpp"
+// #include "MecanumDriveGuidance/MecanumDriveGuidance.hpp"
+#include "MecanumDriveKinematics/MecanumDriveKinematics.hpp"
+
+using namespace time_literals;
+
+class MecanumDrive : public ModuleBase<MecanumDrive>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	MecanumDrive();
+	~MecanumDrive() 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 _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Publication<mecanum_drive_setpoint_s> _mecanum_drive_setpoint_pub{ORB_ID(mecanum_drive_setpoint)};
+
+	bool _manual_driving = false;
+	bool _mission_driving = false;
+	bool _acro_driving = false;
+	hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */
+
+	// MecanumDriveGuidance _mecanum_drive_guidance{this};
+	MecanumDriveControl _mecanum_drive_control{this};
+	MecanumDriveKinematics _mecanum_drive_kinematics{this};
+
+	float _max_speed{0.f};
+	float _max_angular_velocity{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MD_ANG_SCALE>) _param_md_ang_velocity_scale,
+		(ParamFloat<px4::params::MD_SPEED_SCALE>) _param_md_speed_scale,
+		(ParamFloat<px4::params::MD_WHEEL_BASE>) _param_md_wheel_base,
+		(ParamFloat<px4::params::MD_WHEEL_SPEED>) _param_md_wheel_speed,
+		(ParamFloat<px4::params::MD_WHEEL_RADIUS>) _param_md_wheel_radius
+	)
+};

src/modules/mecanum_drive/MecanumDriveControl/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(MecanumDriveControl
+	MecanumDriveControl.cpp
+)
+
+target_link_libraries(MecanumDriveControl PUBLIC pid)
+target_include_directories(MecanumDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/mecanum_drive/MecanumDriveControl/MecanumDriveControl.cpp

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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 "MecanumDriveControl.hpp"
+
+using namespace matrix;
+using namespace time_literals;
+
+MecanumDriveControl::MecanumDriveControl(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 MecanumDriveControl::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 MecanumDriveControl::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);
+		}
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			_spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s;
+		}
+	}
+
+	_mecanum_drive_setpoint_sub.update(&_mecanum_drive_setpoint);
+
+	// PID to reach setpoint using control_output
+	mecanum_drive_setpoint_s mecanum_drive_control_output = _mecanum_drive_setpoint;
+
+	if (_mecanum_drive_setpoint.closed_loop_speed_control) {
+		mecanum_drive_control_output.speed[0] +=
+			pid_calculate(&_pid_speed, _mecanum_drive_setpoint.speed[0], _vehicle_forward_speed, 0, dt);
+		mecanum_drive_control_output.speed[1] +=
+			pid_calculate(&_pid_speed, _mecanum_drive_setpoint.speed[1], _vehicle_forward_speed, 0, dt);
+	}
+
+	if (_mecanum_drive_setpoint.closed_loop_yaw_rate_control) {
+		mecanum_drive_control_output.yaw_rate +=
+			pid_calculate(&_pid_angular_velocity, _mecanum_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
+	}
+
+
+	if (!_spooled_up) {
+		mecanum_drive_control_output.speed[0] = 0.0f;
+		mecanum_drive_control_output.speed[1] = 0.0f;
+		mecanum_drive_control_output.yaw_rate = 0.0f;
+	}
+
+	mecanum_drive_control_output.timestamp = hrt_absolute_time();
+	_mecanum_drive_control_output_pub.publish(mecanum_drive_control_output);
+}

src/modules/mecanum_drive/MecanumDriveControl/MecanumDriveControl.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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 MecanumDriveControl.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/mecanum_drive_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+class MecanumDriveControl : public ModuleParams
+{
+public:
+	MecanumDriveControl(ModuleParams *parent);
+	~MecanumDriveControl() = 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 _mecanum_drive_setpoint_sub{ORB_ID(mecanum_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::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::Publication<mecanum_drive_setpoint_s> _mecanum_drive_control_output_pub{ORB_ID(mecanum_drive_control_output)};
+
+	mecanum_drive_setpoint_s _mecanum_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.
+
+	bool _spooled_up{false};
+
+	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::COM_SPOOLUP_TIME>) _param_com_spoolup_time
+	)
+};

src/modules/mecanum_drive/MecanumDriveGuidance/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(MecanumDriveGuidance
+	MecanumDriveGuidance.cpp
+)
+
+target_include_directories(MecanumDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/mecanum_drive/MecanumDriveGuidance/MecanumDriveGuidance.cpp

@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   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 "MecanumDriveGuidance.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+MecanumDriveGuidance::MecanumDriveGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+
+	pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+void MecanumDriveGuidance::computeGuidance(float 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 - yaw);
+
+	if (_current_waypoint != current_waypoint) {
+		_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;
+	}
+
+	float desired_speed = 0.f;
+
+	switch (_currentState) {
+	case GuidanceState::TURNING:
+		desired_speed = 0.f;
+
+		if (fabsf(heading_error) < 0.05f) {
+			_currentState = GuidanceState::DRIVING;
+		}
+
+		break;
+
+	case GuidanceState::DRIVING: {
+			const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_md_max_jerk.get(),
+						   _param_md_max_accel.get(), distance_to_next_wp, 0.0f);
+			_forwards_velocity_smoothing.updateDurations(max_velocity);
+			_forwards_velocity_smoothing.updateTraj(dt);
+			desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
+					_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) + heading_error;
+
+	mecanum_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();
+
+	_mecanum_drive_setpoint_pub.publish(output);
+
+	_current_waypoint = current_waypoint;
+}
+
+void MecanumDriveGuidance::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_heading_p_controller,
+			   _param_md_p_gain_heading.get(),  // Proportional gain
+			   0,  // Integral gain
+			   0,  // Derivative gain
+			   0,  // Integral limit
+			   200);  // Output limit
+
+	_forwards_velocity_smoothing.setMaxJerk(_param_md_max_jerk.get());
+	_forwards_velocity_smoothing.setMaxAccel(_param_md_max_accel.get());
+	_forwards_velocity_smoothing.setMaxVel(_max_speed);
+}

src/modules/mecanum_drive/MecanumDriveGuidance/MecanumDriveGuidance.hpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#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 <lib/motion_planning/PositionSmoothing.hpp>
+#include <lib/motion_planning/VelocitySmoothing.hpp>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/mecanum_drive_setpoint.h>
+#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 mecanum drive guidance.
+ */
+class MecanumDriveGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for MecanumDriveGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	MecanumDriveGuidance(ModuleParams *parent);
+	~MecanumDriveGuidance() = 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.
+	 */
+	void computeGuidance(float 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)};
+
+	uORB::Publication<mecanum_drive_setpoint_s> _mecanum_drive_setpoint_pub{ORB_ID(mecanum_drive_setpoint)};
+
+	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 _current_waypoint; ///< The current 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::MD_P_HEADING>) _param_md_p_gain_heading,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::MD_MAX_JERK>) _param_md_max_jerk,
+		(ParamFloat<px4::params::MD_MAX_ACCEL>) _param_md_max_accel
+	)
+};

src/modules/mecanum_drive/MecanumDriveKinematics/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(MecanumDriveKinematics
+	MecanumDriveKinematics.cpp
+)
+
+target_include_directories(MecanumDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_functional_gtest(SRC MecanumDriveKinematicsTest.cpp LINKLIBS MecanumDriveKinematics)

src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematics.cpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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 "MecanumDriveKinematics.hpp"
+
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+MecanumDriveKinematics::MecanumDriveKinematics(ModuleParams *parent) : ModuleParams(parent)
+{}
+
+void MecanumDriveKinematics::allocate()
+{
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_mecanum_drive_control_output_sub.updated()) {
+		_mecanum_drive_control_output_sub.copy(&_mecanum_drive_control_output);
+	}
+
+	const bool setpoint_timeout = (_mecanum_drive_control_output.timestamp + 100_ms) < now;
+
+	Vector4f wheel_speeds =
+		computeInverseKinematics(_mecanum_drive_control_output.speed[0], _mecanum_drive_control_output.speed[1],
+					 _mecanum_drive_control_output.yaw_rate);
+
+	// printf("wheel_speeds: %f, %f, %f, %f\n", (double)wheel_speeds(0), (double)wheel_speeds(1), (double)wheel_speeds(2),
+	//        (double)wheel_speeds(3));
+
+	if (!_armed || setpoint_timeout) {
+		wheel_speeds = {}; // stop
+	}
+
+	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
+
+	// printf("wheel_speeds after all checks: %f, %f, %f, %f\n", (double)wheel_speeds(0), (double)wheel_speeds(1),
+	//        (double)wheel_speeds(2),
+	//        (double)wheel_speeds(3));
+
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 15 see rc.rover_mecanum_defaults
+	wheel_speeds.copyTo(actuator_motors.control);
+	actuator_motors.timestamp = now;
+	_actuator_motors_pub.publish(actuator_motors);
+}
+
+matrix::Vector4f MecanumDriveKinematics::computeInverseKinematics(float linear_velocity_x, float linear_velocity_y,
+		float yaw_rate) const
+{
+	// if (_max_speed < FLT_EPSILON) {
+	// 	return Vector4f();
+	// }
+
+	linear_velocity_x = math::constrain(linear_velocity_x, -_vx_max, _vx_max);
+	linear_velocity_y = math::constrain(linear_velocity_y, -_vy_max, _vy_max);
+	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
+
+	// Define input vector and matrix data
+	float input_data[3] = {linear_velocity_x, linear_velocity_y, yaw_rate};
+	Matrix<float, 3, 1> input(input_data);
+
+	float m_data[12] = {1, -1, -(_lx + _ly), 1, 1, (_lx + _ly), 1, 1, -(_lx + _ly), 1, -1, (_lx + _ly)};
+	Matrix<float, 4, 3> m(m_data);
+
+	// Perform matrix-vector multiplication
+	auto result = m * input; // result is Matrix<float, 4, 1>
+
+	// Precompute scale factor
+	const float scale = 1 / _r;
+
+	// Scale the result by 1/_r
+	for (size_t i = 0; i < 4; ++i) {
+		result(i, 0) *= scale; // Efficiently use precomputed scale factor
+	}
+
+	// Initialize Vector4f with the scaled results
+	Vector4f output(result(0, 0), result(1, 0), result(2, 0), result(3, 0));
+
+	// output = {0.1f, 0.5f, -0.5f, -0.1f};
+
+	// printf("output: %f, %f, %f, %f\n", (double)output(0), (double)output(1), (double)output(2), (double)output(3));
+
+	return output;
+}

src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematics.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#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/mecanum_drive_setpoint.h>
+
+/**
+ * @brief Mecanum Drive Kinematics class for computing the kinematics of a mecanum 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 MecanumDriveKinematics : public ModuleParams
+{
+public:
+	MecanumDriveKinematics(ModuleParams *parent);
+	~MecanumDriveKinematics() = default;
+
+	/**
+	 * @brief Sets the wheel base of the robot.
+	 *
+	 * @param wheel_base The distance between the centers of the wheels.
+	 */
+	void setWheelBase(const float lx, const float ly) { _lx = lx; _ly = ly; };
+
+	/**
+	 * @brief Sets the maximum speed of the robot.
+	 *
+	 * @param max_speed The maximum speed of the robot.
+	 */
+	void setMaxSpeed(const float vx_max, const float vy_max) { _vx_max = vx_max; _vy_max = vy_max; };
+
+	/**
+	 * @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; };
+
+	void setWheelRadius(const float wheel_radius) {_r = wheel_radius;};
+
+	void setArmed(const bool armed) { _armed = armed; };
+
+	void allocate();
+
+	/**
+	 * @brief Computes the inverse kinematics for mecanum 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::Vector4f computeInverseKinematics(float linear_velocity_x, float linear_velocity_y, float yaw_rate) const;
+
+private:
+	uORB::Subscription _mecanum_drive_control_output_sub{ORB_ID(mecanum_drive_control_output)};
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+
+	mecanum_drive_setpoint_s _mecanum_drive_control_output{};
+	bool _armed = false;
+
+	float _lx{0.f};
+	float _ly{0.f};
+	float _vx_max{0.f};
+	float _vy_max{0.f};
+	float _r{1.f};
+	float _max_angular_velocity{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
+};

src/modules/mecanum_drive/MecanumDriveKinematics/MecanumDriveKinematicsTest.cpp

@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+#include "MecanumDriveKinematics.hpp"
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+class MecanumDriveKinematicsTest : public ::testing::Test
+{
+public:
+	MecanumDriveKinematics kinematics{nullptr};
+};
+
+TEST_F(MecanumDriveKinematicsTest, AllZeroInputCase)
+{
+	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_F(MecanumDriveKinematicsTest, InvalidParameterCase)
+{
+	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_F(MecanumDriveKinematicsTest, UnitCase)
+{
+	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_F(MecanumDriveKinematicsTest, UnitSaturationCase)
+{
+	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_F(MecanumDriveKinematicsTest, OppositeUnitSaturationCase)
+{
+	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_F(MecanumDriveKinematicsTest, RandomCase)
+{
+	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_F(MecanumDriveKinematicsTest, RotateInPlaceCase)
+{
+	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_F(MecanumDriveKinematicsTest, StraightMovementCase)
+{
+	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_F(MecanumDriveKinematicsTest, MinInputValuesCase)
+{
+	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_F(MecanumDriveKinematicsTest, MaxSpeedLimitCase)
+{
+	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_F(MecanumDriveKinematicsTest, MaxSpeedForwardsCase)
+{
+	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_F(MecanumDriveKinematicsTest, MaxAngularCase)
+{
+	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/mecanum_drive/module.yaml

@@ -0,0 +1,122 @@
+module_name: Mecanum Drive
+
+parameters:
+  - group: Rover Mecanum Drive
+    definitions:
+      MD_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
+      MD_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
+      MD_SPEED_SCALE:
+        description:
+          short: Manual speed scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      MD_ANG_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+      MD_WHEEL_SPEED:
+        description:
+          short: Maximum wheel speed
+        type: float
+        unit: rad/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.3
+      MD_P_HEADING:
+        description:
+          short: Proportional gain for heading controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      MD_P_SPEED:
+        description:
+          short: Proportional gain for speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      MD_I_SPEED:
+        description:
+          short: Integral gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0
+      MD_P_ANG_VEL:
+        description:
+          short: Proportional gain for angular velocity controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+      MD_I_ANG_VEL:
+        description:
+          short: Integral gain for angular velocity controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0
+      MD_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
+      MD_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

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp

@@ -63,6 +63,8 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA
 {
 	unsigned active_output_count = 0;
 
+	// printf("num_outputs: %d\n", num_outputs);
+
 	for (unsigned i = 0; i < num_outputs; i++) {
 		if (_mixing_output.isFunctionSet(i)) {
 			active_output_count++;
@@ -72,6 +74,10 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA
 		}
 	}
 
+	// printf("active_output_count: %d\n", active_output_count);
+
+	// active_output_count = 4;
+
 	if (active_output_count > 0) {
 		gz::msgs::Actuators wheel_velocity_message;
 		wheel_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
@@ -79,10 +85,14 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA
 		for (unsigned i = 0; i < active_output_count; i++) {
 			// Offsetting the output allows for negative values despite unsigned integer to reverse the wheels
 			static constexpr float output_offset = 100.0f;
+			// printf("outputs[%d]: %d\n", i, outputs[i]);
 			float scaled_output = (float)outputs[i] - output_offset;
+			// printf("scaled_output: %f\n", (double)scaled_output);
 			wheel_velocity_message.set_velocity(i, scaled_output);
 		}
 
+		// printf("\n");
+
 
 		if (_actuators_pub.Valid()) {
 			return _actuators_pub.Publish(wheel_velocity_message);

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp

@@ -50,6 +50,7 @@ class GZMixingInterfaceWheel : public OutputModuleInterface
 {
 public:
 	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
+	// static constexpr int MAX_ACTUATORS = 4;
 
 	GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
 		OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl),

src/modules/simulation/gz_bridge/module.yaml

@@ -33,4 +33,4 @@ actuator_output:
         min: { min: 0, max: 200, default: 0 }
         max: { min: 0, max: 200, default: 200 }
         failsafe: { min: 0, max: 200 }
-      num_channels: 2
+      num_channels: 4