differential: seperate velocity control

msg/CMakeLists.txt

@@ -64,6 +64,7 @@ set(msg_files
 	DebugValue.msg
 	DebugVect.msg
 	DifferentialPressure.msg
+	DifferentialVelocitySetpoint.msg
 	DistanceSensor.msg
 	DistanceSensorModeChangeRequest.msg
 	Ekf2Timestamps.msg

msg/DifferentialVelocitySetpoint.msg

@@ -0,0 +1,5 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32[2] velocity_ned # 2-dimensional velocity setpoint in NED frame
+bool backwards	        # Flag for backwards driving
+float32 yaw		# [rad] [-pi,pi] from North. Optional, NAN if not set. (Exclusive with velocity_ned, the rover will turn on the spot to reach the yaw setpoint)

src/modules/logger/logged_topics.cpp

@@ -58,6 +58,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("commander_state");
 	add_topic("config_overrides");
 	add_topic("cpuload");
+	add_optional_topic("differential_velocity_setpoint", 100);
 	add_topic("distance_sensor_mode_change_request");
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");

src/modules/rover_differential/CMakeLists.txt

@@ -33,7 +33,8 @@
 
 add_subdirectory(DifferentialRateControl)
 add_subdirectory(DifferentialAttControl)
-add_subdirectory(DifferentialPosVelControl)
+add_subdirectory(DifferentialVelControl)
+add_subdirectory(DifferentialPosControl)
 
 px4_add_module(
 	MODULE modules__rover_differential
@@ -44,7 +45,8 @@ px4_add_module(
 	DEPENDS
 		DifferentialRateControl
 		DifferentialAttControl
-		DifferentialPosVelControl
+		DifferentialVelControl
+		DifferentialPosControl
 		px4_work_queue
 		rover_control
 		pure_pursuit

src/modules/rover_differential/DifferentialPosControl/CMakeLists.txt

@@ -31,9 +31,8 @@
 #
 ############################################################################
 
-px4_add_library(DifferentialPosVelControl
-	DifferentialPosVelControl.cpp
+px4_add_library(DifferentialPosControl
+	DifferentialPosControl.cpp
 )
 
-target_link_libraries(DifferentialPosVelControl PUBLIC PID)
-target_include_directories(DifferentialPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_include_directories(DifferentialPosControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp

@@ -0,0 +1,390 @@
+/****************************************************************************
+ *
+ *   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 "DifferentialPosControl.hpp"
+
+using namespace time_literals;
+
+DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_differential_velocity_setpoint_pub.advertise();
+	_rover_position_setpoint_pub.advertise();
+	_pure_pursuit_status_pub.advertise();
+
+	// Initially set to NaN to indicate that the rover has no position setpoint
+	_rover_position_setpoint.position_ned[0] = NAN;
+	_rover_position_setpoint.position_ned[1] = NAN;
+
+	updateParams();
+}
+
+void DifferentialPosControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
+}
+
+void DifferentialPosControl::updatePosControl()
+{
+	const hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	updateSubscriptions();
+
+	if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
+		if (_vehicle_control_mode.flag_control_offboard_enabled) {
+			generatePositionSetpoint();
+		}
+
+		generateVelocitySetpoint();
+
+	}
+
+}
+
+void DifferentialPosControl::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);
+		_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);
+
+		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);
+		}
+
+		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+		const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
+	}
+
+}
+
+void DifferentialPosControl::generatePositionSetpoint()
+{
+	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 = _timestamp;
+	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 = NAN;
+	_rover_position_setpoint_pub.publish(rover_position_setpoint);
+
+}
+
+void DifferentialPosControl::generateVelocitySetpoint()
+{
+	// Manual Position Mode
+	if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) {
+		manualPositionMode();
+		return;
+	}
+
+	// Auto Mode
+	if (_vehicle_control_mode.flag_control_auto_enabled) {
+		autoPositionMode();
+		return;
+	}
+
+	// Rover Position Setpoint
+	if (_rover_position_setpoint_sub.copy(&_rover_position_setpoint)
+	    && PX4_ISFINITE(_rover_position_setpoint.position_ned[0]) && PX4_ISFINITE(_rover_position_setpoint.position_ned[1])) {
+		goToPositionMode();
+		return;
+	}
+
+}
+
+void DifferentialPosControl::manualPositionMode()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	const float speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+					    -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
+	const float bearing_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+				    _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(speed_body_x_setpoint) < FLT_EPSILON) { // Turn on spot
+		_course_control = false;
+		const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta);
+		differential_velocity_setpoint_s differential_velocity_setpoint{};
+		differential_velocity_setpoint.timestamp = _timestamp;
+		differential_velocity_setpoint.velocity_ned[0] = 0.f;
+		differential_velocity_setpoint.velocity_ned[1] = 0.f;
+		differential_velocity_setpoint.backwards = false;
+		differential_velocity_setpoint.yaw = yaw_setpoint;
+		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+
+	} else if (fabsf(bearing_delta) > FLT_EPSILON) { // Closed loop yaw rate control
+		_course_control = false;
+		const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta);
+		differential_velocity_setpoint_s differential_velocity_setpoint{};
+		differential_velocity_setpoint.timestamp = _timestamp;
+		differential_velocity_setpoint.velocity_ned[0] = speed_body_x_setpoint * cosf(bearing_setpoint);
+		differential_velocity_setpoint.velocity_ned[1] = speed_body_x_setpoint * sinf(bearing_setpoint);
+		differential_velocity_setpoint.yaw = NAN;
+		differential_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON;
+		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+
+	} else { // Course control if the steering input is zero (keep driving on a straight line)
+		if (!_course_control) {
+			_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
+			_pos_ctl_start_position_ned = _curr_pos_ned;
+			_course_control = true;
+		}
+
+		// 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 + sign(speed_body_x_setpoint) *
+						     vector_scaling * _pos_ctl_course_direction;
+		pure_pursuit_status_s pure_pursuit_status{};
+		pure_pursuit_status.timestamp = _timestamp;
+		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, _pos_ctl_start_position_ned,
+				     _curr_pos_ned, fabsf(speed_body_x_setpoint));
+		_pure_pursuit_status_pub.publish(pure_pursuit_status);
+		differential_velocity_setpoint_s differential_velocity_setpoint{};
+		differential_velocity_setpoint.timestamp = _timestamp;
+		differential_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		differential_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		differential_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON;
+		differential_velocity_setpoint.yaw = NAN;
+		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+	}
+}
+
+void DifferentialPosControl::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
+		_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();
+	}
+
+	// Distances to waypoints
+	const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
+					       2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
+
+	// Check stopping conditions
+	bool auto_stop{false};
+
+	if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+	    || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE
+	    || !_next_wp_ned.isAllFinite()) { // Check stopping conditions
+		auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
+	}
+
+	// State machine
+	pure_pursuit_status_s pure_pursuit_status{};
+	pure_pursuit_status.timestamp = _timestamp;
+	float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
+			     _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
+			     fabsf(_vehicle_speed_body_x));
+	_pure_pursuit_status_pub.publish(pure_pursuit_status);
+	const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw);
+
+	if (!auto_stop) {
+		if (_currentState == GuidanceState::STOPPED) {
+			_currentState = GuidanceState::DRIVING;
+		}
+
+		if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) {
+			_currentState = GuidanceState::SPOT_TURNING;
+
+		} else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) {
+			_currentState = GuidanceState::DRIVING;
+		}
+
+	} else { // Mission finished or delay command
+		_currentState = GuidanceState::STOPPED;
+	}
+
+	// Guidance logic
+	switch (_currentState) {
+	case GuidanceState::DRIVING: {
+			// Calculate desired speed in body x direction
+			const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
+							    _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(),
+							    _param_rd_miss_spd_gain.get(), _curr_wp_type);
+			differential_velocity_setpoint_s differential_velocity_setpoint{};
+			differential_velocity_setpoint.timestamp = _timestamp;
+			differential_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+			differential_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+			differential_velocity_setpoint.backwards = false;
+			differential_velocity_setpoint.yaw = NAN;
+			_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+
+		} break;
+
+	case GuidanceState::SPOT_TURNING: {
+			differential_velocity_setpoint_s differential_velocity_setpoint{};
+			differential_velocity_setpoint.timestamp = _timestamp;
+			differential_velocity_setpoint.velocity_ned[0] = 0.f;
+			differential_velocity_setpoint.velocity_ned[1] = 0.f;
+			differential_velocity_setpoint.backwards = false;
+			differential_velocity_setpoint.yaw = yaw_setpoint;
+			_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+		} break;
+
+	case GuidanceState::STOPPED:
+	default:
+		differential_velocity_setpoint_s differential_velocity_setpoint{};
+		differential_velocity_setpoint.timestamp = _timestamp;
+		differential_velocity_setpoint.velocity_ned[0] = 0.f;
+		differential_velocity_setpoint.velocity_ned[1] = 0.f;
+		differential_velocity_setpoint.backwards = false;
+		differential_velocity_setpoint.yaw = NAN;
+		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+		break;
+
+	}
+
+}
+
+float DifferentialPosControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp,
+		const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
+		const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type)
+{
+	// Upcoming stop
+	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle)
+			|| _waypoint_transition_angle < M_PI_F - trans_drv_trn || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
+			|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
+		const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
+						  max_decel, distance_to_curr_wp, 0.f);
+		return math::min(straight_line_speed, cruising_speed);
+	}
+
+	// Straight line speed
+	if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && 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);
+		const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel,
+						  distance_to_curr_wp,
+						  max_speed * (1.f - speed_reduction));
+		return math::min(straight_line_speed, cruising_speed);
+	}
+
+	return cruising_speed; // Fallthrough
+
+}
+
+void DifferentialPosControl::goToPositionMode()
+{
+	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
+	const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
+
+	if (distance_to_target > _param_nav_acc_rad.get()) {
+		const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
+					     _param_ro_decel_limit.get(), distance_to_target, 0.f);
+		const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ?
+					_rover_position_setpoint.cruising_speed :
+					_param_ro_speed_limit.get();
+		const float speed_body_x_setpoint = math::min(speed_setpoint, max_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, _curr_pos_ned,
+					   _curr_pos_ned, fabsf(speed_body_x_setpoint));
+		_pure_pursuit_status_pub.publish(pure_pursuit_status);
+		differential_velocity_setpoint_s differential_velocity_setpoint{};
+		differential_velocity_setpoint.timestamp = _timestamp;
+		differential_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		differential_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		differential_velocity_setpoint.backwards = false;
+		differential_velocity_setpoint.yaw = NAN;
+		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+
+	} else {
+		differential_velocity_setpoint_s differential_velocity_setpoint{};
+		differential_velocity_setpoint.timestamp = _timestamp;
+		differential_velocity_setpoint.velocity_ned[0] = 0.f;
+		differential_velocity_setpoint.velocity_ned[1] = 0.f;
+		differential_velocity_setpoint.backwards = false;
+		differential_velocity_setpoint.yaw = NAN;
+		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+	}
+}
+
+bool DifferentialPosControl::runSanityChecks()
+{
+	bool ret = true;
+
+	if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
+		ret = false;
+	}
+
+	if (_param_ro_speed_limit.get() < FLT_EPSILON) {
+		ret = false;
+	}
+
+	_prev_param_check_passed = ret;
+	return ret;
+}

src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.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>
@@ -49,11 +47,10 @@
 // uORB includes
 #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/differential_velocity_setpoint.h>
+#include <uORB/topics/pure_pursuit_status.h>
+#include <uORB/topics/rover_position_setpoint.h>
 #include <uORB/topics/rover_velocity_status.h>
-#include <uORB/topics/rover_attitude_setpoint.h>
-#include <uORB/topics/rover_steering_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/trajectory_setpoint.h>
@@ -75,22 +72,22 @@ enum class GuidanceState {
 };
 
 /**
- * @brief Class for differential position/velocity control.
+ * @brief Class for differential position control.
  */
-class DifferentialPosVelControl : public ModuleParams
+class DifferentialPosControl : public ModuleParams
 {
 public:
 	/**
-	 * @brief Constructor for DifferentialPosVelControl.
+	 * @brief Constructor for DifferentialPosControl.
 	 * @param parent The parent ModuleParams object.
 	 */
-	DifferentialPosVelControl(ModuleParams *parent);
-	~DifferentialPosVelControl() = default;
+	DifferentialPosControl(ModuleParams *parent);
+	~DifferentialPosControl() = default;
 
 	/**
 	 * @brief Update position controller.
 	 */
-	void updatePosVelControl();
+	void updatePosControl();
 
 protected:
 	/**
@@ -105,32 +102,31 @@ private:
 	void updateSubscriptions();
 
 	/**
-	 * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or
-	 * 	  trajcetorySetpoint (Offboard position or velocity control) or
-	 * 	  positionSetpointTriplet (Auto Mode).
+	 * @brief Generate and publish roverPositionSetpoint from position of trajectorySetpoint.
 	 */
-	void generateAttitudeSetpoint();
+	void generatePositionSetpoint();
 
 	/**
-	 * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint.
+	 * @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 roverAttitudeSetpoint from position of trajectorySetpoint.
-	 */
-	void offboardPositionMode();
-
-	/**
-	 * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint.
-	 */
-	void offboardVelocityMode();
-
-	/**
-	 * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet.
+	 * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet.
 	 */
 	void autoPositionMode();
 
+	/**
+	 * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint.
+	 */
+	void goToPositionMode();
+
 	/**
 	 * @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to
 	 * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
@@ -165,17 +161,18 @@ private:
 	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_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
+	uORB::Subscription _differential_velocity_setpoint_sub{ORB_ID(differential_velocity_setpoint)};
+	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_steering_setpoint_s _rover_steering_setpoint{};
+	rover_position_setpoint_s _rover_position_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_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<pure_pursuit_status_s>	_pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
+	uORB::Publication<rover_velocity_status_s>          _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
+	uORB::Publication<differential_velocity_setpoint_s> _differential_velocity_setpoint_pub{ORB_ID(differential_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)};
 
 	// Variables
 	hrt_abstime _timestamp{0};
@@ -184,13 +181,12 @@ private:
 	Vector2f _pos_ctl_course_direction{};
 	Vector2f _pos_ctl_start_position_ned{};
 	float _vehicle_speed_body_x{0.f};
-	float _vehicle_speed_body_y{0.f};
 	float _vehicle_yaw{0.f};
 	float _max_yaw_rate{0.f};
-	float _speed_body_x_setpoint{0.f};
 	float _dt{0.f};
 	int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
 	bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode.
+	bool _prev_param_check_passed{true};
 
 	// Waypoint variables
 	Vector2f _curr_wp_ned{};
@@ -198,11 +194,6 @@ private:
 	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]
-	bool _prev_param_check_passed{true};
-
-	// Controllers
-	PID _pid_speed;
-	SlewRate<float> _speed_setpoint;
 
 	// Class Instances
 	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
@@ -213,14 +204,12 @@ private:
 		(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
 		(ParamFloat<px4::params::RD_MISS_SPD_GAIN>) _param_rd_miss_spd_gain,
 		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
-		(ParamFloat<px4::params::RO_SPEED_P>) 	    _param_ro_speed_p,
-		(ParamFloat<px4::params::RO_SPEED_I>)       _param_ro_speed_i,
 		(ParamFloat<px4::params::RO_YAW_STICK_DZ>)  _param_ro_yaw_stick_dz,
-		(ParamFloat<px4::params::RO_ACCEL_LIM>)     _param_ro_accel_limit,
 		(ParamFloat<px4::params::RO_DECEL_LIM>)     _param_ro_decel_limit,
 		(ParamFloat<px4::params::RO_JERK_LIM>)      _param_ro_jerk_limit,
 		(ParamFloat<px4::params::RO_SPEED_LIM>)     _param_ro_speed_limit,
 		(ParamFloat<px4::params::RO_SPEED_TH>)      _param_ro_speed_th,
+		(ParamFloat<px4::params::RO_YAW_P>)  	    _param_ro_yaw_p,
 		(ParamFloat<px4::params::PP_LOOKAHD_GAIN>)  _param_pp_lookahd_gain,
 		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max,
 		(ParamFloat<px4::params::PP_LOOKAHD_MIN>)   _param_pp_lookahd_min,

src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp

@@ -1,418 +0,0 @@
-/****************************************************************************
- *
- *   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 "DifferentialPosVelControl.hpp"
-
-using namespace time_literals;
-
-DifferentialPosVelControl::DifferentialPosVelControl(ModuleParams *parent) : ModuleParams(parent)
-{
-	_rover_rate_setpoint_pub.advertise();
-	_rover_throttle_setpoint_pub.advertise();
-	_rover_attitude_setpoint_pub.advertise();
-	_rover_velocity_status_pub.advertise();
-	_pure_pursuit_status_pub.advertise();
-	updateParams();
-}
-
-void DifferentialPosVelControl::updateParams()
-{
-	ModuleParams::updateParams();
-	_pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
-	_pid_speed.setIntegralLimit(1.f);
-	_pid_speed.setOutputLimit(1.f);
-	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
-
-	if (_param_ro_accel_limit.get() > FLT_EPSILON) {
-		_speed_setpoint.setSlewRate(_param_ro_accel_limit.get());
-	}
-}
-
-void DifferentialPosVelControl::updatePosVelControl()
-{
-	const hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	updateSubscriptions();
-
-	if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled)
-	    && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-		generateAttitudeSetpoint();
-
-		if (_param_ro_max_thr_speed.get() > FLT_EPSILON) {
-
-			const 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);
-
-			if (_rover_steering_setpoint_sub.updated()) {
-				_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
-			}
-
-			if (fabsf(speed_body_x_setpoint_normalized) > 1.f -
-			    fabsf(_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels
-				_speed_body_x_setpoint = sign(_speed_body_x_setpoint) *
-							 math::interpolate<float>(1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff), -1.f, 1.f,
-									 -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
-			}
-		}
-
-		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;
-		rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed,
-				_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 = 0.f;
-		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-
-	} else { // Reset controller and slew rate when position control is not active
-		_pid_speed.resetIntegral();
-		_speed_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.speed_body_x_setpoint = _speed_body_x_setpoint;
-	rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState();
-	rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
-	rover_velocity_status.speed_body_y_setpoint = NAN;
-	rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
-	rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
-	rover_velocity_status.pid_throttle_body_y_integral = NAN;
-	_rover_velocity_status_pub.publish(rover_velocity_status);
-}
-
-void DifferentialPosVelControl::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);
-		_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);
-
-		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);
-		}
-
-		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
-		const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-		const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-		_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
-		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
-	}
-
-}
-
-void DifferentialPosVelControl::generateAttitudeSetpoint()
-{
-	if (_vehicle_control_mode.flag_control_manual_enabled
-	    && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode
-		manualPositionMode();
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		if (_offboard_control_mode.position) {
-			offboardPositionMode();
-
-		} else if (_offboard_control_mode.velocity) {
-			offboardVelocityMode();
-		}
-
-	} else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode
-		autoPositionMode();
-	}
-}
-
-void DifferentialPosVelControl::manualPositionMode()
-{
-	manual_control_setpoint_s manual_control_setpoint{};
-
-	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-		_speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-					 -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
-		const float yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
-						_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-
-		if (fabsf(yaw_rate_setpoint) > FLT_EPSILON
-		    || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
-			_course_control = false;
-			rover_rate_setpoint_s rover_rate_setpoint{};
-			rover_rate_setpoint.timestamp = _timestamp;
-			rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint;
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-
-		} else { // Course control if the steering input is zero (keep driving on a straight line)
-			if (!_course_control) {
-				_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
-				_pos_ctl_start_position_ned = _curr_pos_ned;
-				_course_control = true;
-			}
-
-			// 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 + sign(_speed_body_x_setpoint) *
-							     vector_scaling * _pos_ctl_course_direction;
-			pure_pursuit_status_s pure_pursuit_status{};
-			pure_pursuit_status.timestamp = _timestamp;
-			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, _pos_ctl_start_position_ned,
-					     _curr_pos_ned, fabsf(_speed_body_x_setpoint));
-			_pure_pursuit_status_pub.publish(pure_pursuit_status);
-			yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F);
-			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);
-		}
-	}
-}
-
-void DifferentialPosVelControl::offboardPositionMode()
-{
-	trajectory_setpoint_s trajectory_setpoint{};
-	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-	// Translate trajectory setpoint to rover setpoints
-	const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]);
-	const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
-
-	if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) {
-		const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-					     _param_ro_decel_limit.get(), distance_to_target, 0.f);
-		_speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
-		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, _curr_pos_ned,
-					   _curr_pos_ned, fabsf(_speed_body_x_setpoint));
-		_pure_pursuit_status_pub.publish(pure_pursuit_status);
-		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 {
-		_speed_body_x_setpoint = 0.f;
-	}
-}
-
-void DifferentialPosVelControl::offboardVelocityMode()
-{
-	trajectory_setpoint_s trajectory_setpoint{};
-	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-	const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
-
-	if (velocity_in_local_frame.isAllFinite()) {
-		rover_attitude_setpoint_s rover_attitude_setpoint{};
-		rover_attitude_setpoint.timestamp = _timestamp;
-		rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0));
-		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-		_speed_body_x_setpoint = velocity_in_local_frame.norm();
-	}
-}
-
-void DifferentialPosVelControl::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
-		_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();
-	}
-
-	// Distances to waypoints
-	const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
-					       2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
-
-	// Check stopping conditions
-	bool auto_stop{false};
-
-	if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
-	    || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE
-	    || !_next_wp_ned.isAllFinite()) { // Check stopping conditions
-		auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
-	}
-
-	// State machine
-	pure_pursuit_status_s pure_pursuit_status{};
-	pure_pursuit_status.timestamp = _timestamp;
-	float yaw_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
-			     _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
-			     fabsf(_speed_body_x_setpoint));
-	_pure_pursuit_status_pub.publish(pure_pursuit_status);
-	const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw);
-
-	if (!auto_stop) {
-		if (_currentState == GuidanceState::STOPPED) {
-			_currentState = GuidanceState::DRIVING;
-		}
-
-		if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) {
-			_currentState = GuidanceState::SPOT_TURNING;
-
-		} else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) {
-			_currentState = GuidanceState::DRIVING;
-		}
-
-	} else { // Mission finished or delay command
-		_currentState = GuidanceState::STOPPED;
-	}
-
-	// Guidance logic
-	switch (_currentState) {
-	case GuidanceState::DRIVING: {
-			// Calculate desired speed in body x direction
-			_speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
-						 _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(),
-						 _param_rd_miss_spd_gain.get(), _curr_wp_type);
-			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);
-
-		} break;
-
-	case GuidanceState::SPOT_TURNING: {
-			_speed_body_x_setpoint = 0.f;
-
-			if (fabsf(_vehicle_speed_body_x) > 0.f) {
-				yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop
-
-			}
-
-			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);
-		} break;
-
-	case GuidanceState::STOPPED:
-	default:
-		_speed_body_x_setpoint = 0.f;
-		rover_rate_setpoint_s rover_rate_setpoint{};
-		rover_rate_setpoint.timestamp = _timestamp;
-		rover_rate_setpoint.yaw_rate_setpoint = 0.f;
-		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-		break;
-
-	}
-
-
-}
-
-float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp,
-		const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
-		const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type)
-{
-	// Upcoming stop
-	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle)
-			|| _waypoint_transition_angle < M_PI_F - trans_drv_trn || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
-			|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
-		const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-						  max_decel, distance_to_curr_wp, 0.f);
-		return math::min(straight_line_speed, cruising_speed);
-	}
-
-	// Straight line speed
-	if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && 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);
-		const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel,
-						  distance_to_curr_wp,
-						  max_speed * (1.f - speed_reduction));
-		return math::min(straight_line_speed, cruising_speed);
-	}
-
-	return cruising_speed; // Fallthrough
-
-}
-
-bool DifferentialPosVelControl::runSanityChecks()
-{
-	bool ret = true;
-
-	if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
-		ret = false;
-	}
-
-	if (_param_ro_speed_limit.get() < FLT_EPSILON) {
-		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("differential_posVel_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("differential_posVel_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_differential/DifferentialVelControl/CMakeLists.txt

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

src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp

@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   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 "DifferentialVelControl.hpp"
+
+using namespace time_literals;
+
+DifferentialVelControl::DifferentialVelControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_rover_throttle_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
+	_differential_velocity_setpoint_pub.advertise();
+	_rover_velocity_status_pub.advertise();
+	updateParams();
+}
+
+void DifferentialVelControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
+	_pid_speed.setIntegralLimit(1.f);
+	_pid_speed.setOutputLimit(1.f);
+
+	if (_param_ro_accel_limit.get() > FLT_EPSILON) {
+		_speed_setpoint.setSlewRate(_param_ro_accel_limit.get());
+	}
+}
+
+void DifferentialVelControl::updateVelControl()
+{
+	const hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	_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();
+		}
+
+		generateAttitudeAndThrottleSetpoint();
+
+	} else { // Reset controller and slew rate when velocity control is not active
+		_pid_speed.resetIntegral();
+		_speed_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_setpoint.getState();
+	rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
+	rover_velocity_status.adjusted_speed_body_y_setpoint = NAN;
+	rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral();
+	rover_velocity_status.pid_throttle_body_y_integral = NAN;
+	_rover_velocity_status_pub.publish(rover_velocity_status);
+}
+void DifferentialVelControl::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);
+		_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);
+		const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
+		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
+	}
+
+}
+
+void DifferentialVelControl::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()) {
+		differential_velocity_setpoint_s differential_velocity_setpoint{};
+		differential_velocity_setpoint.timestamp = _timestamp;
+		differential_velocity_setpoint.velocity_ned[0] = velocity_in_local_frame(0);
+		differential_velocity_setpoint.velocity_ned[1] = velocity_in_local_frame(1);
+		differential_velocity_setpoint.backwards = false;
+		_differential_velocity_setpoint_pub.publish(differential_velocity_setpoint);
+	}
+}
+
+void DifferentialVelControl::generateAttitudeAndThrottleSetpoint()
+{
+	if (_differential_velocity_setpoint_sub.updated()) {
+		_differential_velocity_setpoint_sub.copy(&_differential_velocity_setpoint);
+	}
+
+	// Catch spot turning
+	if (PX4_ISFINITE(_differential_velocity_setpoint.yaw)) {
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = _timestamp;
+		rover_attitude_setpoint.yaw_setpoint = _differential_velocity_setpoint.yaw;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+		rover_throttle_setpoint_s rover_throttle_setpoint{};
+		rover_throttle_setpoint.timestamp = _timestamp;
+		rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed,
+				0.f, _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 = 0.f;
+		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+		return;
+	}
+
+	// Attitude Setpoint
+	if (fabsf(_differential_velocity_setpoint.velocity_ned[1]) < FLT_EPSILON
+	    && fabsf(_differential_velocity_setpoint.velocity_ned[0]) < FLT_EPSILON) {
+		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);
+
+	} else {
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = _timestamp;
+		const float yaw_setpoint = atan2f(_differential_velocity_setpoint.velocity_ned[1],
+						  _differential_velocity_setpoint.velocity_ned[0]);
+		rover_attitude_setpoint.yaw_setpoint = _differential_velocity_setpoint.backwards ? matrix::wrap_pi(
+				yaw_setpoint + M_PI_F) : yaw_setpoint;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+	}
+
+	// Throttle Setpoint
+	float speed_magnitude = math::min(sqrtf(powf(_differential_velocity_setpoint.velocity_ned[0],
+						2) + powf(_differential_velocity_setpoint.velocity_ned[1], 2)), _param_ro_speed_limit.get());
+
+	if (_param_ro_max_thr_speed.get() > FLT_EPSILON) {
+
+		const float speed_body_x_setpoint_normalized = math::interpolate<float>(speed_magnitude,
+				-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
+
+		if (_rover_steering_setpoint_sub.updated()) {
+			_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
+		}
+
+		if (fabsf(speed_body_x_setpoint_normalized) > 1.f -
+		    fabsf(_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels
+			speed_magnitude = math::interpolate<float>(1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff), -1.f, 1.f,
+					  -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
+		}
+	}
+
+	const float speed_body_x_setpoint = _differential_velocity_setpoint.backwards ? -speed_magnitude : speed_magnitude;
+	rover_throttle_setpoint_s rover_throttle_setpoint{};
+	rover_throttle_setpoint.timestamp = _timestamp;
+	rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed,
+			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 = 0.f;
+	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+
+}
+
+bool DifferentialVelControl::runSanityChecks()
+{
+	bool ret = true;
+
+	if (_param_ro_speed_limit.get() < FLT_EPSILON) {
+		ret = false;
+
+		if (_prev_param_check_passed) {
+			events::send<float>(events::ID("differential_posVel_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("differential_posVel_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_differential/DifferentialVelControl/DifferentialVelControl.hpp

@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   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>
+#include <px4_platform_common/events.h>
+
+// Libraries
+#include <lib/rover_control/RoverControl.hpp>
+#include <lib/pid/PID.hpp>
+#include <matrix/matrix/math.hpp>
+#include <lib/slew_rate/SlewRate.hpp>
+#include <math.h>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/rover_steering_setpoint.h>
+#include <uORB/topics/rover_throttle_setpoint.h>
+#include <uORB/topics/rover_velocity_status.h>
+#include <uORB/topics/differential_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;
+
+/**
+ * @brief Class for differential velocity control.
+ */
+class DifferentialVelControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for DifferentialVelControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	DifferentialVelControl(ModuleParams *parent);
+	~DifferentialVelControl() = default;
+
+	/**
+	 * @brief Update velocity controller.
+	 */
+	void updateVelControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Update uORB subscriptions used in velocity controller.
+	 */
+	void updateSubscriptions();
+
+	/**
+	 * @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint.
+	 */
+	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();
+
+	// 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 _differential_velocity_setpoint_sub{ORB_ID(differential_velocity_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<differential_velocity_setpoint_s> _differential_velocity_setpoint_pub{ORB_ID(differential_velocity_setpoint)};
+	differential_velocity_setpoint_s _differential_velocity_setpoint{};
+
+	// Variables
+	hrt_abstime _timestamp{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};
+
+	// Controllers
+	PID _pid_speed;
+	SlewRate<float> _speed_setpoint;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
+		(ParamFloat<px4::params::RO_SPEED_P>) 	    _param_ro_speed_p,
+		(ParamFloat<px4::params::RO_SPEED_I>)       _param_ro_speed_i,
+		(ParamFloat<px4::params::RO_ACCEL_LIM>)     _param_ro_accel_limit,
+		(ParamFloat<px4::params::RO_DECEL_LIM>)     _param_ro_decel_limit,
+		(ParamFloat<px4::params::RO_JERK_LIM>)      _param_ro_jerk_limit,
+		(ParamFloat<px4::params::RO_SPEED_LIM>)     _param_ro_speed_limit,
+		(ParamFloat<px4::params::RO_SPEED_TH>)      _param_ro_speed_th
+
+	)
+};

src/modules/rover_differential/RoverDifferential.cpp

@@ -69,7 +69,8 @@ void RoverDifferential::Run()
 	_timestamp = hrt_absolute_time();
 	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
-	_differential_pos_vel_control.updatePosVelControl();
+	_differential_pos_control.updatePosControl();
+	_differential_vel_control.updateVelControl();
 	_differential_att_control.updateAttControl();
 	_differential_rate_control.updateRateControl();
 
@@ -82,7 +83,7 @@ void RoverDifferential::Run()
 					      && !_vehicle_control_mode.flag_control_rates_enabled;
 
 	if (full_manual_mode_enabled) { // Manual mode
-		generateSteeringSetpoint();
+		generateSteeringAndThrottleSetpoint();
 	}
 
 	if (_vehicle_control_mode.flag_armed) {
@@ -92,7 +93,7 @@ void RoverDifferential::Run()
 
 }
 
-void RoverDifferential::generateSteeringSetpoint()
+void RoverDifferential::generateSteeringAndThrottleSetpoint()
 {
 	manual_control_setpoint_s manual_control_setpoint{};
 

src/modules/rover_differential/RoverDifferential.hpp

@@ -58,7 +58,8 @@
 // Local includes
 #include "DifferentialRateControl/DifferentialRateControl.hpp"
 #include "DifferentialAttControl/DifferentialAttControl.hpp"
-#include "DifferentialPosVelControl/DifferentialPosVelControl.hpp"
+#include "DifferentialVelControl/DifferentialVelControl.hpp"
+#include "DifferentialPosControl/DifferentialPosControl.hpp"
 
 class RoverDifferential : public ModuleBase<RoverDifferential>, public ModuleParams,
 	public px4::ScheduledWorkItem
@@ -91,9 +92,9 @@ private:
 	void Run() override;
 
 	/**
-	 * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode).
+	 * @brief Generate and publish roverSteeringSetpoint and roverThrottleSetpoint from manualControlSetpoint (Manual Mode).
 	 */
-	void generateSteeringSetpoint();
+	void generateSteeringAndThrottleSetpoint();
 
 	/**
 	 * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
@@ -127,7 +128,8 @@ private:
 	// Class instances
 	DifferentialRateControl _differential_rate_control{this};
 	DifferentialAttControl _differential_att_control{this};
-	DifferentialPosVelControl _differential_pos_vel_control{this};
+	DifferentialVelControl _differential_vel_control{this};
+	DifferentialPosControl _differential_pos_control{this};
 
 	// Variables
 	hrt_abstime _timestamp{0};