Mecanum rover: add dedicated module for mecanum rovers (#23708)

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

@@ -0,0 +1,64 @@
+#!/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:=rover}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Rover parameters
+param set-default RM_WHEEL_TRACK 0.3
+param set-default RM_MAN_YAW_SCALE 0.1
+param set-default RM_YAW_RATE_I 0
+param set-default RM_YAW_RATE_P 0.01
+param set-default RM_MAX_ACCEL 3
+param set-default RM_MAX_JERK 5
+param set-default RM_MAX_SPEED 4
+param set-default RM_MAX_THR_SPD 7
+param set-default RM_MAX_THR_YAW_R 7.5
+param set-default RM_YAW_P 5
+param set-default RM_YAW_I 0.1
+param set-default RM_MAX_YAW_RATE 180
+param set-default RM_MISS_SPD_DEF 3
+param set-default RM_MISS_VEL_GAIN 1
+param set-default RM_SPEED_I 0.01
+param set-default RM_SPEED_P 0.1
+
+# Pure pursuit parameters
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1
+param set-default PP_LOOKAHD_GAIN 0.5
+
+# 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 0
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 102 # 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 101 # 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 104 # 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 103 # 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 10

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

@@ -86,6 +86,7 @@ px4_add_romfs_files(
 	4012_gz_rover_ackermann
 	4013_gz_x500_lidar
 	4014_gz_x500_mono_cam_down
+	4015_gz_r1_rover_mecanum
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -92,6 +92,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN)
 	)
 endif()
 
+if(CONFIG_MODULES_ROVER_MECANUM)
+	px4_add_romfs_files(
+		rc.rover_mecanum_apps
+		rc.rover_mecanum_defaults
+	)
+endif()
+
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		rc.uuv_apps

ROMFS/px4fmu_common/init.d/airframes/52000_generic_rover_mecanum

@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Generic Rover Mecanum
+#
+# @type Rover
+# @class Rover
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.rover_mecanum_defaults

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

@@ -152,6 +152,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN)
 	)
 endif()
 
+if(CONFIG_MODULES_ROVER_MECANUM)
+	px4_add_romfs_files(
+		# [52000, 52999] Mecanum rovers
+		52000_generic_rover_mecanum
+	)
+endif()
+
 if(CONFIG_MODULES_ROVER_POS_CONTROL)
 	px4_add_romfs_files(
 		# [59000, 59999] Rover position control (deprecated)

ROMFS/px4fmu_common/init.d/rc.rover_mecanum_apps

@@ -0,0 +1,8 @@
+#!/bin/sh
+# Standard apps for a mecanum drive rover.
+
+# Start rover mecanum drive controller.
+rover_mecanum 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
+param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -50,6 +50,15 @@ then
 	. ${R}etc/init.d/rc.rover_ackermann_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-v5/rover.px4board

@@ -19,4 +19,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v5x/rover.px4board

@@ -16,4 +16,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6c/rover.px4board

@@ -15,4 +15,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6u/rover.px4board

@@ -15,4 +15,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6x/rover.px4board

@@ -15,4 +15,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/fmu-v6xrt/rover.px4board

@@ -16,4 +16,5 @@ CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_DRIVERS_ROBOCLAW=y

boards/px4/sitl/default.px4board

@@ -47,6 +47,7 @@ CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_REPLAY=y
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y
+CONFIG_MODULES_ROVER_MECANUM=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_COMMON_SIMULATION=y

msg/CMakeLists.txt

@@ -189,6 +189,9 @@ set(msg_files
 	RoverDifferentialGuidanceStatus.msg
 	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
+	RoverMecanumGuidanceStatus.msg
+	RoverMecanumSetpoint.msg
+	RoverMecanumStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg

msg/RoverMecanumGuidanceStatus.msg

@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
+float32 heading_error      # [rad] Heading error of the pure pursuit controller
+float32 desired_speed      # [m/s] Desired velocity magnitude (speed)
+
+# TOPICS rover_mecanum_guidance_status

msg/RoverMecanumSetpoint.msg

@@ -0,0 +1,11 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 forward_speed_setpoint            # [m/s] Desired forward speed
+float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward speed
+float32 lateral_speed_setpoint            # [m/s] Desired lateral speed
+float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed
+float32 yaw_rate_setpoint                 # [rad/s] Desired yaw rate
+float32 yaw_rate_setpoint_normalized      # [-1, 1] Desired normalized yaw rate
+float32 yaw_setpoint 	                  # [rad] Desired yaw (heading)
+
+# TOPICS rover_mecanum_setpoint

msg/RoverMecanumStatus.msg

@@ -0,0 +1,13 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 measured_forward_speed        # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 measured_lateral_speed        # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
+float32 adjusted_yaw_rate_setpoint    # [rad/s] Yaw rate setpoint output of the closed loop yaw controller
+float32 measured_yaw_rate  	      # [rad/s] Measured yaw rate
+float32 measured_yaw        	      # [rad] Measured yaw
+float32 pid_yaw_rate_integral  	      # Integral of the PID for the closed loop yaw rate controller
+float32 pid_yaw_integral       	      # Integral of the PID for the closed loop yaw controller
+float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller
+float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller
+
+# TOPICS rover_mecanum_status

src/modules/control_allocator/module.yaml

@@ -30,6 +30,7 @@ parameters:
                 10: Helicopter (tail ESC)
                 11: Helicopter (tail Servo)
                 12: Helicopter (Coaxial)
+                13: Rover (Mecanum)
             default: 0
 
         CA_METHOD:
@@ -950,11 +951,11 @@ mixer:
                   - actuator_type: 'motor'
                     instances:
                       - name: 'Throttle'
-                        position: [ -1., 0, 0 ]
+                        position: [ -1, 0, 0 ]
                   - actuator_type: 'servo'
                     instances:
                       - name: 'Steering'
-                        position: [ 1., 0, 0 ]
+                        position: [ 1, 0, 0 ]
 
             6: # Rover (Differential)
                 title: 'Rover (Differential)'
@@ -962,9 +963,9 @@ mixer:
                   - actuator_type: 'motor'
                     instances:
                       - name: 'Right Motor'
-                        position: [ 0, 1., 0 ]
+                        position: [ 0, 1, 0 ]
                       - name: 'Left Motor'
-                        position: [ 0, -1., 0 ]
+                        position: [ 0, -1, 0 ]
 
             7: # Motors (6DOF)
                 actuators:
@@ -1140,3 +1141,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/logger/logged_topics.cpp

@@ -108,6 +108,9 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("rover_differential_guidance_status", 100);
 	add_optional_topic("rover_differential_setpoint", 100);
 	add_optional_topic("rover_differential_status", 100);
+	add_optional_topic("rover_mecanum_guidance_status", 100);
+	add_optional_topic("rover_mecanum_setpoint", 100);
+	add_optional_topic("rover_mecanum_status", 100);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);
 	add_optional_topic("sensor_airflow", 100);

src/modules/rover_mecanum/CMakeLists.txt

@@ -0,0 +1,50 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(RoverMecanumGuidance)
+add_subdirectory(RoverMecanumControl)
+
+px4_add_module(
+	MODULE modules__rover_mecanum
+	MAIN rover_mecanum
+	SRCS
+		RoverMecanum.cpp
+		RoverMecanum.hpp
+	DEPENDS
+		RoverMecanumGuidance
+		RoverMecanumControl
+		px4_work_queue
+		pure_pursuit
+	MODULE_CONFIG
+		module.yaml
+)

src/modules/rover_mecanum/Kconfig

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

src/modules/rover_mecanum/RoverMecanum.cpp

@@ -0,0 +1,323 @@
+/****************************************************************************
+ *
+ *   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 "RoverMecanum.hpp"
+using namespace matrix;
+using namespace time_literals;
+
+RoverMecanum::RoverMecanum() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+	_rover_mecanum_setpoint_pub.advertise();
+}
+
+bool RoverMecanum::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void RoverMecanum::updateParams()
+{
+	ModuleParams::updateParams();
+
+	_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
+}
+
+void RoverMecanum::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	updateSubscriptions();
+
+	// Generate and publish attitude and velocity setpoints
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	switch (_nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
+			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = NAN;
+				rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
+				rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll;
+				rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+				rover_mecanum_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.yaw * _param_rm_man_yaw_scale.get();
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
+			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = NAN;
+				rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
+				rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll;
+				rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.yaw,
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_STAB: {
+			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = NAN;
+				rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
+				rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll;
+				rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
+						STICK_DEADZONE),
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON
+					&& fabsf(rover_mecanum_setpoint.lateral_speed_setpoint_normalized) < FLT_EPSILON)) { // Closed loop yaw rate control
+					_yaw_ctl = false;
+
+				} else { // Closed loop yaw control
+
+					if (!_yaw_ctl) {
+						_desired_yaw = _vehicle_yaw;
+						_yaw_ctl = true;
+					}
+
+					rover_mecanum_setpoint.yaw_setpoint = _desired_yaw;
+					rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+				}
+
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
+			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+						-1.f, 1.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get());;
+				rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
+						-1.f, 1.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get());
+				rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
+						STICK_DEADZONE),
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint) < FLT_EPSILON
+					&& fabsf(rover_mecanum_setpoint.lateral_speed_setpoint) < FLT_EPSILON)) { // Closed loop yaw rate control
+					rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.yaw,
+							-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+					rover_mecanum_setpoint.yaw_setpoint = NAN;
+					_yaw_ctl = false;
+
+				} else { // Closed loop yaw control // TODO: Add cruise control
+					if (!_yaw_ctl) {
+						_desired_yaw = _vehicle_yaw;
+						_yaw_ctl = true;
+					}
+
+					rover_mecanum_setpoint.yaw_setpoint = _desired_yaw;
+					rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+				}
+
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+		_rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _nav_state);
+		break;
+
+	default: // Unimplemented nav states will stop the rover
+		rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+		rover_mecanum_setpoint.timestamp = timestamp;
+		rover_mecanum_setpoint.forward_speed_setpoint = NAN;
+		rover_mecanum_setpoint.forward_speed_setpoint_normalized = 0.f;
+		rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
+		rover_mecanum_setpoint.lateral_speed_setpoint_normalized = 0.f;
+		rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+		rover_mecanum_setpoint.yaw_rate_setpoint_normalized = 0.f;
+		rover_mecanum_setpoint.yaw_setpoint = NAN;
+		_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+		break;
+	}
+
+	if (!_armed) { // Reset when disarmed
+		_rover_mecanum_control.resetControllers();
+		_yaw_ctl = false;
+	}
+
+	_rover_mecanum_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed,
+			_vehicle_lateral_speed);
+
+}
+
+void RoverMecanum::updateSubscriptions()
+{
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
+		updateParams();
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);
+
+		if (vehicle_status.nav_state != _nav_state) {
+			_rover_mecanum_control.resetControllers();
+			_yaw_ctl = false;
+
+			if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
+			    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+				_rover_mecanum_guidance.setDesiredYaw(_vehicle_yaw);
+			}
+		}
+
+		_nav_state = vehicle_status.nav_state;
+		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+	}
+
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+		_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);
+		// Apply threshold to the velocity measurement to cut off measurement noise when standing still
+		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
+		_vehicle_lateral_speed = fabsf(velocity_in_body_frame(1)) > SPEED_THRESHOLD ? velocity_in_body_frame(1) : 0.f;
+	}
+
+	if (_vehicle_angular_velocity_sub.updated()) {
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
+
+		// Apply threshold to the yaw rate measurement if the rover is standing still to avoid stuttering due to closed loop yaw(rate) control
+		if ((fabsf(_vehicle_forward_speed) > FLT_EPSILON && fabsf(_vehicle_lateral_speed) > FLT_EPSILON)
+		    || fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD) {
+			_vehicle_yaw_rate = vehicle_angular_velocity.xyz[2];
+
+		} else {
+			_vehicle_yaw_rate = 0.f;
+		}
+
+	}
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
+	}
+
+}
+
+int RoverMecanum::task_spawn(int argc, char *argv[])
+{
+	RoverMecanum *instance = new RoverMecanum();
+
+	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 RoverMecanum::custom_command(int argc, char *argv[])
+{
+	return print_usage("unk_timestampn command");
+}
+
+int RoverMecanum::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Rover Mecanum controller.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("rover_mecanum", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int rover_mecanum_main(int argc, char *argv[])
+{
+	return RoverMecanum::main(argc, argv);
+}

src/modules/rover_mecanum/RoverMecanum.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#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/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+
+// Standard libraries
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+
+// Local includes
+#include "RoverMecanumGuidance/RoverMecanumGuidance.hpp"
+#include "RoverMecanumControl/RoverMecanumControl.hpp"
+
+// Constants
+static constexpr float YAW_RATE_THRESHOLD =
+	0.02f; // [rad/s] Threshold for the yaw rate measurement to avoid stuttering when the rover is standing still
+static constexpr float SPEED_THRESHOLD =
+	0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still
+static constexpr float STICK_DEADZONE =
+	0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
+
+using namespace time_literals;
+
+class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	RoverMecanum();
+	~RoverMecanum() 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;
+
+	/**
+	 * @brief Update uORB subscriptions.
+	 */
+	void updateSubscriptions();
+
+	// uORB Subscriptions
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	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 Publications
+	uORB::Publication<rover_mecanum_setpoint_s> _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)};
+
+	// Instances
+	RoverMecanumGuidance _rover_mecanum_guidance{this};
+	RoverMecanumControl _rover_mecanum_control{this};
+
+	// Variables
+	matrix::Quatf _vehicle_attitude_quaternion{};
+	float _vehicle_yaw_rate{0.f};
+	float _vehicle_forward_speed{0.f};
+	float _vehicle_lateral_speed{0.f};
+	float _vehicle_yaw{0.f};
+	float _max_yaw_rate{0.f};
+	int _nav_state{0};
+	bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in position mode
+	float _desired_yaw{0.f}; // Yaw setpoint for position mode
+	bool _armed{false};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
+		(ParamFloat<px4::params::RM_MAN_YAW_SCALE>) _param_rm_man_yaw_scale,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate
+	)
+};

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

src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp

@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   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 "RoverMecanumControl.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_mecanum_status_pub.advertise();
+	pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_forward_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_lateral_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
+	pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f);
+}
+
+void RoverMecanumControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
+	pid_set_parameters(&_pid_yaw_rate,
+			   _param_rm_yaw_rate_p.get(), // Proportional gain
+			   _param_rm_yaw_rate_i.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
+	pid_set_parameters(&_pid_forward_throttle,
+			   _param_rm_p_gain_speed.get(), // Proportional gain
+			   _param_rm_i_gain_speed.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
+	pid_set_parameters(&_pid_lateral_throttle,
+			   _param_rm_p_gain_speed.get(), // Proportional gain
+			   _param_rm_i_gain_speed.get(), // Integral gain
+			   0.f, // Derivative gain
+			   1.f, // Integral limit
+			   1.f); // Output limit
+	pid_set_parameters(&_pid_yaw,
+			   _param_rm_p_gain_yaw.get(),  // Proportional gain
+			   _param_rm_i_gain_yaw.get(),  // Integral gain
+			   0.f,  // Derivative gain
+			   _max_yaw_rate,  // Integral limit
+			   _max_yaw_rate);  // Output limit
+}
+
+void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
+		const float vehicle_forward_speed, const float vehicle_lateral_speed)
+{
+	// Timestamps
+	hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	// Update mecanum setpoint
+	_rover_mecanum_setpoint_sub.update(&_rover_mecanum_setpoint);
+
+	// Closed loop yaw control
+	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) {
+		const float heading_error = matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw);
+		_rover_mecanum_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0.f, 0.f, dt);
+
+	} else {
+		pid_reset_integral(&_pid_yaw);
+	}
+
+	// Yaw rate control
+	float speed_diff_normalized{0.f};
+
+	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { 	// Closed loop yaw rate control
+		if (_param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward
+			const float speed_diff = _rover_mecanum_setpoint.yaw_rate_setpoint * _param_rm_wheel_track.get();
+			speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rm_max_thr_yaw_r.get(),
+						_param_rm_max_thr_yaw_r.get(), -1.f, 1.f);
+		}
+
+		speed_diff_normalized = math::constrain(speed_diff_normalized +
+							pid_calculate(&_pid_yaw_rate, _rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0.f, dt),
+							-1.f, 1.f); // Feedback
+
+	} else { // Use normalized setpoint
+		speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint_normalized) ? math::constrain(
+						_rover_mecanum_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f;
+	}
+
+	// Speed control
+	float forward_throttle{0.f};
+	float lateral_throttle{0.f};
+
+	if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint)
+	    && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control
+		// Closed loop forward speed control
+		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+			forward_throttle = math::interpolate<float>(_rover_mecanum_setpoint.forward_speed_setpoint,
+					   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
+		}
+
+		forward_throttle += pid_calculate(&_pid_forward_throttle, _rover_mecanum_setpoint.forward_speed_setpoint,
+						  vehicle_forward_speed, 0, dt); // Feedback
+
+		// Closed loop lateral speed control
+		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+			lateral_throttle = math::interpolate<float>(_rover_mecanum_setpoint.lateral_speed_setpoint,
+					   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
+		}
+
+		lateral_throttle += pid_calculate(&_pid_lateral_throttle, _rover_mecanum_setpoint.lateral_speed_setpoint,
+						  vehicle_lateral_speed, 0, dt); // Feedback
+
+	} else { // Use normalized setpoint
+		forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain(
+					   _rover_mecanum_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
+		lateral_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized) ? math::constrain(
+					   _rover_mecanum_setpoint.lateral_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
+	}
+
+	// Publish rover mecanum status (logging)
+	rover_mecanum_status_s rover_mecanum_status{};
+	rover_mecanum_status.timestamp = _timestamp;
+	rover_mecanum_status.measured_forward_speed = vehicle_forward_speed;
+	rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed;
+	rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint;
+	rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate;
+	rover_mecanum_status.measured_yaw = vehicle_yaw;
+	rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
+	rover_mecanum_status.pid_yaw_integral = _pid_yaw.integral;
+	rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.integral;
+	rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.integral;
+	_rover_mecanum_status_pub.publish(rover_mecanum_status);
+
+	// Publish to motors
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get();
+	computeInverseKinematics(forward_throttle, lateral_throttle,
+				 speed_diff_normalized).copyTo(actuator_motors.control);
+	actuator_motors.timestamp = _timestamp;
+	_actuator_motors_pub.publish(actuator_motors);
+
+}
+
+matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_throttle, float lateral_throttle,
+		float speed_diff)
+{
+	// Prioritize ratio between forward and lateral speed over either magnitude
+	float combined_speed =  fabsf(forward_throttle) + fabsf(lateral_throttle);
+
+	if (combined_speed > 1.f) {
+		forward_throttle /= combined_speed;
+		lateral_throttle /= combined_speed;
+		combined_speed = 1.f;
+	}
+
+	// Prioritize yaw rate over forward and lateral speed
+	const float total_speed = combined_speed + fabsf(speed_diff);
+
+	if (total_speed > 1.f) {
+		const float excess_velocity = fabsf(total_speed - 1.f);
+		const float forward_throttle_temp = forward_throttle - sign(forward_throttle) * 0.5f * excess_velocity;
+		const float lateral_throttle_temp = lateral_throttle - sign(lateral_throttle) * 0.5f * excess_velocity;
+
+		if (sign(forward_throttle_temp) == sign(forward_throttle) && sign(lateral_throttle) == sign(lateral_throttle_temp)) {
+			forward_throttle = forward_throttle_temp;
+			lateral_throttle = lateral_throttle_temp;
+
+		} else {
+			forward_throttle = lateral_throttle = 0.f;
+		}
+	}
+
+	// Calculate motor commands
+	const float input_data[3] = {forward_throttle, lateral_throttle, speed_diff};
+	const Matrix<float, 3, 1> input(input_data);
+	const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f};
+	const Matrix<float, 4, 3> m(m_data);
+	const Vector4f motor_commands = m * input;
+
+	return motor_commands;
+}
+
+void RoverMecanumControl::resetControllers()
+{
+	pid_reset_integral(&_pid_forward_throttle);
+	pid_reset_integral(&_pid_lateral_throttle);
+	pid_reset_integral(&_pid_yaw_rate);
+	pid_reset_integral(&_pid_yaw);
+}

src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp

@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+#include <uORB/topics/rover_mecanum_status.h>
+#include <uORB/topics/actuator_motors.h>
+
+
+// Standard libraries
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <math.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for mecanum rover guidance.
+ */
+class RoverMecanumControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for RoverMecanumControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	RoverMecanumControl(ModuleParams *parent);
+	~RoverMecanumControl() = default;
+
+	/**
+	 * @brief Compute motor commands based on setpoints
+	 * @param vehicle_yaw Yaw of the vehicle [rad]
+	 * @param vehicle_yaw_rate Yaw rate of the vehicle [rad/s]
+	 * @param vehicle_forward_speed Forward speed of the vehicle [m/s]
+	 * @param vehicle_lateral_speed Lateral speed of the vehicle [m/s]
+	 */
+	void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed,
+				  float vehicle_lateral_speed);
+
+	/**
+	 * @brief Reset PID controllers
+	 */
+	void resetControllers();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Turn normalized speed setpoints into normalized motor commands.
+	 *
+	 * @param forward_speed Normalized linear speed in body forward direction [-1, 1].
+	 * @param lateral_speed Normalized linear speed in body lateral direction [-1, 1].
+	 * @param speed_diff Normalized speed difference between left and right wheels [-1, 1].
+	 * @return matrix::Vector4f: Normalized motor inputs [-1, 1]
+	 */
+	matrix::Vector4f computeInverseKinematics(float forward_speed, float lateral_speed, float speed_diff);
+
+	// uORB subscriptions
+	uORB::Subscription _rover_mecanum_setpoint_sub{ORB_ID(rover_mecanum_setpoint)};
+
+	// uORB publications
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<rover_mecanum_status_s> _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)};
+
+	// Variables
+	rover_mecanum_setpoint_s _rover_mecanum_setpoint{};
+	hrt_abstime _timestamp{0};
+	float _max_yaw_rate{0.f};
+
+	// Controllers
+	PID_t _pid_forward_throttle; // PID for the closed loop forward speed control
+	PID_t _pid_lateral_throttle; // PID for the closed loop lateral speed control
+	PID_t _pid_yaw; // PID for the closed loop yaw control
+	PID_t _pid_yaw_rate; // PID for the closed loop yaw rate control
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
+		(ParamFloat<px4::params::RM_MAX_THR_SPD>) _param_rm_max_thr_spd,
+		(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
+		(ParamFloat<px4::params::RM_YAW_RATE_P>) _param_rm_yaw_rate_p,
+		(ParamFloat<px4::params::RM_YAW_RATE_I>) _param_rm_yaw_rate_i,
+		(ParamFloat<px4::params::RM_SPEED_P>) _param_rm_p_gain_speed,
+		(ParamFloat<px4::params::RM_SPEED_I>) _param_rm_i_gain_speed,
+		(ParamFloat<px4::params::RM_YAW_P>) _param_rm_p_gain_yaw,
+		(ParamFloat<px4::params::RM_YAW_I>) _param_rm_i_gain_yaw,
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
+};

src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt

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

src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp

@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ *   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 "RoverMecanumGuidance.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_max_velocity_magnitude = _param_rm_miss_spd_def.get();
+	_rover_mecanum_guidance_status_pub.advertise();
+}
+
+void RoverMecanumGuidance::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state)
+{
+	updateSubscriptions();
+
+	// Calculate desired velocity magnitude
+	float desired_velocity_magnitude{_max_velocity_magnitude};
+	const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
+					  _curr_wp(0), _curr_wp(1));
+
+	if (_param_rm_max_jerk.get() > FLT_EPSILON && _param_rm_max_accel.get() > FLT_EPSILON
+	    && _param_rm_miss_vel_gain.get() > FLT_EPSILON) {
+		const float speed_reduction = math::constrain(_param_rm_miss_vel_gain.get() * math::interpolate(
+						      M_PI_F - _waypoint_transition_angle, 0.f,
+						      M_PI_F, 0.f, 1.f), 0.f, 1.f);
+		desired_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(_param_rm_max_jerk.get(),
+					     _param_rm_max_accel.get(), distance_to_curr_wp, _max_velocity_magnitude * (1.f - speed_reduction));
+		desired_velocity_magnitude = math::constrain(desired_velocity_magnitude, -_max_velocity_magnitude,
+					     _max_velocity_magnitude);
+	}
+
+
+	// Calculate heading error
+	const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
+				      desired_velocity_magnitude);
+	const float heading_error = matrix::wrap_pi(desired_heading - yaw);
+
+	// Catch return to launch
+	const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
+					  _next_wp(0), _next_wp(1));
+
+	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+		_mission_finished = distance_to_next_wp < _param_nav_acc_rad.get();
+	}
+
+	// Guidance logic
+	Vector2f desired_velocity(0.f, 0.f);
+
+	if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) {
+		desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error));
+	}
+
+	// Timestamp
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	// Publish mecanum controller status (logging)
+	rover_mecanum_guidance_status_s rover_mecanum_guidance_status{};
+	rover_mecanum_guidance_status.timestamp = timestamp;
+	rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance();
+	rover_mecanum_guidance_status.heading_error = heading_error;
+	rover_mecanum_guidance_status.desired_speed = desired_velocity_magnitude;
+	_rover_mecanum_guidance_status_pub.publish(rover_mecanum_guidance_status);
+
+	// Publish setpoints
+	rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+	rover_mecanum_setpoint.timestamp = timestamp;
+	rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0);
+	rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN;
+	rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1);
+	rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN;
+	rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+	rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN;
+	rover_mecanum_setpoint.yaw_setpoint = _desired_yaw;
+	_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+}
+
+void RoverMecanumGuidance::updateSubscriptions()
+{
+	if (_vehicle_global_position_sub.updated()) {
+		vehicle_global_position_s vehicle_global_position{};
+		_vehicle_global_position_sub.copy(&vehicle_global_position);
+		_curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
+	}
+
+	if (_local_position_sub.updated()) {
+		vehicle_local_position_s local_position{};
+		_local_position_sub.copy(&local_position);
+
+		if (!_global_ned_proj_ref.isInitialized()
+		    || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
+			_global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
+		}
+
+		_curr_pos_ned = Vector2f(local_position.x, local_position.y);
+	}
+
+	if (_position_setpoint_triplet_sub.updated()) {
+		updateWaypoints();
+	}
+
+	if (_mission_result_sub.updated()) {
+		mission_result_s mission_result{};
+		_mission_result_sub.copy(&mission_result);
+		_mission_finished = mission_result.finished;
+	}
+
+	if (_home_position_sub.updated()) {
+		home_position_s home_position{};
+		_home_position_sub.copy(&home_position);
+		_home_position = Vector2d(home_position.lat, home_position.lon);
+	}
+}
+
+void RoverMecanumGuidance::updateWaypoints()
+{
+	position_setpoint_triplet_s position_setpoint_triplet{};
+	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+
+	// Global waypoint coordinates
+	if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
+		_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
+
+	} else {
+		_curr_wp = Vector2d(0, 0);
+	}
+
+	if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) {
+		_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
+
+	} else {
+		_prev_wp = _curr_pos;
+	}
+
+	if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.next.lon)) {
+		_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
+
+	} else {
+		_next_wp = _home_position;
+	}
+
+	// NED waypoint coordinates
+	_curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));
+	_prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
+	_next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));
+
+	// Distances
+	const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned;
+	const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned;
+	float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
+	cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
+	_waypoint_transition_angle = acosf(cosin);
+
+	// Waypoint cruising speed
+	if (position_setpoint_triplet.current.cruising_speed > FLT_EPSILON) {
+		_max_velocity_magnitude = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f,
+					  _param_rm_max_speed.get());
+
+	} else {
+		_max_velocity_magnitude = _param_rm_miss_spd_def.get();
+	}
+
+	// Waypoint yaw setpoint
+	if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) {
+		_desired_yaw = position_setpoint_triplet.current.yaw;
+	}
+}

src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+#include <lib/pure_pursuit/PurePursuit.hpp>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/rover_mecanum_guidance_status.h>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+
+// Standard libraries
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for mecanum rover guidance.
+ */
+class RoverMecanumGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for RoverMecanumGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	RoverMecanumGuidance(ModuleParams *parent);
+	~RoverMecanumGuidance() = default;
+
+	/**
+	 * @brief Compute guidance for the vehicle.
+	 * @param yaw The yaw orientation of the vehicle in radians.
+	 * @param nav_state Navigation state of the rover.
+	 */
+	void computeGuidance(float yaw, int nav_state);
+
+	void setDesiredYaw(float desired_yaw) { _desired_yaw = desired_yaw; };
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Update uORB subscriptions
+	 */
+	void updateSubscriptions();
+
+	/**
+	 * @brief Update global/ned waypoint coordinates
+	 */
+	void updateWaypoints();
+
+	// uORB subscriptions
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
+	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+
+	// uORB publications
+	uORB::Publication<rover_mecanum_guidance_status_s> _rover_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)};
+	uORB::Publication<rover_mecanum_setpoint_s> _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)};
+
+	// Variables
+	MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates.
+	PurePursuit _pure_pursuit{this}; // Pure pursuit library
+	bool _mission_finished{false};
+
+	// Waypoints
+	Vector2d _curr_pos{};
+	Vector2f _curr_pos_ned{};
+	Vector2d _prev_wp{};
+	Vector2f _prev_wp_ned{};
+	Vector2d _curr_wp{};
+	Vector2f _curr_wp_ned{};
+	Vector2d _next_wp{};
+	Vector2f _next_wp_ned{};
+	Vector2d _home_position{};
+	float _max_velocity_magnitude{0.f};
+	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	float _desired_yaw{0.f};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::RM_MAX_JERK>) _param_rm_max_jerk,
+		(ParamFloat<px4::params::RM_MAX_ACCEL>) _param_rm_max_accel,
+		(ParamFloat<px4::params::RM_MISS_SPD_DEF>) _param_rm_miss_spd_def,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
+		(ParamFloat<px4::params::RM_MISS_VEL_GAIN>) _param_rm_miss_vel_gain
+	)
+};

src/modules/rover_mecanum/module.yaml

@@ -0,0 +1,201 @@
+module_name: Rover Mecanum
+
+parameters:
+  - group: Rover Mecanum
+    definitions:
+
+      RM_MAN_YAW_SCALE:
+        description:
+          short: Manual yaw rate scale
+          long: |
+            In Manual mode the setpoint for the yaw rate received from the control stick
+            is scaled by this value.
+        type: float
+        min: 0.01
+        max: 1
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RM_WHEEL_TRACK:
+        description:
+          short: Wheel track
+          long: Distance from the center of the right wheels to the center of the left wheels.
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+
+      RM_MAX_YAW_RATE:
+        description:
+          short: Maximum allowed yaw rate for the rover.
+          long: |
+            This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode.
+        type: float
+        unit: deg/s
+        min: 0.01
+        max: 1000
+        increment: 0.01
+        decimal: 2
+        default: 90
+
+      RM_MAX_THR_YAW_R:
+        description:
+          short: Yaw rate turning left/right wheels at max speed in opposite directions
+          long: |
+            This parameter is used to calculate the feedforward term of the closed loop yaw rate control.
+            The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate.
+            This desired speed difference is then linearly mapped to normalized motor commands.
+            A good starting point is twice the speed the rover drives at maximum throttle (RM_MAX_THRTL_SPD)).
+            Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 2
+
+      RM_YAW_RATE_P:
+        description:
+          short: Proportional gain for the closed-loop yaw rate controller.
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RM_YAW_RATE_I:
+        description:
+          short: Integral gain for the closed-loop yaw rate controller.
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0
+
+      RM_YAW_P:
+        description:
+          short: Proportional gain for closed loop yaw controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RM_YAW_I:
+        description:
+            short: Integral gain for closed loop yaw controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.1
+
+      RM_MAX_SPEED:
+        description:
+          short: Maximum speed the rover is allowed to drive
+          long: |
+            This parameter is used cap the maximum speed the rover is allowed to drive
+            and to map stick inputs to desired speeds in position mode.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 7
+
+      RM_MAX_THR_SPD:
+        description:
+          short: Speed the rover drives at maximum throttle
+          long: |
+            This parameter is used to calculate the feedforward term of the closed loop speed control which linearly
+            maps desired speeds to normalized motor commands [-1. 1].
+            A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode.
+            Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RM_SPEED_P:
+        description:
+          short: Proportional gain for speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 3
+        default: 1
+
+      RM_SPEED_I:
+        description:
+          short: Integral gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 3
+        default: 0
+
+      RM_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
+
+      RM_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
+
+      RM_MISS_SPD_DEF:
+        description:
+          short: Default rover speed during a mission
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RM_MISS_VEL_GAIN:
+        description:
+          short: Tuning parameter for the velocity reduction during waypoint transition
+          long: |
+            The waypoint transition speed is calculated as:
+            Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN)
+            The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP
+            interpolated from [0, 180] -> [0, 1].
+            Higher value -> More velocity reduction during cornering.
+        type: float
+        min: 0.05
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1

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