ackermann: seperate velocity control

msg/AckermannVelocitySetpoint.msg

@@ -0,0 +1,4 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32[2] velocity_ned # 2-dimensional velocity setpoint in NED frame [m/s]
+bool backwards	        # Flag for backwards driving

msg/CMakeLists.txt

@@ -37,6 +37,7 @@ cmake_policy(SET CMP0057 NEW)
 include(px4_list_make_absolute)
 
 set(msg_files
+	AckermannVelocitySetpoint.msg
 	ActionRequest.msg
 	ActuatorArmed.msg
 	ActuatorControlsStatus.msg

msg/PurePursuitStatus.msg

@@ -5,5 +5,3 @@ float32 target_bearing       # [rad] Target bearing calculated by the pure pursu
 float32 crosstrack_error     # [m] Shortest distance from the vehicle to the path (Positiv: Vehicle is on the right hand side with respect to the oriented path vector, Negativ: Left of the path)
 float32 distance_to_waypoint # [m] Distance from the vehicle to the current waypoint
 float32 bearing_to_waypoint  # [rad] Bearing towards current waypoint
-
-# TOPICS pure_pursuit_status

msg/RoverAttitudeSetpoint.msg

@@ -1,5 +1,3 @@
 uint64 timestamp # time since system start (microseconds)
 
 float32 yaw_setpoint # [rad] Expressed in NED frame
-
-# TOPICS rover_attitude_setpoint

msg/RoverAttitudeStatus.msg

@@ -2,5 +2,3 @@ uint64 timestamp # time since system start (microseconds)
 
 float32 measured_yaw          # [rad/s] Measured yaw rate
 float32 adjusted_yaw_setpoint # [rad/s] Yaw setpoint that is being tracked (Applied slew rates)
-
-# TOPICS rover_attitude_status

msg/RoverRateSetpoint.msg

@@ -1,5 +1,3 @@
 uint64 timestamp # time since system start (microseconds)
 
 float32 yaw_rate_setpoint # [rad/s] Expressed in NED frame
-
-# TOPICS rover_rate_setpoint

msg/RoverRateStatus.msg

@@ -3,5 +3,3 @@ uint64 timestamp # time since system start (microseconds)
 float32 measured_yaw_rate          # [rad/s] Measured yaw rate
 float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint that is being tracked (Applied slew rates)
 float32 pid_yaw_rate_integral      # Integral of the PID for the closed loop yaw rate controller
-
-# TOPICS rover_rate_status

msg/RoverSteeringSetpoint.msg

@@ -1,6 +1,5 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Only for Ackermann-steered rovers)
-float32 normalized_speed_diff     # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Only for Differential/Mecanum rovers)
+float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Ackermann only, Positiv: Steer right, Negativ: Steer left)
 
-# TOPICS rover_steering_setpoint
+float32 normalized_speed_diff     # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Differential/Mecanum only, Positiv = Turn right, Negativ: Turn left)

msg/RoverThrottleSetpoint.msg

@@ -1,7 +1,6 @@
 
 uint64 timestamp        # time since system start (microseconds)
 
-float32 throttle_body_x # throttle setpoint along body X axis [-1, 1]
-float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1]
+float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] (Positiv = forwards, Negativ = backwards)
 
-# TOPICS rover_throttle_setpoint
+float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1] (Mecanum only, Positiv = right, Negativ = left)

msg/RoverVelocityStatus.msg

@@ -8,5 +8,3 @@ float32 speed_body_y_setpoint          # [m/s] Speed setpoint in body y directio
 float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers)
 float32 pid_throttle_body_x_integral   # Integral of the PID for the closed loop controller of the speed in body x direction
 float32 pid_throttle_body_y_integral   # Integral of the PID for the closed loop controller of the speed in body y direction
-
-# TOPICS rover_velocity_status

src/modules/logger/logged_topics.cpp

@@ -45,6 +45,7 @@ using namespace px4::logger;
 
 void LoggedTopics::add_default_topics()
 {
+	add_optional_topic("ackermann_velocity_setpoint", 100);
 	add_topic("action_request");
 	add_topic("actuator_armed");
 	add_optional_topic("actuator_controls_status_0", 300);
@@ -107,6 +108,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("radio_status");
 	add_optional_topic("rover_attitude_setpoint", 100);
 	add_optional_topic("rover_attitude_status", 100);
+	add_optional_topic("rover_position_setpoint", 100);
 	add_optional_topic("rover_rate_setpoint", 100);
 	add_optional_topic("rover_rate_status", 100);
 	add_optional_topic("rover_steering_setpoint", 100);

src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp

@@ -31,39 +31,29 @@
  *
  ****************************************************************************/
 
-#include "AckermannPosVelControl.hpp"
+#include "AckermannPosControl.hpp"
 
 using namespace time_literals;
 
-AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModuleParams(parent)
+AckermannPosControl::AckermannPosControl(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();
+	_ackermann_velocity_setpoint_pub.advertise();
 	updateParams();
 }
 
-void AckermannPosVelControl::updateParams()
+void AckermannPosControl::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());
-	}
-
 	if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON
 	    && _param_ra_max_str_ang.get() > FLT_EPSILON) {
 		_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
 	}
 }
 
-void AckermannPosVelControl::updatePosVelControl()
+void AckermannPosControl::updatePosControl()
 {
 	const hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
@@ -71,37 +61,14 @@ void AckermannPosVelControl::updatePosVelControl()
 
 	updateSubscriptions();
 
-	if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled)
-	    && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-		generateAttitudeSetpoint();
-		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);
+	if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
+		generateVelocitySetpoint();
 
-	} 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 AckermannPosVelControl::updateSubscriptions()
+void AckermannPosControl::updateSubscriptions()
 {
 	if (_vehicle_control_mode_sub.updated()) {
 		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
@@ -125,128 +92,81 @@ void AckermannPosVelControl::updateSubscriptions()
 		}
 
 		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
-		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-		_vehicle_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 AckermannPosVelControl::generateAttitudeSetpoint()
+void AckermannPosControl::generateVelocitySetpoint()
 {
-	if (_vehicle_control_mode.flag_control_manual_enabled
-	    && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode
+	// Manual Position Mode
+	if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) {
 		manualPositionMode();
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Position 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();
+		return;
 	}
+
+	// Auto Mode
+	if (_vehicle_control_mode.flag_control_auto_enabled) {
+		autoPositionMode();
+		return;
+	}
+
+	// Offboard Position Mode
+	if (_offboard_control_mode_sub.copy(&_offboard_control_mode) && _offboard_control_mode.position) {
+		goToPositionMode();
+	}
+
 }
 
-void AckermannPosVelControl::manualPositionMode()
+void AckermannPosControl::manualPositionMode()
 {
 	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&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);
+	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 yaw_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(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 = matrix::sign(_vehicle_speed_body_x) * yaw_rate_setpoint;
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+	if (fabsf(yaw_delta) > FLT_EPSILON
+	    || fabsf(speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
+		_course_control = false;
+		const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
+		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
+		ackermann_velocity_setpoint.timestamp = _timestamp;
+		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		ackermann_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON;
+		_ackermann_velocity_setpoint_pub.publish(ackermann_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);
-			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);
+	} 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;
 		}
-	}
-}
 
-void AckermannPosVelControl::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());
+		// 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;
 		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));
+					   _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);
-		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;
+		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
+		ackermann_velocity_setpoint.timestamp = _timestamp;
+		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		ackermann_velocity_setpoint.backwards = speed_body_x_setpoint < -FLT_EPSILON;
+		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
 	}
 }
 
-void AckermannPosVelControl::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 AckermannPosVelControl::autoPositionMode()
+void AckermannPosControl::autoPositionMode()
 {
 	if (_position_setpoint_triplet_sub.updated()) {
 		updateWaypointsAndAcceptanceRadius();
@@ -263,36 +183,40 @@ void AckermannPosVelControl::autoPositionMode()
 
 	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
+	    || !_next_wp_ned.isAllFinite()) {
 		auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
 	}
 
 	if (auto_stop) {
-		_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);
+		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
+		ackermann_velocity_setpoint.timestamp = _timestamp;
+		ackermann_velocity_setpoint.velocity_ned[0] = 0.f;
+		ackermann_velocity_setpoint.velocity_ned[1] = 0.f;
+		ackermann_velocity_setpoint.backwards = false;
+		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
 
 	} else { // Regular guidance algorithm
-		_speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp,
-					 _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _curr_wp_type,
-					 _waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get());
+		const float speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp,
+						    distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(),
+						    _param_ro_jerk_limit.get(), _curr_wp_type, _waypoint_transition_angle, _prev_waypoint_transition_angle,
+						    _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(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
-					   fabsf(_speed_body_x_setpoint));
+					   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);
+		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
+		ackermann_velocity_setpoint.timestamp = _timestamp;
+		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		ackermann_velocity_setpoint.backwards = false;
+		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
 
 	}
 }
 
-void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius()
+void AckermannPosControl::updateWaypointsAndAcceptanceRadius()
 {
 	position_setpoint_triplet_s position_setpoint_triplet{};
 	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
@@ -320,7 +244,7 @@ void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius()
 				  position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
 }
 
-float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transition_angle,
+float AckermannPosControl::updateAcceptanceRadius(const float waypoint_transition_angle,
 		const float default_acceptance_radius, const float acceptance_radius_gain,
 		const float acceptance_radius_max, const float wheel_base, const float max_steer_angle)
 {
@@ -345,7 +269,7 @@ float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transi
 	return acceptance_radius;
 }
 
-float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min,
+float AckermannPosControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min,
 		const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad,
 		const float prev_acc_rad, const float max_decel, const float max_jerk, const int curr_wp_type,
 		const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed)
@@ -394,7 +318,41 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons
 
 }
 
-bool AckermannPosVelControl::runSanityChecks()
+void AckermannPosControl::goToPositionMode()
+{
+	trajectory_setpoint_s trajectory_setpoint{};
+	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
+	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 (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 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);
+		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
+		ackermann_velocity_setpoint.timestamp = _timestamp;
+		ackermann_velocity_setpoint.velocity_ned[0] = fabsf(speed_body_x_setpoint) * cosf(yaw_setpoint);
+		ackermann_velocity_setpoint.velocity_ned[1] = fabsf(speed_body_x_setpoint) * sinf(yaw_setpoint);
+		ackermann_velocity_setpoint.backwards = false;
+		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
+
+	} else {
+		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
+		ackermann_velocity_setpoint.timestamp = _timestamp;
+		ackermann_velocity_setpoint.velocity_ned[0] = 0.f;
+		ackermann_velocity_setpoint.velocity_ned[1] = 0.f;
+		ackermann_velocity_setpoint.backwards = false;
+		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
+	}
+}
+
+bool AckermannPosControl::runSanityChecks()
 {
 	bool ret = true;
 
@@ -412,12 +370,10 @@ bool AckermannPosVelControl::runSanityChecks()
 
 	if (_param_ro_speed_limit.get() < FLT_EPSILON) {
 		ret = false;
+	}
 
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("ackermann_posVel_control_conf_invalid_speed_lim"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_yaw_rate_limit.get());
-		}
-
+	if (_param_ro_yaw_p.get() < FLT_EPSILON) {
+		ret = false;
 	}
 
 	_prev_param_check_passed = ret;

src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp

@@ -49,10 +49,7 @@
 // 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/rover_velocity_status.h>
-#include <uORB/topics/rover_attitude_setpoint.h>
+#include <uORB/topics/ackermann_velocity_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/trajectory_setpoint.h>
@@ -69,20 +66,20 @@ using namespace matrix;
 /**
  * @brief Class for ackermann position control.
  */
-class AckermannPosVelControl : public ModuleParams
+class AckermannPosControl : public ModuleParams
 {
 public:
 	/**
-	 * @brief Constructor for AckermannPosVelControl.
+	 * @brief Constructor for AckermannPosControl.
 	 * @param parent The parent ModuleParams object.
 	 */
-	AckermannPosVelControl(ModuleParams *parent);
-	~AckermannPosVelControl() = default;
+	AckermannPosControl(ModuleParams *parent);
+	~AckermannPosControl() = default;
 
 	/**
 	 * @brief Update position controller.
 	 */
-	void updatePosVelControl();
+	void updatePosControl();
 
 protected:
 	/**
@@ -97,32 +94,26 @@ 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 roverVelocitySetpoint from manualControlSetpoint (Position Mode) or
+	 * 	  positionSetpointTriplet (Auto Mode) or roverPositionSetpoint.
 	 */
-	void generateAttitudeSetpoint();
+	void generateVelocitySetpoint();
 
 	/**
-	 * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint.
+	 * @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 trajectorySetpoint.
+	 */
+	void goToPositionMode();
+
 	/**
 	 * @brief Update global/NED waypoint coordinates and acceptance radius.
 	 */
@@ -182,12 +173,9 @@ private:
 	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_velocity_status_s>      _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
-	uORB::Publication<position_controller_status_s>	_position_controller_status_pub{ORB_ID(position_controller_status)};
-	uORB::Publication<pure_pursuit_status_s>	_pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
+	uORB::Publication<ackermann_velocity_setpoint_s> _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_setpoint)};
+	uORB::Publication<position_controller_status_s>	 _position_controller_status_pub{ORB_ID(position_controller_status)};
+	uORB::Publication<pure_pursuit_status_s>	 _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
 
 	// Variables
 	hrt_abstime _timestamp{0};
@@ -195,11 +183,8 @@ private:
 	Vector2f _curr_pos_ned{};
 	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 _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
 	float _dt{0.f};
 	int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
@@ -216,17 +201,11 @@ private:
 	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 	float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 
-	// Controllers
-	PID _pid_speed;
-	SlewRate<float> _speed_setpoint;
-
 	// Class Instances
 	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
 
 	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_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,
@@ -237,6 +216,7 @@ private:
 		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max,
 		(ParamFloat<px4::params::PP_LOOKAHD_MIN>)   _param_pp_lookahd_min,
 		(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::RA_ACC_RAD_MAX>)   _param_ra_acc_rad_max,
 		(ParamFloat<px4::params::RA_ACC_RAD_GAIN>)  _param_ra_acc_rad_gain,
 		(ParamFloat<px4::params::RA_MAX_STR_ANG>)   _param_ra_max_str_ang,

src/modules/rover_ackermann/AckermannPosControl/CMakeLists.txt

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

src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp

@@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   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 "AckermannVelControl.hpp"
+
+using namespace time_literals;
+
+AckermannVelControl::AckermannVelControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_rover_throttle_setpoint_pub.advertise();
+	_rover_velocity_status_pub.advertise();
+	_ackermann_velocity_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
+	updateParams();
+}
+
+void AckermannVelControl::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 AckermannVelControl::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 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.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 AckermannVelControl::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);
+
+		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		_vehicle_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 AckermannVelControl::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()) {
+		ackermann_velocity_setpoint_s ackermann_velocity_setpoint{};
+		ackermann_velocity_setpoint.timestamp = _timestamp;
+		ackermann_velocity_setpoint.velocity_ned[0] = velocity_in_local_frame(0);
+		ackermann_velocity_setpoint.velocity_ned[1] = velocity_in_local_frame(1);
+		ackermann_velocity_setpoint.backwards = false;
+		_ackermann_velocity_setpoint_pub.publish(ackermann_velocity_setpoint);
+	}
+}
+
+void AckermannVelControl::generateAttitudeAndThrottleSetpoint()
+{
+	if (_ackermann_velocity_setpoint_sub.updated()) {
+		_ackermann_velocity_setpoint_sub.copy(&_ackermann_velocity_setpoint);
+	}
+
+	// Attitude Setpoint
+	if (fabsf(_ackermann_velocity_setpoint.velocity_ned[1]) < FLT_EPSILON
+	    && fabsf(_ackermann_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(_ackermann_velocity_setpoint.velocity_ned[1],
+						  _ackermann_velocity_setpoint.velocity_ned[0]);
+		rover_attitude_setpoint.yaw_setpoint = _ackermann_velocity_setpoint.backwards ? matrix::wrap_pi(
+				yaw_setpoint + M_PI_F) : yaw_setpoint;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+	}
+
+	// Throttle Setpoint
+	const float speed_magnitude = math::min(sqrtf(powf(_ackermann_velocity_setpoint.velocity_ned[0],
+						2) + powf(_ackermann_velocity_setpoint.velocity_ned[1], 2)), _param_ro_speed_limit.get());
+	const float speed_body_x_setpoint = _ackermann_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 AckermannVelControl::runSanityChecks()
+{
+	bool ret = true;
+
+	if (_param_ro_max_thr_speed.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("ackermann_vel_control_conf_invalid_speed_lim"), events::Log::Error,
+					    "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get());
+		}
+
+	}
+
+	_prev_param_check_passed = ret;
+	return ret;
+}

src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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>
+
+// Library includes
+#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_throttle_setpoint.h>
+#include <uORB/topics/ackermann_velocity_setpoint.h>
+#include <uORB/topics/rover_velocity_status.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 ackermann position control.
+ */
+class AckermannVelControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for AckermannVelControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	AckermannVelControl(ModuleParams *parent);
+	~AckermannVelControl() = 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 _ackermann_velocity_setpoint_sub{ORB_ID(ackermann_velocity_setpoint)};
+	vehicle_control_mode_s _vehicle_control_mode{};
+	offboard_control_mode_s _offboard_control_mode{};
+
+	// 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<ackermann_velocity_setpoint_s> _ackermann_velocity_setpoint_pub{ORB_ID(ackermann_velocity_setpoint)};
+	ackermann_velocity_setpoint_s _ackermann_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{true};
+
+	// 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_ackermann/AckermannVelControl/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(AckermannVelControl
+	AckermannVelControl.cpp
+)
+
+target_link_libraries(AckermannVelControl PUBLIC PID)
+target_include_directories(AckermannVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_ackermann/CMakeLists.txt

@@ -33,7 +33,8 @@
 
 add_subdirectory(AckermannRateControl)
 add_subdirectory(AckermannAttControl)
-add_subdirectory(AckermannPosVelControl)
+add_subdirectory(AckermannVelControl)
+add_subdirectory(AckermannPosControl)
 
 px4_add_module(
 	MODULE modules__rover_ackermann
@@ -44,7 +45,8 @@ px4_add_module(
 	DEPENDS
 		AckermannRateControl
 		AckermannAttControl
-		AckermannPosVelControl
+		AckermannVelControl
+		AckermannPosControl
 		px4_work_queue
 		rover_control
 		pure_pursuit

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -73,7 +73,8 @@ void RoverAckermann::Run()
 	_timestamp = hrt_absolute_time();
 	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
-	_ackermann_pos_vel_control.updatePosVelControl();
+	_ackermann_pos_control.updatePosControl();
+	_ackermann_vel_control.updateVelControl();
 	_ackermann_att_control.updateAttControl();
 	_ackermann_rate_control.updateRateControl();
 
@@ -86,14 +87,14 @@ void RoverAckermann::Run()
 					      && !_vehicle_control_mode.flag_control_rates_enabled;
 
 	if (full_manual_mode_enabled) { // Manual mode
-		generateSteeringSetpoint();
+		generateSteeringAndThrottleSetpoint();
 	}
 
 	generateActuatorSetpoint();
 
 }
 
-void RoverAckermann::generateSteeringSetpoint()
+void RoverAckermann::generateSteeringAndThrottleSetpoint()
 {
 	manual_control_setpoint_s manual_control_setpoint{};
 

src/modules/rover_ackermann/RoverAckermann.hpp

@@ -60,7 +60,8 @@
 // Local includes
 #include "AckermannRateControl/AckermannRateControl.hpp"
 #include "AckermannAttControl/AckermannAttControl.hpp"
-#include "AckermannPosVelControl/AckermannPosVelControl.hpp"
+#include "AckermannVelControl/AckermannVelControl.hpp"
+#include "AckermannPosControl/AckermannPosControl.hpp"
 
 class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	public px4::ScheduledWorkItem
@@ -93,9 +94,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/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
@@ -122,8 +123,9 @@ private:
 
 	// Class instances
 	AckermannRateControl _ackermann_rate_control{this};
-	AckermannAttControl _ackermann_att_control{this};
-	AckermannPosVelControl _ackermann_pos_vel_control{this};
+	AckermannAttControl  _ackermann_att_control{this};
+	AckermannVelControl  _ackermann_vel_control{this};
+	AckermannPosControl  _ackermann_pos_control{this};
 
 	// Variables
 	hrt_abstime _timestamp{0};