mecanum: centralize mode management, resets and checks

src/modules/rover_mecanum/CMakeLists.txt

@@ -36,6 +36,7 @@ add_subdirectory(MecanumRateControl)
 add_subdirectory(MecanumAttControl)
 add_subdirectory(MecanumVelControl)
 add_subdirectory(MecanumPosControl)
+add_subdirectory(MecanumDriveModes)
 
 px4_add_module(
 	MODULE modules__rover_mecanum
@@ -49,6 +50,9 @@ px4_add_module(
 		MecanumAttControl
 		MecanumVelControl
 		MecanumPosControl
+		MecanumAutoMode
+		MecanumManualMode
+		MecanumOffboardMode
 		px4_work_queue
 		rover_control
 		pure_pursuit

src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp

@@ -88,15 +88,6 @@ void MecanumActControl::updateActControl()
 		actuator_motors.timestamp = _timestamp;
 		_actuator_motors_pub.publish(actuator_motors);
 
-	} else {
-		actuator_motors_s actuator_motors{};
-		actuator_motors.reversible_flags = _param_r_rev.get();
-		actuator_motors.control[0] = 0.f;
-		actuator_motors.control[1] = 0.f;
-		actuator_motors.control[2] = 0.f;
-		actuator_motors.control[3] = 0.f;
-		actuator_motors.timestamp = _timestamp;
-		_actuator_motors_pub.publish(actuator_motors);
 	}
 
 }
@@ -125,21 +116,6 @@ Vector4f MecanumActControl::computeInverseKinematics(float throttle_body_x, floa
 	return motor_commands;
 }
 
-void MecanumActControl::manualManualMode()
-{
-	manual_control_setpoint_s manual_control_setpoint{};
-	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
-	rover_steering_setpoint_s rover_steering_setpoint{};
-	rover_steering_setpoint.timestamp = _timestamp;
-	rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw;
-	_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
-	rover_throttle_setpoint_s rover_throttle_setpoint{};
-	rover_throttle_setpoint.timestamp = _timestamp;
-	rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
-	rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
-	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-}
-
 void MecanumActControl::stopVehicle()
 {
 	actuator_motors_s actuator_motors{};

src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp

@@ -47,7 +47,6 @@
 #include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/rover_steering_setpoint.h>
 #include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
 
 /**
  * @brief Class for mecanum actuator control.
@@ -67,11 +66,6 @@ public:
 	 */
 	void updateActControl();
 
-	/**
-	 * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
-	 */
-	void manualManualMode();
-
 	/**
 	 * @brief Stop the vehicle by sending 0 commands to motors and servos.
 	 */
@@ -97,12 +91,9 @@ private:
 	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
 	uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 
 	// uORB publications
-	uORB::Publication<actuator_motors_s> 	     _actuator_motors_pub{ORB_ID(actuator_motors)};
-	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
+	uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
 
 	// Variables
 	hrt_abstime _timestamp{0};
@@ -116,9 +107,9 @@ private:
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
-		(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
-		(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
+		(ParamInt<px4::params::CA_R_REV>)           _param_r_rev,
+		(ParamFloat<px4::params::RO_ACCEL_LIM>)     _param_ro_accel_limit,
+		(ParamFloat<px4::params::RO_DECEL_LIM>)     _param_ro_decel_limit,
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
 	)
 };

src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp

@@ -38,8 +38,6 @@ using namespace time_literals;
 MecanumAttControl::MecanumAttControl(ModuleParams *parent) : ModuleParams(parent)
 {
 	_rover_rate_setpoint_pub.advertise();
-	_rover_throttle_setpoint_pub.advertise();
-	_rover_attitude_setpoint_pub.advertise();
 	_rover_attitude_status_pub.advertise();
 	updateParams();
 }
@@ -52,21 +50,20 @@ void MecanumAttControl::updateParams()
 		_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
 	}
 
+	// Set up PID controller
 	_pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f);
 	_pid_yaw.setIntegralLimit(_max_yaw_rate);
 	_pid_yaw.setOutputLimit(_max_yaw_rate);
+
+	// Set up slew rate
 	_adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate);
 }
 
 void MecanumAttControl::updateAttControl()
 {
-	const hrt_abstime timestamp_prev = _timestamp;
+	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
@@ -75,17 +72,20 @@ void MecanumAttControl::updateAttControl()
 		_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
 	}
 
-	if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
+	if (_rover_attitude_setpoint_sub.updated()) {
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		_rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint);
+		_yaw_setpoint = rover_attitude_setpoint.yaw_setpoint;
+	}
 
-		if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
-			generateAttitudeAndThrottleSetpoint();
-		}
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
+						_vehicle_yaw, _yaw_setpoint, dt);
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = _timestamp;
+		rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
 
-		generateRateSetpoint();
-
-	} else { // Reset pid and slew rate when attitude control is not active
-		_pid_yaw.resetIntegral();
-		_adjusted_yaw_setpoint.setForcedValue(0.f);
 	}
 
 	// Publish attitude controller status (logging only)
@@ -97,93 +97,6 @@ void MecanumAttControl::updateAttControl()
 
 }
 
-void MecanumAttControl::generateAttitudeAndThrottleSetpoint()
-{
-	const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-				       && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
-
-	if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode
-		manual_control_setpoint_s manual_control_setpoint{};
-
-		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-
-			rover_throttle_setpoint_s rover_throttle_setpoint{};
-			rover_throttle_setpoint.timestamp = _timestamp;
-			rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
-			rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
-			_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-
-			const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
-						_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
-						_max_yaw_rate / _param_ro_yaw_p.get());
-
-			if (fabsf(yaw_delta) > FLT_EPSILON) { // Closed loop yaw rate control
-				_stab_yaw_setpoint = NAN;
-				const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
-				rover_attitude_setpoint_s rover_attitude_setpoint{};
-				rover_attitude_setpoint.timestamp = _timestamp;
-				rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
-				_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-
-			} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
-				if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
-					_stab_yaw_setpoint = _vehicle_yaw;
-				}
-
-				rover_attitude_setpoint_s rover_attitude_setpoint{};
-				rover_attitude_setpoint.timestamp = _timestamp;
-				rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
-				_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-
-			}
-
-		}
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control
-		trajectory_setpoint_s trajectory_setpoint{};
-		_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		const bool offboard_att_control = _offboard_control_mode.attitude;
-
-		if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) {
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = _timestamp;
-			rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-		}
-	}
-}
-
-void MecanumAttControl::generateRateSetpoint()
-{
-	if (_rover_attitude_setpoint_sub.updated()) {
-		_rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint);
-	}
-
-	if (_rover_rate_setpoint_sub.updated()) {
-		_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
-	}
-
-	// Check if a new rate setpoint was already published from somewhere else
-	if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update
-	    && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) {
-		return;
-	}
-
-	const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
-					_vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt);
-
-	_last_rate_setpoint_update = _timestamp;
-	rover_rate_setpoint_s rover_rate_setpoint{};
-	rover_rate_setpoint.timestamp = _timestamp;
-	rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
-	_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-}
-
 bool MecanumAttControl::runSanityChecks()
 {
 	bool ret = true;
@@ -194,13 +107,9 @@ bool MecanumAttControl::runSanityChecks()
 
 	if (_param_ro_yaw_p.get() < FLT_EPSILON) {
 		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get());
-		}
+		events::send<float>(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error,
+				    "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get());
 	}
 
-	_prev_param_check_passed = ret;
 	return ret;
 }

src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp

@@ -48,15 +48,9 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/rover_rate_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/rover_attitude_status.h>
 #include <uORB/topics/rover_attitude_setpoint.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/trajectory_setpoint.h>
 
 /**
  * @brief Class for mecanum attitude control.
@@ -72,10 +66,21 @@ public:
 	~MecanumAttControl() = default;
 
 	/**
-	 * @brief Update attitude controller.
+	 * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
 	 */
 	void updateAttControl();
 
+	/**
+	 * @brief Reset attitude controller.
+	 */
+	void reset() {_pid_yaw.resetIntegral(); _yaw_setpoint = NAN;};
+
+	/**
+	 * @brief Check if the necessary parameters are set.
+	 * @return True if all checks pass.
+	 */
+	bool runSanityChecks();
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -83,51 +88,20 @@ protected:
 	void updateParams() override;
 
 private:
-	/**
-	 * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode)
-	 * 	  or trajectorySetpoint (Offboard attitude control).
-	 */
-	void generateAttitudeAndThrottleSetpoint();
-
-	/**
-	 * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
-	 */
-	void generateRateSetpoint();
-
-	/**
-	 * @brief Check if the necessary parameters are set.
-	 * @return True if all checks pass.
-	 */
-	bool runSanityChecks();
 
 	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 	uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
-	vehicle_control_mode_s _vehicle_control_mode{};
-	rover_attitude_setpoint_s _rover_attitude_setpoint{};
-	rover_rate_setpoint_s _rover_rate_setpoint{};
-	offboard_control_mode_s _offboard_control_mode{};
 
 	// uORB publications
-	uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
+	uORB::Publication<rover_rate_setpoint_s>   _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
 	uORB::Publication<rover_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
 
 	// Variables
-	hrt_abstime _timestamp{0};
-	hrt_abstime _last_rate_setpoint_update{0};
 	float _vehicle_yaw{0.f};
-	float _dt{0.f};
+	hrt_abstime _timestamp{0};
 	float _max_yaw_rate{0.f};
-	float _stab_yaw_setpoint{NAN}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad]
-	bool _prev_param_check_passed{true};
+	float _yaw_setpoint{NAN};
 
 	// Controllers
 	PID _pid_yaw;
@@ -135,8 +109,8 @@ private:
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
-		(ParamFloat<px4::params::RO_YAW_P>)         _param_ro_yaw_p,
-		(ParamFloat<px4::params::RO_YAW_STICK_DZ>)  _param_ro_yaw_stick_dz
+		(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
+		(ParamFloat<px4::params::RO_YAW_P>)        _param_ro_yaw_p,
+		(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz
 	)
 };

src/modules/rover_mecanum/MecanumDriveModes/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2025 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(MecanumAutoMode)
+add_subdirectory(MecanumManualMode)
+add_subdirectory(MecanumOffboardMode)

src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/CMakeLists.txt

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

src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 "MecanumAutoMode.hpp"
+
+using namespace time_literals;
+
+MecanumAutoMode::MecanumAutoMode(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_position_setpoint_pub.advertise();
+}
+
+void MecanumAutoMode::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void MecanumAutoMode::autoControl()
+{
+	if (_position_setpoint_triplet_sub.updated()) {
+		position_setpoint_triplet_s position_setpoint_triplet{};
+		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+		int curr_wp_type = position_setpoint_triplet.current.type;
+
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+
+		MapProjection global_ned_proj_ref{};
+
+		if (!global_ned_proj_ref.isInitialized()
+		    || (global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
+			global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
+							  vehicle_local_position.ref_timestamp);
+		}
+
+		Vector2f curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+		Vector2f curr_wp_ned{NAN, NAN};
+		Vector2f prev_wp_ned{NAN, NAN};
+		Vector2f next_wp_ned{NAN, NAN};
+
+		RoverControl::globalToLocalSetpointTriplet(curr_wp_ned, prev_wp_ned, next_wp_ned, position_setpoint_triplet,
+				curr_pos_ned, global_ned_proj_ref);
+
+		float waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+
+		// Waypoint cruising speed
+		float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
+					       position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
+
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = curr_wp_ned(0);
+		rover_position_setpoint.position_ned[1] = curr_wp_ned(1);
+		rover_position_setpoint.start_ned[0] = prev_wp_ned(0);
+		rover_position_setpoint.start_ned[1] = prev_wp_ned(1);
+		rover_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle,
+							_param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), curr_wp_type);
+		rover_position_setpoint.cruising_speed = cruising_speed;
+		rover_position_setpoint.yaw = PX4_ISFINITE(position_setpoint_triplet.current.yaw) ?
+					      position_setpoint_triplet.current.yaw : NAN;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
+	}
+}
+
+float MecanumAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle,
+				    const float max_speed, const float miss_spd_gain, int curr_wp_type)
+{
+	// Upcoming stop
+	if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		return 0.f;
+	}
+
+	// Straight line speed
+	if (miss_spd_gain > FLT_EPSILON) {
+		const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
+					      M_PI_F, 0.f, 1.f), 0.f, 1.f);
+		return max_speed * (1.f - speed_reduction);
+	}
+
+	return cruising_speed; // Fallthrough
+
+}

src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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>
+
+// Libraries
+#include <lib/rover_control/RoverControl.hpp>
+#include <math.h>
+
+// uORB includes
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/rover_position_setpoint.h>
+
+/**
+ * @brief Class for Mecanum auto mode.
+ */
+class MecanumAutoMode : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for auto mode.
+	 * @param parent The parent ModuleParams object.
+	 */
+	MecanumAutoMode(ModuleParams *parent);
+	~MecanumAutoMode() = default;
+
+	/**
+	 * @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet.
+	 */
+	void autoControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Calculate the speed at which the rover should arrive at the current waypoint. During waypoint transition the speed is restricted to
+	 * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
+	 * @param cruising_speed Cruising speed [m/s].
+	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	 * @param max_speed Maximum speed setpoint [m/s]
+	 * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
+	 * @param curr_wp_type Type of the current waypoint.
+	 * @return Speed setpoint [m/s].
+	 */
+	float arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, const float max_speed,
+			   const float miss_spd_gain, int curr_wp_type);
+
+	// uORB subscriptions
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+
+	// uORB publications
+	uORB::Publication<rover_position_setpoint_s>    _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RO_SPEED_LIM>)     _param_ro_speed_limit,
+		(ParamFloat<px4::params::RM_MISS_SPD_GAIN>) _param_rm_miss_spd_gain
+	)
+};

src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/CMakeLists.txt

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

src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp

@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 "MecanumManualMode.hpp"
+
+using namespace time_literals;
+
+MecanumManualMode::MecanumManualMode(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_throttle_setpoint_pub.advertise();
+	_rover_steering_setpoint_pub.advertise();
+	_rover_rate_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
+	_rover_velocity_setpoint_pub.advertise();
+	_rover_position_setpoint_pub.advertise();
+}
+
+void MecanumManualMode::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
+}
+
+void MecanumManualMode::manual()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+	rover_steering_setpoint_s rover_steering_setpoint{};
+	rover_steering_setpoint.timestamp = hrt_absolute_time();
+	rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw;
+	_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
+	rover_throttle_setpoint_s rover_throttle_setpoint{};
+	rover_throttle_setpoint.timestamp = hrt_absolute_time();
+	rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
+	rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
+	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+}
+
+void MecanumManualMode::acro()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+	rover_throttle_setpoint_s rover_throttle_setpoint{};
+	rover_throttle_setpoint.timestamp = hrt_absolute_time();
+	rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
+	rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
+	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+	rover_rate_setpoint_s rover_rate_setpoint{};
+	rover_rate_setpoint.timestamp = hrt_absolute_time();
+	rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float> (manual_control_setpoint.yaw, -1.f, 1.f,
+						-_max_yaw_rate, _max_yaw_rate);
+	_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+}
+
+void MecanumManualMode::stab()
+{
+	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();
+	}
+
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+	rover_throttle_setpoint_s rover_throttle_setpoint{};
+	rover_throttle_setpoint.timestamp = hrt_absolute_time();
+	rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
+	rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
+	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+
+	if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON) {
+		_stab_yaw_setpoint = NAN;
+
+		// Rate control
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = hrt_absolute_time();
+		rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
+							_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+
+		// Set uncontrolled setpoint invalid
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = NAN;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+	} else { // Heading control
+		if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
+			_stab_yaw_setpoint = _vehicle_yaw;
+		}
+
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+	}
+}
+
+void MecanumManualMode::position()
+{
+	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();
+	}
+
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+	}
+
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	Vector3f velocity_setpoint_body{};
+	velocity_setpoint_body(0) = math::interpolate<float>(manual_control_setpoint.throttle,
+				    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
+	velocity_setpoint_body(1) = math::interpolate<float>(manual_control_setpoint.roll,
+				    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
+	velocity_setpoint_body(2) = 0.f;
+	const Vector3f velocity_setpoint_ned = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body);
+
+
+	if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON || velocity_setpoint_ned.norm() < FLT_EPSILON) {
+		_pos_ctl_yaw_setpoint = NAN;
+
+		// Speed control
+		rover_velocity_setpoint_s rover_velocity_setpoint{};
+		rover_velocity_setpoint.timestamp = hrt_absolute_time();
+		rover_velocity_setpoint.speed = velocity_setpoint_ned.norm();
+		rover_velocity_setpoint.bearing = atan2f(velocity_setpoint_ned(1), velocity_setpoint_ned(0));
+		rover_velocity_setpoint.yaw = NAN;
+		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+
+		// Rate control
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = hrt_absolute_time();
+		rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
+							_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+
+		// Set uncontrolled setpoints invalid
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = NAN;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = NAN;
+		rover_position_setpoint.position_ned[1] = NAN;
+		rover_position_setpoint.start_ned[0] = NAN;
+		rover_position_setpoint.start_ned[1] = NAN;
+		rover_position_setpoint.arrival_speed = NAN;
+		rover_position_setpoint.cruising_speed = NAN;
+		rover_position_setpoint.yaw = NAN;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
+
+	} else { // Course control
+		const Vector3f pos_ctl_course_direction_local = velocity_setpoint_ned.normalized();
+		const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0),
+				pos_ctl_course_direction_local(1));
+
+		// Reset course control if course direction change is above threshold
+		if (fabsf(acosf(pos_ctl_course_direction_temp(0) * _pos_ctl_course_direction(0) + pos_ctl_course_direction_temp(
+					1) * _pos_ctl_course_direction(1))) > _param_rm_course_ctl_th.get()) {
+			_pos_ctl_yaw_setpoint = NAN;
+		}
+
+		if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) {
+			_pos_ctl_start_position_ned = _curr_pos_ned;
+			_pos_ctl_yaw_setpoint = _vehicle_yaw;
+			_pos_ctl_course_direction = pos_ctl_course_direction_temp;
+		}
+
+		// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
+		const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
+		const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
+		const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
+		rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
+		rover_position_setpoint.start_ned[0] = NAN;
+		rover_position_setpoint.start_ned[1] = NAN;
+		rover_position_setpoint.arrival_speed = NAN;
+		rover_position_setpoint.cruising_speed = velocity_setpoint_ned.norm();
+		rover_position_setpoint.yaw = _pos_ctl_yaw_setpoint;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
+	}
+}
+
+void MecanumManualMode::reset()
+{
+	_stab_yaw_setpoint = NAN;
+	_pos_ctl_course_direction = Vector2f(NAN, NAN);
+	_pos_ctl_start_position_ned = Vector2f(NAN, NAN);
+	_curr_pos_ned = Vector2f(NAN, NAN);
+}

src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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>
+
+// Libraries
+#include <math.h>
+#include <matrix/matrix/math.hpp>
+
+// uORB includes
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/rover_throttle_setpoint.h>
+#include <uORB/topics/rover_steering_setpoint.h>
+#include <uORB/topics/rover_rate_setpoint.h>
+#include <uORB/topics/rover_attitude_setpoint.h>
+#include <uORB/topics/rover_velocity_setpoint.h>
+#include <uORB/topics/rover_position_setpoint.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for Mecanum manual mode.
+ */
+class MecanumManualMode : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for MecanumManualMode.
+	 * @param parent The parent ModuleParams object.
+	 */
+	MecanumManualMode(ModuleParams *parent);
+	~MecanumManualMode() = default;
+
+	/**
+	 * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
+	 */
+	void manual();
+
+	/**
+	 * @brief Generate and publish roverThrottleSetpoint/RoverRateSetpoint from manualControlSetpoint.
+	 */
+	void acro();
+
+	/**
+	 * @brief Generate and publish roverSetpoints from manualControlSetpoint.
+	 */
+	void stab();
+
+	/**
+	 * @brief Generate and publish roverSetpoints from manualControlSetpoint.
+	 */
+	void position();
+
+	/**
+	 * @brief Reset manual mode variables.
+	 */
+	void reset();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	// uORB subscriptions
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+
+	// uORB publications
+	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
+	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
+	uORB::Publication<rover_rate_setpoint_s>     _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
+	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
+	uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
+	uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
+
+	// Variables
+	Vector2f _pos_ctl_course_direction{NAN, NAN};
+	Vector2f _pos_ctl_start_position_ned{NAN, NAN};
+	Vector2f _curr_pos_ned{NAN, NAN};
+	Quatf _vehicle_attitude_quaternion;
+	float _pos_ctl_yaw_setpoint{NAN};
+	float _stab_yaw_setpoint{NAN};
+	float _vehicle_yaw{NAN};
+	float _max_yaw_rate{NAN};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
+		(ParamFloat<px4::params::RO_YAW_STICK_DZ>)  _param_ro_yaw_stick_dz,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max,
+		(ParamFloat<px4::params::RO_SPEED_LIM>)     _param_ro_speed_limit,
+		(ParamFloat<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th
+	)
+};

src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/CMakeLists.txt

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

src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp

@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 "MecanumOffboardMode.hpp"
+
+using namespace time_literals;
+
+MecanumOffboardMode::MecanumOffboardMode(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_rate_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
+	_rover_velocity_setpoint_pub.advertise();
+	_rover_position_setpoint_pub.advertise();
+}
+
+void MecanumOffboardMode::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void MecanumOffboardMode::offboardControl()
+{
+	offboard_control_mode_s offboard_control_mode{};
+	_offboard_control_mode_sub.copy(&offboard_control_mode);
+
+	trajectory_setpoint_s trajectory_setpoint{};
+	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
+
+	if (offboard_control_mode.position) {
+		rover_position_setpoint_s rover_position_setpoint{};
+		rover_position_setpoint.timestamp = hrt_absolute_time();
+		rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
+		rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
+		rover_position_setpoint.start_ned[0] = NAN;
+		rover_position_setpoint.start_ned[1] = NAN;
+		rover_position_setpoint.cruising_speed = NAN;
+		rover_position_setpoint.arrival_speed = NAN;
+		rover_position_setpoint.yaw = NAN;
+		_rover_position_setpoint_pub.publish(rover_position_setpoint);
+
+	} else if (offboard_control_mode.velocity) {
+		const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
+		rover_velocity_setpoint_s rover_velocity_setpoint{};
+		rover_velocity_setpoint.timestamp = hrt_absolute_time();
+		rover_velocity_setpoint.speed = velocity_ned.norm();
+		rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
+		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+
+	} else if (offboard_control_mode.attitude) {
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+	} else if (offboard_control_mode.body_rate) {
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = hrt_absolute_time();
+		rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+	}
+}

src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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>
+
+// Libraries
+#include <math.h>
+#include <matrix/matrix/math.hpp>
+
+// uORB includes
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/rover_rate_setpoint.h>
+#include <uORB/topics/rover_attitude_setpoint.h>
+#include <uORB/topics/rover_velocity_setpoint.h>
+#include <uORB/topics/rover_position_setpoint.h>
+#include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/trajectory_setpoint.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for Mecanum manual mode.
+ */
+class MecanumOffboardMode : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for MecanumOffboardMode.
+	 * @param parent The parent ModuleParams object.
+	 */
+	MecanumOffboardMode(ModuleParams *parent);
+	~MecanumOffboardMode() = default;
+
+	/**
+	 * @brief Generate and publish roverSetpoints from trajectorySetpoint.
+	 */
+	void offboardControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	// uORB subscriptions
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+
+	// uORB publications
+	uORB::Publication<rover_rate_setpoint_s>     _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
+	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
+	uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
+	uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
+};

src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp

@@ -38,7 +38,6 @@ using namespace time_literals;
 MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent)
 {
 	_rover_velocity_setpoint_pub.advertise();
-	_rover_position_setpoint_pub.advertise();
 	_pure_pursuit_status_pub.advertise();
 
 	updateParams();
@@ -55,29 +54,62 @@ void MecanumPosControl::updatePosControl()
 {
 	updateSubscriptions();
 
-	if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-		if (_vehicle_control_mode.flag_control_offboard_enabled) {
-			offboardPositionMode();
+	hrt_abstime timestamp = hrt_absolute_time();
 
-		} else if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) {
-			manualPositionMode();
+	if (_rover_position_setpoint_sub.updated()) {
+		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
+		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
+		_yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw;
+	}
 
-		} else if (_vehicle_control_mode.flag_control_auto_enabled) {
-			autoPositionMode();
+	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
 
+	if (target_waypoint_ned.isAllFinite()) {
+
+		float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
+
+		if (distance_to_target > _param_nav_acc_rad.get()) {
+			float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
+					      0.f;
+			const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() :
+					       distance_to_target;
+			float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
+					       _param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
+			speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
+
+			if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
+				speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
+						 fabsf(_rover_position_setpoint.cruising_speed));
+			}
+
+			pure_pursuit_status_s pure_pursuit_status{};
+			pure_pursuit_status.timestamp = timestamp;
+
+			const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
+						       _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
+						       _curr_pos_ned, fabsf(speed_setpoint));
+			_pure_pursuit_status_pub.publish(pure_pursuit_status);
+			rover_velocity_setpoint_s rover_velocity_setpoint{};
+			rover_velocity_setpoint.timestamp = timestamp;
+			rover_velocity_setpoint.speed = speed_setpoint;
+			rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
+					bearing_setpoint + M_PI_F);
+			rover_velocity_setpoint.yaw = _yaw_setpoint;
+			_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+
+		} else {
+			rover_velocity_setpoint_s rover_velocity_setpoint{};
+			rover_velocity_setpoint.timestamp = timestamp;
+			rover_velocity_setpoint.speed = 0.f;
+			rover_velocity_setpoint.bearing = _vehicle_yaw;
+			_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
 		}
-
-		generateVelocitySetpoint();
-
 	}
 }
 
 void MecanumPosControl::updateSubscriptions()
 {
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
-
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
 		_vehicle_attitude_sub.copy(&vehicle_attitude);
@@ -100,213 +132,6 @@ void MecanumPosControl::updateSubscriptions()
 
 }
 
-void MecanumPosControl::offboardPositionMode()
-{
-	if (_offboard_control_mode_sub.updated()) {
-		_offboard_control_mode_sub.copy(&_offboard_control_mode);
-	}
-
-	if (!_offboard_control_mode.position) {
-		return;
-	}
-
-	trajectory_setpoint_s trajectory_setpoint{};
-	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-	// Translate trajectory setpoint to rover position setpoint
-	rover_position_setpoint_s rover_position_setpoint{};
-	rover_position_setpoint.timestamp = hrt_absolute_time();
-	rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
-	rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
-	rover_position_setpoint.cruising_speed = _param_ro_speed_limit.get();
-	rover_position_setpoint.yaw = _vehicle_yaw;
-	_rover_position_setpoint_pub.publish(rover_position_setpoint);
-
-}
-
-void MecanumPosControl::generateVelocitySetpoint()
-{
-	hrt_abstime timestamp = hrt_absolute_time();
-
-	if (_rover_position_setpoint_sub.updated()) {
-		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
-		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
-		_yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw;
-	}
-
-	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
-	float distance_to_target = target_waypoint_ned.isAllFinite() ? (target_waypoint_ned - _curr_pos_ned).norm() : NAN;
-
-	if (PX4_ISFINITE(distance_to_target) && distance_to_target > _param_nav_acc_rad.get()) {
-
-		float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
-				      0.f;
-		const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() :
-				       distance_to_target;
-		float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-				       _param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
-		speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
-
-		if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
-			speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
-					 fabsf(_rover_position_setpoint.cruising_speed));
-		}
-
-		pure_pursuit_status_s pure_pursuit_status{};
-		pure_pursuit_status.timestamp = timestamp;
-
-		const float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
-					   _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
-					   _curr_pos_ned, fabsf(speed_setpoint));
-		_pure_pursuit_status_pub.publish(pure_pursuit_status);
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = timestamp;
-		rover_velocity_setpoint.speed = speed_setpoint;
-		rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(
-				yaw_setpoint + M_PI_F);
-		rover_velocity_setpoint.yaw = _yaw_setpoint;
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
-
-	} else {
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = timestamp;
-		rover_velocity_setpoint.speed = 0.f;
-		rover_velocity_setpoint.bearing = _vehicle_yaw;
-		rover_velocity_setpoint.yaw = _vehicle_yaw;
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
-	}
-
-}
-
-void MecanumPosControl::manualPositionMode()
-{
-	manual_control_setpoint_s manual_control_setpoint{};
-	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
-
-	Vector3f velocity_setpoint_body{};
-	velocity_setpoint_body(0) = math::interpolate<float>(manual_control_setpoint.throttle,
-				    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
-	velocity_setpoint_body(1) = math::interpolate<float>(manual_control_setpoint.roll,
-				    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
-	const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
-				_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
-				_max_yaw_rate / _param_ro_yaw_p.get());
-
-	if (fabsf(yaw_delta) > FLT_EPSILON || velocity_setpoint_body.norm() < FLT_EPSILON) { // Closed loop yaw rate control
-		_pos_ctl_yaw_setpoint = NAN;
-		// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
-		const Vector3f velocity = Vector3f(velocity_setpoint_body(0), velocity_setpoint_body(1), 0.f);
-		const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized());
-		const Vector2f pos_ctl_course_direction = Vector2f(pos_ctl_course_direction_local(0),
-				pos_ctl_course_direction_local(1));
-		const Vector2f target_waypoint_ned = _curr_pos_ned + _param_pp_lookahd_max.get() * pos_ctl_course_direction;
-
-		rover_position_setpoint_s rover_position_setpoint{};
-		rover_position_setpoint.timestamp = hrt_absolute_time();
-		rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
-		rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
-		rover_position_setpoint.start_ned[0] = NAN;
-		rover_position_setpoint.start_ned[1] = NAN;
-		rover_position_setpoint.arrival_speed = NAN;
-		rover_position_setpoint.cruising_speed = velocity_setpoint_body.norm();
-		rover_position_setpoint.yaw = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
-		_rover_position_setpoint_pub.publish(rover_position_setpoint);
-
-	} else { // Course control if the steering input is zero (keep driving on a straight line)
-		const Vector3f velocity = Vector3f(velocity_setpoint_body(0), velocity_setpoint_body(1), 0.f);
-		const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized());
-		const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0),
-				pos_ctl_course_direction_local(1));
-
-		// Reset course control if course direction change is above threshold
-		if (fabsf(acosf(pos_ctl_course_direction_temp(0) * _pos_ctl_course_direction(0) + pos_ctl_course_direction_temp(
-					1) * _pos_ctl_course_direction(1))) > _param_rm_course_ctl_th.get()) {
-			_pos_ctl_yaw_setpoint = NAN;
-		}
-
-		if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) {
-			_pos_ctl_start_position_ned = _curr_pos_ned;
-			_pos_ctl_yaw_setpoint = _vehicle_yaw;
-			_pos_ctl_course_direction = pos_ctl_course_direction_temp;
-		}
-
-		// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
-		const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
-		const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
-		const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
-		rover_position_setpoint_s rover_position_setpoint{};
-		rover_position_setpoint.timestamp = hrt_absolute_time();
-		rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
-		rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
-		rover_position_setpoint.start_ned[0] = NAN;
-		rover_position_setpoint.start_ned[1] = NAN;
-		rover_position_setpoint.arrival_speed = NAN;
-		rover_position_setpoint.cruising_speed = velocity.norm();
-		rover_position_setpoint.yaw = _pos_ctl_yaw_setpoint;
-		_rover_position_setpoint_pub.publish(rover_position_setpoint);
-	}
-}
-
-void MecanumPosControl::autoPositionMode()
-{
-	if (_position_setpoint_triplet_sub.updated()) {
-		position_setpoint_triplet_s position_setpoint_triplet{};
-		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
-		_curr_wp_type = position_setpoint_triplet.current.type;
-
-		RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
-				_curr_pos_ned, _global_ned_proj_ref);
-
-		_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
-
-		// Waypoint cruising speed
-		_auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
-				      position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
-
-		// Waypoint yaw setpoint
-		if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) {
-			_auto_yaw = position_setpoint_triplet.current.yaw;
-
-		} else {
-			_auto_yaw = _vehicle_yaw;
-		}
-
-		rover_position_setpoint_s rover_position_setpoint{};
-		rover_position_setpoint.timestamp = hrt_absolute_time();
-		rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
-		rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
-		rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
-		rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
-		rover_position_setpoint.arrival_speed = autoArrivalSpeed(_auto_speed, _waypoint_transition_angle,
-							_param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), _curr_wp_type);
-		rover_position_setpoint.cruising_speed = _auto_speed;
-		rover_position_setpoint.yaw = _auto_yaw;
-		_rover_position_setpoint_pub.publish(rover_position_setpoint);
-	}
-
-}
-
-
-float MecanumPosControl::autoArrivalSpeed(const float auto_speed, const float waypoint_transition_angle,
-		const float max_speed, const float miss_spd_gain, const int curr_wp_type)
-{
-	// Upcoming stop
-	if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
-	    || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
-		return 0.f;
-	}
-
-	// Straight line speed
-	if (miss_spd_gain > FLT_EPSILON) {
-		const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
-					      M_PI_F, 0.f, 1.f), 0.f, 1.f);
-		return  max_speed * (1.f - speed_reduction);
-	}
-
-	return auto_speed; // Fallthrough
-}
-
 bool MecanumPosControl::runSanityChecks()
 {
 	bool ret = true;
@@ -323,6 +148,5 @@ bool MecanumPosControl::runSanityChecks()
 		ret = false;
 	}
 
-	_prev_param_check_passed = ret;
 	return ret;
 }

src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp

@@ -39,9 +39,7 @@
 
 // Libraries
 #include <lib/rover_control/RoverControl.hpp>
-#include <lib/pid/PID.hpp>
 #include <matrix/matrix/math.hpp>
-#include <lib/slew_rate/SlewRate.hpp>
 #include <lib/pure_pursuit/PurePursuit.hpp>
 #include <lib/geo/geo.h>
 #include <math.h>
@@ -51,13 +49,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/rover_velocity_setpoint.h>
 #include <uORB/topics/rover_position_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/position_setpoint.h>
-#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/pure_pursuit_status.h>
 
@@ -77,10 +69,16 @@ public:
 	~MecanumPosControl() = default;
 
 	/**
-	 * @brief Update position controller.
+	 * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint.
 	 */
 	void updatePosControl();
 
+	/**
+	 * @brief Check if the necessary parameters are set.
+	 * @return True if all checks pass.
+	 */
+	bool runSanityChecks();
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -93,87 +91,23 @@ private:
 	 */
 	void updateSubscriptions();
 
-	/**
-	 * @brief Generate and publish roverPositionSetpoint from position of trajectorySetpoint.
-	 */
-	void offboardPositionMode();
-
-	/**
-	 * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint (Position Mode) or
-	 * 	  positionSetpointTriplet (Auto Mode) or roverPositionSetpoint.
-	 */
-	void generateVelocitySetpoint();
-
-	/**
-	 * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
-	 */
-	void manualPositionMode();
-
-	/**
-	 * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet.
-	 */
-	void autoPositionMode();
-
-	/**
-	 * @brief Calculate the speed at which the rover should arrive at the current waypoint. During waypoint transition the speed is restricted to
-	 * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
-	 * @param auto_speed Default auto speed [m/s].
-	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
-	 * @param max_speed Maximum velocity magnitude setpoint [m/s]
-	 * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
-	 * @param curr_wp_type Type of the current waypoint.
-	 * @return Velocity magnitude setpoint [m/s].
-	 */
-	float autoArrivalSpeed(float auto_speed, float waypoint_transition_angle, float max_speed, float miss_spd_gain,
-			       int curr_wp_type);
-
-
-	/**
-	 * @brief Check if the necessary parameters are set.
-	 * @return True if all checks pass.
-	 */
-	bool runSanityChecks();
-
 	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
 	uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)};
-	vehicle_control_mode_s    _vehicle_control_mode{};
-	offboard_control_mode_s   _offboard_control_mode{};
 	rover_position_setpoint_s _rover_position_setpoint{};
 
 	// uORB publications
 	uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
-	uORB::Publication<pure_pursuit_status_s>       _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
-	uORB::Publication<rover_position_setpoint_s>   _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
-
+	uORB::Publication<pure_pursuit_status_s>     _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
 
 	// Variables
 	Quatf _vehicle_attitude_quaternion{};
 	Vector2f _curr_pos_ned{};
-	Vector2f _pos_ctl_course_direction{};
-	Vector2f _pos_ctl_start_position_ned{};
 	Vector2f _start_ned{};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
-	float _pos_ctl_yaw_setpoint{0.f}; // Yaw setpoint for manual position mode, NAN if yaw rate is manually controlled [rad]
-	float _auto_speed{0.f};
-	float _auto_yaw{0.f};
 	float _yaw_setpoint{NAN};
-	int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
-	bool _prev_param_check_passed{true};
-
-	// Waypoint variables
-	Vector2f _curr_wp_ned{};
-	Vector2f _prev_wp_ned{};
-	Vector2f _next_wp_ned{};
-	float _cruising_speed{0.f};
-	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 
 	// Class Instances
 	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates

src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp

@@ -37,8 +37,6 @@ using namespace time_literals;
 
 MecanumRateControl::MecanumRateControl(ModuleParams *parent) : ModuleParams(parent)
 {
-	_rover_rate_setpoint_pub.advertise();
-	_rover_throttle_setpoint_pub.advertise();
 	_rover_steering_setpoint_pub.advertise();
 	_rover_rate_status_pub.advertise();
 	updateParams();
@@ -47,24 +45,21 @@ MecanumRateControl::MecanumRateControl(ModuleParams *parent) : ModuleParams(pare
 void MecanumRateControl::updateParams()
 {
 	ModuleParams::updateParams();
-	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
-	_max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F;
-	_max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F;
+
+	// Set up PID controller
 	_pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f);
 	_pid_yaw_rate.setIntegralLimit(1.f);
 	_pid_yaw_rate.setOutputLimit(1.f);
-	_adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel);
+
+	// Set up slew rate
+	_adjusted_yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F);
 }
 
 void MecanumRateControl::updateRateControl()
 {
-	const hrt_abstime timestamp_prev = _timestamp;
+	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
 	if (_vehicle_angular_velocity_sub.updated()) {
 		vehicle_angular_velocity_s vehicle_angular_velocity{};
@@ -73,16 +68,25 @@ void MecanumRateControl::updateRateControl()
 				    vehicle_angular_velocity.xyz[2] : 0.f;
 	}
 
-	if (_vehicle_control_mode.flag_control_rates_enabled  && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-		if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
-			generateRateAndThrottleSetpoint();
-		}
+	if (_rover_rate_setpoint_sub.updated()) {
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		_rover_rate_setpoint_sub.copy(&rover_rate_setpoint);
+		_yaw_rate_setpoint = rover_rate_setpoint.yaw_rate_setpoint;
+	}
 
-		generateSteeringSetpoint();
+	if (PX4_ISFINITE(_yaw_rate_setpoint)) {
+		const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ?
+						_yaw_rate_setpoint : 0.f;
+		const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate,
+						    yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F,
+						    _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt);
+		rover_steering_setpoint_s rover_steering_setpoint{};
+		rover_steering_setpoint.timestamp = _timestamp;
+		rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized;
+		_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
 
-	} else { // Reset controller and slew rate when rate control is not active
+	} else {
 		_pid_yaw_rate.resetIntegral();
-		_adjusted_yaw_rate_setpoint.setForcedValue(0.f);
 	}
 
 	// Publish rate controller status (logging only)
@@ -95,96 +99,25 @@ void MecanumRateControl::updateRateControl()
 
 }
 
-void MecanumRateControl::generateRateAndThrottleSetpoint()
-{
-	const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-				       && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled;
-
-	if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode
-		manual_control_setpoint_s manual_control_setpoint{};
-
-		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-			rover_throttle_setpoint_s rover_throttle_setpoint{};
-			rover_throttle_setpoint.timestamp = _timestamp;
-			rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
-			rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
-			_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-			rover_rate_setpoint_s rover_rate_setpoint{};
-			rover_rate_setpoint.timestamp = _timestamp;
-			rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float> (manual_control_setpoint.yaw, -1.f, 1.f,
-								-_max_yaw_rate, _max_yaw_rate);
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-		}
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control
-		trajectory_setpoint_s trajectory_setpoint{};
-		_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.attitude;
-
-		if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) {
-			rover_rate_setpoint_s rover_rate_setpoint{};
-			rover_rate_setpoint.timestamp = _timestamp;
-			rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed;
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-		}
-	}
-}
-
-void MecanumRateControl::generateSteeringSetpoint()
-{
-	if (_rover_rate_setpoint_sub.updated()) {
-		_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
-
-	}
-
-	float speed_diff_normalized{0.f};
-
-	if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) {
-		const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() *
-						M_DEG_TO_RAD_F ?
-						_rover_rate_setpoint.yaw_rate_setpoint : 0.f;
-		speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate,
-					yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _max_yaw_accel,
-					_max_yaw_decel, _param_rm_wheel_track.get(), _dt);
-	}
-
-	rover_steering_setpoint_s rover_steering_setpoint{};
-	rover_steering_setpoint.timestamp = _timestamp;
-	rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized;
-	_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
-}
-
 bool MecanumRateControl::runSanityChecks()
 {
 	bool ret = true;
 
 	if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
 		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("mecanum_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get());
-		}
+		events::send<float>(events::ID("mecanum_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error,
+				    "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get());
 
 	}
 
 	if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_rm_max_thr_yaw_r.get() < FLT_EPSILON)
 	    && _param_ro_yaw_rate_p.get() < FLT_EPSILON) {
 		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float, float, float>(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error,
-							  "Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup",
-							  _param_rm_wheel_track.get(),
-							  _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get());
-		}
+		events::send<float, float, float>(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error,
+						  "Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup",
+						  _param_rm_wheel_track.get(),
+						  _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get());
 	}
 
-	_prev_param_check_passed = ret;
 	return ret;
 }

src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp

@@ -47,15 +47,9 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/rover_rate_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/rover_steering_setpoint.h>
 #include <uORB/topics/rover_rate_status.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/trajectory_setpoint.h>
 
 /**
  * @brief Class for mecanum rate control.
@@ -71,10 +65,21 @@ public:
 	~MecanumRateControl() = default;
 
 	/**
-	 * @brief Update rate controller.
+	 * @brief Generate and publish roverSteeringSetpoint from roverRateSetpoint.
 	 */
 	void updateRateControl();
 
+	/**
+	 * @brief Check if the necessary parameters are set.
+	 * @return True if all checks pass.
+	 */
+	bool runSanityChecks();
+
+	/**
+	 * @brief Reset rate controller.
+	 */
+	void reset() {_pid_yaw_rate.resetIntegral(); _yaw_rate_setpoint = NAN;};
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -83,48 +88,18 @@ protected:
 
 private:
 
-	/**
-	 * @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode).
-	 */
-	void generateRateAndThrottleSetpoint();
-
-	/**
-	 * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.
-	 */
-	void generateSteeringSetpoint();
-
-	/**
-	 * @brief Check if the necessary parameters are set.
-	 * @return True if all checks pass.
-	 */
-	bool runSanityChecks();
-
 	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
-	vehicle_control_mode_s  _vehicle_control_mode{};
-	offboard_control_mode_s _offboard_control_mode{};
-	rover_rate_setpoint_s   _rover_rate_setpoint{};
 
 	// uORB publications
-	uORB::Publication<rover_rate_setpoint_s>     _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
 	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
 	uORB::Publication<rover_rate_status_s>       _rover_rate_status_pub{ORB_ID(rover_rate_status)};
 
 	// Variables
 	hrt_abstime _timestamp{0};
-	float _max_yaw_rate{0.f};
-	float _max_yaw_accel{0.f};
-	float _max_yaw_decel{0.f};
 	float _vehicle_yaw_rate{0.f};
-	float _dt{0.f}; // Time since last update [s].
-	bool _prev_param_check_passed{true};
+	float _yaw_rate_setpoint{NAN};
 
 	// Controllers
 	PID _pid_yaw_rate;

src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp

@@ -39,7 +39,6 @@ MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent
 {
 	_rover_throttle_setpoint_pub.advertise();
 	_rover_attitude_setpoint_pub.advertise();
-	_rover_velocity_setpoint_pub.advertise();
 	_rover_velocity_status_pub.advertise();
 	updateParams();
 }
@@ -47,6 +46,8 @@ MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent
 void MecanumVelControl::updateParams()
 {
 	ModuleParams::updateParams();
+
+	// Set up PID controllers
 	_pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
 	_pid_speed_x.setIntegralLimit(1.f);
 	_pid_speed_x.setOutputLimit(1.f);
@@ -54,9 +55,10 @@ void MecanumVelControl::updateParams()
 	_pid_speed_y.setIntegralLimit(1.f);
 	_pid_speed_y.setOutputLimit(1.f);
 
+	// Set up slew rates
 	if (_param_ro_accel_limit.get() > FLT_EPSILON) {
-		_speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get());
-		_speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get());
+		_adjusted_speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get());
+		_adjusted_speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get());
 	}
 }
 
@@ -64,31 +66,42 @@ void MecanumVelControl::updateVelControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
 	updateSubscriptions();
 
-	if ((_vehicle_control_mode.flag_control_velocity_enabled) && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-		if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Velocity Control
-			generateVelocitySetpoint();
-		}
+	// Attitude Setpoint
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = _timestamp;
+		rover_attitude_setpoint.yaw_setpoint = _yaw_setpoint;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 
-		generateAttitudeAndThrottleSetpoint();
+	}
+
+	// Throttle Setpoints
+	if (PX4_ISFINITE(_speed_x_setpoint) && PX4_ISFINITE(_speed_y_setpoint)) {
+		Vector2f speed_setpoint = calcSpeedSetpoint();
+		rover_throttle_setpoint_s rover_throttle_setpoint{};
+		rover_throttle_setpoint.timestamp = _timestamp;
+
+		rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_x_setpoint, _pid_speed_x,
+				speed_setpoint(0), _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
+				_param_ro_max_thr_speed.get(), dt);
+		rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_adjusted_speed_y_setpoint, _pid_speed_y,
+				speed_setpoint(1), _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
+				_param_ro_max_thr_speed.get(), dt);
+		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
 
-	} else { // Reset controller and slew rate when velocity control is not active
-		_pid_speed_x.resetIntegral();
-		_speed_x_setpoint.setForcedValue(0.f);
-		_pid_speed_y.resetIntegral();
-		_speed_y_setpoint.setForcedValue(0.f);
 	}
 
 	// Publish position controller status (logging only)
 	rover_velocity_status_s rover_velocity_status;
 	rover_velocity_status.timestamp = _timestamp;
 	rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x;
-	rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_x_setpoint.getState();
+	rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_x_setpoint.getState();
 	rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
-	rover_velocity_status.adjusted_speed_body_y_setpoint = _speed_y_setpoint.getState();
+	rover_velocity_status.adjusted_speed_body_y_setpoint = _adjusted_speed_y_setpoint.getState();
 	rover_velocity_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral();
 	rover_velocity_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral();
 	_rover_velocity_status_pub.publish(rover_velocity_status);
@@ -96,10 +109,6 @@ void MecanumVelControl::updateVelControl()
 
 void MecanumVelControl::updateSubscriptions()
 {
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
-
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
 		_vehicle_attitude_sub.copy(&vehicle_attitude);
@@ -116,110 +125,66 @@ void MecanumVelControl::updateSubscriptions()
 		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
 	}
 
-}
-void MecanumVelControl::generateVelocitySetpoint()
-{
-	trajectory_setpoint_s trajectory_setpoint{};
-	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-	if (_offboard_control_mode_sub.updated()) {
-		_offboard_control_mode_sub.copy(&_offboard_control_mode);
-	}
-
-	const bool offboard_vel_control = _offboard_control_mode.velocity && !_offboard_control_mode.position;
-
-	const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
-
-	if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) {
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = _timestamp;
-		rover_velocity_setpoint.speed = velocity_in_local_frame.norm();
-		rover_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
-		rover_velocity_setpoint.yaw = _vehicle_yaw;
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
-
-	}
-}
-
-void MecanumVelControl::generateAttitudeAndThrottleSetpoint()
-{
 	if (_rover_velocity_setpoint_sub.updated()) {
-		_rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint);
-	}
+		rover_velocity_setpoint_s rover_velocity_setpoint;
+		_rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint);
 
-	// Attitude Setpoint
-	if (PX4_ISFINITE(_rover_velocity_setpoint.yaw)) {
-		rover_attitude_setpoint_s rover_attitude_setpoint{};
-		rover_attitude_setpoint.timestamp = _timestamp;
-		rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.yaw;
-		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-		_last_attitude_setpoint_update = _timestamp;
+		if (PX4_ISFINITE(rover_velocity_setpoint.speed) && PX4_ISFINITE(rover_velocity_setpoint.bearing)) {
+			const Vector3f velocity_in_local_frame(rover_velocity_setpoint.speed * cosf(rover_velocity_setpoint.bearing),
+							       rover_velocity_setpoint.speed * sinf(rover_velocity_setpoint.bearing), 0.f);
+			const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+			_speed_x_setpoint = velocity_in_body_frame(0);
+			_speed_y_setpoint = velocity_in_body_frame(1);
 
-	} else {
-		rover_attitude_setpoint_s rover_attitude_setpoint{};
-		rover_attitude_setpoint.timestamp = _timestamp;
-		rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
-		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-		_last_attitude_setpoint_update = _timestamp;
-	}
+		} else if (PX4_ISFINITE(rover_velocity_setpoint.speed)) {
+			_speed_x_setpoint = rover_velocity_setpoint.speed;
+			_speed_y_setpoint = 0.f;
 
-	// Throttle Setpoint
-	float speed_body_x_setpoint{0.f};
-	float speed_body_y_setpoint{0.f};
-
-	if (fabsf(_rover_velocity_setpoint.speed) > FLT_EPSILON && PX4_ISFINITE(_rover_velocity_setpoint.bearing)) {
-		const Vector3f velocity_in_local_frame(_rover_velocity_setpoint.speed * cosf(
-				_rover_velocity_setpoint.bearing),
-						       _rover_velocity_setpoint.speed * sinf(_rover_velocity_setpoint.bearing), 0.f);
-		const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-		speed_body_x_setpoint = velocity_in_body_frame(0);
-		speed_body_y_setpoint = velocity_in_body_frame(1);
-
-	} else {
-		speed_body_x_setpoint = _rover_velocity_setpoint.speed;
-		speed_body_y_setpoint = 0.f;
-	}
-
-	if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible
-		if (_rover_steering_setpoint_sub.updated()) {
-			_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
+		} else {
+			_speed_x_setpoint = NAN;
+			_speed_y_setpoint = NAN;
 		}
 
-		float speed_body_x_setpoint_normalized = math::interpolate<float>(speed_body_x_setpoint,
-				-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
+		_yaw_setpoint = rover_velocity_setpoint.yaw;
+	}
+}
 
-		float speed_body_y_setpoint_normalized = math::interpolate<float>(speed_body_y_setpoint,
-				-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
-
-		const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf(
-						  _rover_steering_setpoint.normalized_speed_diff);
-
-		if (total_speed > 1.f) {
-			const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized));
-			const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta));
-			const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized,
-							   2.f) + powf(speed_body_y_setpoint_normalized, 2.f)));
-			speed_body_x_setpoint_normalized *= magnitude * normalization;
-			speed_body_y_setpoint_normalized *= magnitude * normalization;
-			speed_body_x_setpoint = math::interpolate<float>(speed_body_x_setpoint_normalized, -1.f, 1.f,
-						-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
-			speed_body_y_setpoint = math::interpolate<float>(speed_body_y_setpoint_normalized, -1.f, 1.f,
-						-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
-		}
+Vector2f MecanumVelControl::calcSpeedSetpoint()
+{
+	if (_rover_steering_setpoint_sub.updated()) {
+		rover_steering_setpoint_s rover_steering_setpoint{};
+		_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
+		_normalized_speed_diff = rover_steering_setpoint.normalized_speed_diff;
 	}
 
-	rover_throttle_setpoint_s rover_throttle_setpoint{};
-	rover_throttle_setpoint.timestamp = _timestamp;
-	speed_body_x_setpoint = fabsf(speed_body_x_setpoint) > _param_ro_speed_th.get() ? speed_body_x_setpoint : 0.f;
-	speed_body_y_setpoint = fabsf(speed_body_y_setpoint) > _param_ro_speed_th.get() ? speed_body_y_setpoint : 0.f;
-	rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x,
-			speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
-			_param_ro_max_thr_speed.get(), _dt);
-	rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y,
-			speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
-			_param_ro_max_thr_speed.get(), _dt);
-	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+	float speed_x_setpoint_normalized = math::interpolate<float>(_speed_x_setpoint,
+					    -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
 
+	float speed_y_setpoint_normalized = math::interpolate<float>(_speed_y_setpoint,
+					    -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
+
+	const float total_speed = fabsf(speed_x_setpoint_normalized) + fabsf(speed_y_setpoint_normalized) + fabsf(
+					  _normalized_speed_diff);
+
+	Vector2f speed_setpoint = Vector2f(_speed_x_setpoint, _speed_y_setpoint);
+
+	if (total_speed > 1.f) {
+		const float theta = atan2f(fabsf(speed_y_setpoint_normalized), fabsf(speed_x_setpoint_normalized));
+		const float magnitude = (1.f - fabsf(_normalized_speed_diff)) / (sinf(theta) + cosf(theta));
+		const float normalization = 1.f / (sqrtf(powf(speed_x_setpoint_normalized,
+						   2.f) + powf(speed_y_setpoint_normalized, 2.f)));
+		speed_x_setpoint_normalized *= magnitude * normalization;
+		speed_y_setpoint_normalized *= magnitude * normalization;
+		speed_setpoint(0) = math::interpolate<float>(speed_x_setpoint_normalized, -1.f, 1.f,
+				    -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
+		speed_setpoint(1) = math::interpolate<float>(speed_y_setpoint_normalized, -1.f, 1.f,
+				    -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
+	}
+
+	speed_setpoint(0) = fabsf(speed_setpoint(0)) > _param_ro_speed_th.get() ? speed_setpoint(0) : 0.f;
+	speed_setpoint(1) = fabsf(speed_setpoint(1)) > _param_ro_speed_th.get() ? speed_setpoint(1) : 0.f;
+
+	return speed_setpoint;
 }
 
 bool MecanumVelControl::runSanityChecks()
@@ -228,25 +193,18 @@ bool MecanumVelControl::runSanityChecks()
 
 	if (_param_ro_speed_limit.get() < FLT_EPSILON) {
 		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("mecanum_vel_control_conf_invalid_speed_lim"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get());
-		}
+		events::send<float>(events::ID("mecanum_vel_control_conf_invalid_speed_lim"), events::Log::Error,
+				    "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get());
 
 	}
 
 	if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) {
 		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float, float>(events::ID("mecanum_vel_control_conf_invalid_speed_control"), events::Log::Error,
-						   "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup",
-						   _param_ro_max_thr_speed.get(),
-						   _param_ro_speed_p.get());
-		}
+		events::send<float, float>(events::ID("mecanum_vel_control_conf_invalid_speed_control"), events::Log::Error,
+					   "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup",
+					   _param_ro_max_thr_speed.get(),
+					   _param_ro_speed_p.get());
 	}
 
-	_prev_param_check_passed = ret;
 	return ret;
 }

src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp

@@ -52,10 +52,7 @@
 #include <uORB/topics/rover_velocity_status.h>
 #include <uORB/topics/rover_velocity_setpoint.h>
 #include <uORB/topics/rover_attitude_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/vehicle_local_position.h>
 
 using namespace matrix;
@@ -74,10 +71,21 @@ public:
 	~MecanumVelControl() = default;
 
 	/**
-	 * @brief Update velocity controller.
+	 * @brief Generate and publish roverAttitudeSetpoint and RoverThrottleSetpoint from roverVelocitySetpoint.
 	 */
 	void updateVelControl();
 
+	/**
+	 * @brief Check if the necessary parameters are set.
+	 * @return True if all checks pass.
+	 */
+	bool runSanityChecks();
+
+	/**
+	 * @brief Reset velocity controller.
+	 */
+	void reset() {_pid_speed_x.resetIntegral(); _pid_speed_y.resetIntegral(); _speed_x_setpoint = NAN; _speed_y_setpoint = NAN; _adjusted_speed_x_setpoint.setForcedValue(0.f); _adjusted_speed_y_setpoint.setForcedValue(0.f); _yaw_setpoint = NAN;};
+
 protected:
 	/**
 	 * @brief Update the parameters of the module.
@@ -91,57 +99,38 @@ private:
 	void updateSubscriptions();
 
 	/**
-	 * @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint.
+	 * @brief Adjust the speed setpoint if they are infeasible
+	 * @return Speed setpoints
 	 */
-	void generateVelocitySetpoint();
-
-	/**
-	 * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint
-	 *        from roverVelocitySetpoint.
-	 */
-	void generateAttitudeAndThrottleSetpoint();
-
-	/**
-	 * @brief Check if the necessary parameters are set.
-	 * @return True if all checks pass.
-	 */
-	bool runSanityChecks();
+	Vector2f calcSpeedSetpoint();
 
 	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)};
-	uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
 	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
-	vehicle_control_mode_s _vehicle_control_mode{};
-	offboard_control_mode_s _offboard_control_mode{};
-	rover_steering_setpoint_s _rover_steering_setpoint{};
 
 	// uORB publications
 	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
 	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
-	uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
-	rover_velocity_setpoint_s _rover_velocity_setpoint{};
+	uORB::Publication<rover_velocity_status_s>   _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
 
 	// Variables
 	hrt_abstime _timestamp{0};
-	hrt_abstime _last_attitude_setpoint_update{0};
 	Quatf _vehicle_attitude_quaternion{};
 	float _vehicle_speed_body_x{0.f};
 	float _vehicle_speed_body_y{0.f};
 	float _vehicle_yaw{0.f};
-	float _dt{0.f};
-	bool _prev_param_check_passed{false};
+	float _speed_x_setpoint{NAN};
+	float _speed_y_setpoint{NAN};
+	float _normalized_speed_diff{NAN};
+	float _yaw_setpoint{NAN};
 
 	// Controllers
 	PID _pid_speed_x;
 	PID _pid_speed_y;
-	SlewRate<float> _speed_x_setpoint;
-	SlewRate<float> _speed_y_setpoint;
+	SlewRate<float> _adjusted_speed_x_setpoint;
+	SlewRate<float> _adjusted_speed_y_setpoint;
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,

src/modules/rover_mecanum/RoverMecanum.cpp

@@ -56,31 +56,130 @@ void RoverMecanum::updateParams()
 void RoverMecanum::Run()
 {
 	if (_parameter_update_sub.updated()) {
+		parameter_update_s param_update{};
+		_parameter_update_sub.copy(&param_update);
 		updateParams();
+		runSanityChecks();
 	}
 
-	_mecanum_pos_control.updatePosControl();
-	_mecanum_vel_control.updateVelControl();
-	_mecanum_att_control.updateAttControl();
-	_mecanum_rate_control.updateRateControl();
-
 	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
+		vehicle_control_mode_s vehicle_control_mode{};
+		_vehicle_control_mode_sub.copy(&vehicle_control_mode);
+
+		// Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated at 2 Hz)
+		if (_vehicle_control_mode.flag_control_manual_enabled != vehicle_control_mode.flag_control_manual_enabled ||
+		    _vehicle_control_mode.flag_control_auto_enabled != vehicle_control_mode.flag_control_auto_enabled ||
+		    _vehicle_control_mode.flag_control_offboard_enabled != vehicle_control_mode.flag_control_offboard_enabled ||
+		    _vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled ||
+		    _vehicle_control_mode.flag_control_velocity_enabled != vehicle_control_mode.flag_control_velocity_enabled ||
+		    _vehicle_control_mode.flag_control_attitude_enabled != vehicle_control_mode.flag_control_attitude_enabled ||
+		    _vehicle_control_mode.flag_control_rates_enabled != vehicle_control_mode.flag_control_rates_enabled ||
+		    _vehicle_control_mode.flag_control_allocation_enabled != vehicle_control_mode.flag_control_allocation_enabled) {
+			_vehicle_control_mode = vehicle_control_mode;
+			runSanityChecks();
+			reset();
+
+		} else {
+			_vehicle_control_mode = vehicle_control_mode;
+		}
+
 	}
 
-	const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-					      && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled
-					      && !_vehicle_control_mode.flag_control_rates_enabled;
+	if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) {
 
-	if (full_manual_mode_enabled) { // Manual mode
-		_mecanum_act_control.manualManualMode();
+		_was_armed = true;
+
+		// Generate setpoints
+		if (_vehicle_control_mode.flag_control_manual_enabled) {
+			manualControl();
+
+		} else if (_vehicle_control_mode.flag_control_auto_enabled) {
+			_auto_mode.autoControl();
+
+		} else if (_vehicle_control_mode.flag_control_offboard_enabled) {
+			_offboard_mode.offboardControl();
+		}
+
+		updateControllers();
+
+	} else if (_was_armed) { // Reset all controllers and stop the vehicle
+		reset();
+		_mecanum_act_control.stopVehicle();
+		_was_armed = false;
 	}
 
-	if (_vehicle_control_mode.flag_armed) {
+}
+
+void RoverMecanum::manualControl()
+{
+	if (_vehicle_control_mode.flag_control_position_enabled) {
+		_manual_mode.position();
+
+	} else if (_vehicle_control_mode.flag_control_attitude_enabled) {
+		_manual_mode.stab();
+
+	} else if (_vehicle_control_mode.flag_control_rates_enabled) {
+		_manual_mode.acro();
+
+	} else if (_vehicle_control_mode.flag_control_allocation_enabled) {
+		_manual_mode.manual();
+	}
+}
+
+void RoverMecanum::updateControllers()
+{
+	if (_vehicle_control_mode.flag_control_position_enabled) {
+		_mecanum_pos_control.updatePosControl();
+	}
+
+	if (_vehicle_control_mode.flag_control_velocity_enabled) {
+		_mecanum_vel_control.updateVelControl();
+	}
+
+	if (_vehicle_control_mode.flag_control_attitude_enabled) {
+		_mecanum_att_control.updateAttControl();
+	}
+
+	if (_vehicle_control_mode.flag_control_rates_enabled) {
+		_mecanum_rate_control.updateRateControl();
+	}
+
+	if (_vehicle_control_mode.flag_control_allocation_enabled) {
 		_mecanum_act_control.updateActControl();
+	}
+}
 
+void RoverMecanum::runSanityChecks()
+{
+	if (_vehicle_control_mode.flag_control_rates_enabled && !_mecanum_rate_control.runSanityChecks()) {
+		_sanity_checks_passed = false;
+		return;
 	}
 
+	if (_vehicle_control_mode.flag_control_attitude_enabled && !_mecanum_att_control.runSanityChecks()) {
+		_sanity_checks_passed = false;
+		return;
+	}
+
+	if (_vehicle_control_mode.flag_control_velocity_enabled && !_mecanum_vel_control.runSanityChecks()) {
+		_sanity_checks_passed = false;
+		return;
+	}
+
+	if (_vehicle_control_mode.flag_control_position_enabled && !_mecanum_pos_control.runSanityChecks()) {
+		_sanity_checks_passed = false;
+		return;
+	}
+
+	_sanity_checks_passed = true;
+}
+
+void RoverMecanum::reset()
+{
+	_mecanum_vel_control.reset();
+	_mecanum_att_control.reset();
+	_mecanum_rate_control.reset();
+	_manual_mode.reset();
 }
 
 int RoverMecanum::task_spawn(int argc, char *argv[])

src/modules/rover_mecanum/RoverMecanum.hpp

@@ -40,20 +40,13 @@
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
-// Libraries
-#include <lib/rover_control/RoverControl.hpp>
-#include <lib/slew_rate/SlewRate.hpp>
+// Library includes
+#include <math.h>
 
 // uORB includes
 #include <uORB/Subscription.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/rover_steering_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
 
 // Local includes
 #include "MecanumActControl/MecanumActControl.hpp"
@@ -61,6 +54,9 @@
 #include "MecanumAttControl/MecanumAttControl.hpp"
 #include "MecanumVelControl/MecanumVelControl.hpp"
 #include "MecanumPosControl/MecanumPosControl.hpp"
+#include "MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp"
+#include "MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp"
+#include "MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp"
 
 class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
 	public px4::ScheduledWorkItem
@@ -92,15 +88,46 @@ protected:
 private:
 	void Run() override;
 
+	/**
+	 * @brief Handle manual control
+	 */
+	void manualControl();
+
+	/**
+	 * @brief Update the controllers
+	 */
+	void updateControllers();
+
+	/**
+	 * @brief Check proper parameter setup for the controllers
+	 *
+	 * Modifies:
+	 *
+	 *   - _sanity_checks_passed: true if checks for all active controllers pass
+	 */
+	void runSanityChecks();
+
+	/**
+	 * @brief Reset controllers and manual mode variables.
+	 */
+	void reset();
+
 	// uORB subscriptions
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	vehicle_control_mode_s    _vehicle_control_mode{};
+	vehicle_control_mode_s _vehicle_control_mode{};
 
 	// Class instances
-	MecanumActControl _mecanum_act_control{this};
-	MecanumRateControl _mecanum_rate_control{this};
-	MecanumAttControl  _mecanum_att_control{this};
-	MecanumVelControl  _mecanum_vel_control{this};
-	MecanumPosControl  _mecanum_pos_control{this};
+	MecanumActControl   _mecanum_act_control{this};
+	MecanumRateControl  _mecanum_rate_control{this};
+	MecanumAttControl   _mecanum_att_control{this};
+	MecanumVelControl   _mecanum_vel_control{this};
+	MecanumPosControl   _mecanum_pos_control{this};
+	MecanumAutoMode	    _auto_mode{this};
+	MecanumManualMode   _manual_mode{this};
+	MecanumOffboardMode _offboard_mode{this};
+
+	// Variables
+	bool _sanity_checks_passed{true}; // True if checks for all active controllers pass
+	bool _was_armed{false}; // True if the vehicle was armed before the last reset
 };