ackermann: refactor code architecture

ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann

@@ -11,23 +11,33 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann}
 
 param set-default SIM_GZ_EN 1 # Gazebo bridge
 
-# Rover parameters
 param set-default NAV_ACC_RAD 0.5
+
+# Ackermann Parameters
+param set-default RA_WHEEL_BASE 0.321
 param set-default RA_ACC_RAD_GAIN 2
 param set-default RA_ACC_RAD_MAX 3
-param set-default RA_LAT_ACCEL_I 0.01
-param set-default RA_LAT_ACCEL_P 0.1
-param set-default RA_MAX_ACCEL 3
-param set-default RA_MAX_DECEL 6
-param set-default RA_MAX_JERK 15
-param set-default RA_MAX_LAT_ACCEL 4
-param set-default RA_MAX_SPEED 3
 param set-default RA_MAX_STR_ANG 0.5236
-param set-default RA_MAX_STR_RATE 360
+param set-default RA_STR_RATE_LIM 360
-param set-default RA_MAX_THR_SPEED 3.1
+
-param set-default RA_SPEED_I 0.01
+# Rover Control Parameters
-param set-default RA_SPEED_P 0.1
+param set-default RO_ACCEL_LIM 3
-param set-default RA_WHEEL_BASE 0.321
+param set-default RO_DECEL_LIM 6
+param set-default RO_JERK_LIM 15
+param set-default RO_MAX_THR_SPEED 3.1
+
+# Rover Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.1
+param set-default RO_YAW_RATE_P 1
+param set-default RO_YAW_RATE_LIM 180
+
+# Rover Attitude Control Parameters
+param set-default RO_YAW_P 3
+
+# Rover Position Control Parameters
+param set-default RO_SPEED_LIM 3
+param set-default RO_SPEED_I 0.1
+param set-default RO_SPEED_P 1
 
 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1

ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho

@@ -14,24 +14,33 @@
 . ${R}etc/init.d/rc.rover_ackermann_defaults
 
 param set-default BAT1_N_CELLS 3
-
-# Rover parameters
 param set-default NAV_ACC_RAD 0.5
+
+# Ackermann Parameters
+param set-default RA_WHEEL_BASE 0.321
 param set-default RA_ACC_RAD_GAIN 2
 param set-default RA_ACC_RAD_MAX 3
-param set-default RA_LAT_ACCEL_I 0.01
-param set-default RA_LAT_ACCEL_P 0.1
-param set-default RA_MAX_ACCEL 1.5
-param set-default RA_MAX_DECEL 10
-param set-default RA_MAX_JERK 20
-param set-default RA_MAX_LAT_ACCEL 3
-param set-default RA_MAX_SPEED 2.5
 param set-default RA_MAX_STR_ANG 0.5236
-param set-default RA_MAX_STR_RATE 270
+param set-default RA_STR_RATE_LIM 270
-param set-default RA_MAX_THR_SPEED 2.8
+
-param set-default RA_SPEED_I 0.01
+# Rover Control Parameters
-param set-default RA_SPEED_P 0.1
+param set-default RO_ACCEL_LIM 1.5
-param set-default RA_WHEEL_BASE 0.321
+param set-default RO_DECEL_LIM 10
+param set-default RO_JERK_LIM 20
+param set-default RO_MAX_THR_SPEED 2.8
+
+# Rover Rate Control Parameters
+param set-default RO_YAW_RATE_I 0
+param set-default RO_YAW_RATE_P 0
+param set-default RO_YAW_RATE_LIM 0
+
+# Rover Attitude Control Parameters
+param set-default RO_YAW_P 0
+
+# Rover Position Control Parameters
+param set-default RO_SPEED_LIM 2.5
+param set-default RO_SPEED_I 0.01
+param set-default RO_SPEED_P 0.1
 
 # Pure pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1

boards/px4/sitl/spacecraft.px4board

@@ -11,7 +11,9 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
-CONFIG_MODULES_DIFFERENTIAL_DRIVE=n
+CONFIG_MODULES_ROVER_ACKERMANN=n
+CONFIG_MODULES_ROVER_DIFFERENTIAL=n
+CONFIG_MODULES_ROVER_MECANUM=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=n
 CONFIG_MODULES_SPACECRAFT=y

msg/CMakeLists.txt

@@ -171,9 +171,13 @@ set(msg_files
 	RateCtrlStatus.msg
 	RcChannels.msg
 	RcParameterMap.msg
-	RoverAckermannGuidanceStatus.msg
+	RoverAttitudeSetpoint.msg
-	RoverAckermannSetpoint.msg
+	RoverAttitudeStatus.msg
-	RoverAckermannStatus.msg
+	RoverVelocityStatus.msg
+	RoverRateSetpoint.msg
+	RoverRateStatus.msg
+	RoverSteeringSetpoint.msg
+	RoverThrottleSetpoint.msg
 	RoverDifferentialGuidanceStatus.msg
 	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg

msg/RoverAckermannGuidanceStatus.msg

@@ -1,6 +0,0 @@
-uint64 timestamp # time since system start (microseconds)
-
-float32 lookahead_distance 	# [m] Lookahead distance of pure the pursuit controller
-float32 heading_error 		# [deg] Heading error of the pure pursuit controller
-
-# TOPICS rover_ackermann_guidance_status

msg/RoverAckermannSetpoint.msg

@@ -1,9 +0,0 @@
-uint64 timestamp # time since system start (microseconds)
-
-float32 forward_speed_setpoint 		  # [m/s] Desired speed in body x direction. Positiv: forwards, Negativ: backwards
-float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized speed in body x direction. Positiv: forwards, Negativ: backwards
-float32 steering_setpoint      		  # [rad] Desired steering angle
-float32 steering_setpoint_normalized      # [-1, 1] Desired normalized steering angle
-float32 lateral_acceleration_setpoint     # [m/s^2] Desired acceleration in body y direction. Positiv: right, Negativ: left.
-
-# TOPICS rover_ackermann_setpoint

msg/RoverAckermannStatus.msg

@@ -1,11 +0,0 @@
-uint64 timestamp # time since system start (microseconds)
-
-float32 measured_forward_speed       	      # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ
-float32 adjusted_forward_speed_setpoint       # [m/s] Speed setpoint after applying slew rate
-float32 steering_setpoint_normalized          # [-1, 1] Normalized steering setpoint
-float32 adjusted_steering_setpoint_normalized # [-1, 1] Normalized steering setpoint after applying slew rate
-float32 measured_lateral_acceleration         # [m/s^2] Measured acceleration in body y direction. Positiv: right, Negativ: left.
-float32 pid_throttle_integral                 # Integral of the PID for the closed loop speed controller
-float32 pid_lat_accel_integral                # Integral of the PID for the closed loop lateral acceleration controller
-
-# TOPICS rover_ackermann_status

msg/RoverAttitudeSetpoint.msg

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

msg/RoverAttitudeStatus.msg

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

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

msg/RoverRateStatus.msg

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

@@ -0,0 +1,6 @@
+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)
+
+# TOPICS rover_steering_setpoint

msg/RoverThrottleSetpoint.msg

@@ -0,0 +1,7 @@
+
+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]
+
+# TOPICS rover_throttle_setpoint

msg/RoverVelocityStatus.msg

@@ -0,0 +1,12 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 measured_speed_body_x          # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 speed_body_x_setpoint          # [m/s] Speed setpoint in body x direction. Positiv: forwards, Negativ: backwards
+float32 adjusted_speed_body_x_setpoint # [m/s] Post slew rate speed setpoint in body x direction. Positiv: forwards, Negativ: backwards
+float32 measured_speed_body_y          # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
+float32 speed_body_y_setpoint          # [m/s] Speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers)
+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/lib/CMakeLists.txt

@@ -70,6 +70,7 @@ add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL)
 add_subdirectory(rate_control EXCLUDE_FROM_ALL)
 add_subdirectory(rc EXCLUDE_FROM_ALL)
 add_subdirectory(ringbuffer EXCLUDE_FROM_ALL)
+add_subdirectory(rover_control EXCLUDE_FROM_ALL)
 add_subdirectory(rtl EXCLUDE_FROM_ALL)
 add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL)
 add_subdirectory(slew_rate EXCLUDE_FROM_ALL)

src/lib/rover_control/CMakeLists.txt

@@ -31,8 +31,11 @@
 #
 ############################################################################
 
-px4_add_library(RoverAckermannGuidance
+px4_add_library(rover_control
-	RoverAckermannGuidance.cpp
+	RoverControl.cpp
+	RoverControl.hpp
 )
 
-target_include_directories(RoverAckermannGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(rover_control PUBLIC PID)
+target_link_libraries(rover_control PUBLIC geo)
+px4_add_unit_gtest(SRC RoverControlTest.cpp LINKLIBS rover_control)

src/lib/rover_control/RoverControl.cpp

@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ *   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 "RoverControl.hpp"
+using namespace matrix;
+namespace RoverControl
+{
+float throttleControl(SlewRate<float> &motor_setpoint, const float throttle_setpoint,
+		      const float current_motor_setpoint, const float max_accel, const float max_decel, const float max_thr_spd,
+		      const float dt)
+{
+	// Sanitize inputs
+	if (!PX4_ISFINITE(throttle_setpoint) || !PX4_ISFINITE(current_motor_setpoint) || !PX4_ISFINITE(dt)) {
+		return NAN;
+	}
+
+	const bool accelerating = fabsf(throttle_setpoint) > fabsf(current_motor_setpoint);
+
+	if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate
+		motor_setpoint.setSlewRate(max_accel / max_thr_spd);
+
+		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
+		if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint -
+				current_motor_setpoint)) {
+			motor_setpoint.setForcedValue(current_motor_setpoint);
+		}
+
+		motor_setpoint.update(throttle_setpoint, dt);
+
+	} else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate
+		motor_setpoint.setSlewRate(max_decel / max_thr_spd);
+
+		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
+		if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint -
+				current_motor_setpoint)) {
+			motor_setpoint.setForcedValue(current_motor_setpoint);
+		}
+
+		motor_setpoint.update(throttle_setpoint, dt);
+
+	} else { // Fallthrough if slew rate parameters are not configured
+		motor_setpoint.setForcedValue(throttle_setpoint);
+	}
+
+	return motor_setpoint.getState();
+}
+
+float attitudeControl(SlewRateYaw<float> &adjusted_yaw_setpoint, PID &pid_yaw,
+		      const float yaw_slew_rate, float vehicle_yaw, float yaw_setpoint, const float dt)
+{
+	// Sanitize inputs
+	if (!PX4_ISFINITE(yaw_setpoint) || !PX4_ISFINITE(vehicle_yaw) || !PX4_ISFINITE(dt)) {
+		return NAN;
+	}
+
+	yaw_setpoint = wrap_pi(yaw_setpoint);
+	vehicle_yaw = wrap_pi(vehicle_yaw);
+
+	if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured
+		adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate);
+
+		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
+		if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) {
+			adjusted_yaw_setpoint.setForcedValue(vehicle_yaw);
+		}
+
+		adjusted_yaw_setpoint.update(yaw_setpoint, dt);
+
+	} else {
+		adjusted_yaw_setpoint.setForcedValue(yaw_setpoint);
+	}
+
+	// Feedback control
+	pid_yaw.setSetpoint(
+		wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw));  // Error as setpoint to take care of wrapping
+	return pid_yaw.update(0.f, dt);
+
+}
+
+float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const float speed_setpoint,
+		   const float vehicle_speed, const float max_accel, const float max_decel, const float max_thr_speed, const float dt)
+{
+	// Sanitize inputs
+	if (!PX4_ISFINITE(speed_setpoint) || !PX4_ISFINITE(vehicle_speed) || !PX4_ISFINITE(dt)) {
+		return NAN;
+	}
+
+	// Apply acceleration and deceleration limit
+	if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) {
+		speed_with_rate_limit.setSlewRate(max_accel);
+
+		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
+		if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf(
+			    speed_setpoint - vehicle_speed)) {
+			speed_with_rate_limit.setForcedValue(vehicle_speed);
+		}
+
+		speed_with_rate_limit.update(speed_setpoint, dt);
+
+	} else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) {
+		speed_with_rate_limit.setSlewRate(max_decel);
+
+		// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
+		if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf(
+			    speed_setpoint - vehicle_speed)) {
+			speed_with_rate_limit.setForcedValue(vehicle_speed);
+		}
+
+		speed_with_rate_limit.update(speed_setpoint, dt);
+
+	} else { // Fallthrough if slew rate is not configured
+		speed_with_rate_limit.setForcedValue(speed_setpoint);
+	}
+
+	// Calculate normalized forward speed setpoint
+	float forward_speed_normalized{0.f};
+
+	// Feedforward
+	if (max_thr_speed > FLT_EPSILON) {
+		forward_speed_normalized = math::interpolate<float>(speed_with_rate_limit.getState(),
+					   -max_thr_speed, max_thr_speed,
+					   -1.f, 1.f);
+	}
+
+	// Feedback control
+	pid_speed.setSetpoint(speed_with_rate_limit.getState());
+	forward_speed_normalized += pid_speed.update(vehicle_speed, dt);
+
+	return math::constrain(forward_speed_normalized, -1.f, 1.f);
+}
+
+void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
+				  position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
+				  MapProjection &global_ned_proj_ref)
+{
+	if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
+		curr_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
+
+	} else {
+		curr_wp_ned = curr_pos_ned.isAllFinite() ? curr_pos_ned : Vector2f(NAN, NAN); // Fallback if current waypoint is invalid
+	}
+
+	if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) {
+		prev_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.previous.lat,
+				position_setpoint_triplet.previous.lon);
+
+	} else {
+		prev_wp_ned = curr_pos_ned.isAllFinite() ? curr_pos_ned : Vector2f(NAN,
+				NAN); // Fallback if previous waypoint is invalid
+	}
+
+	if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.next.lon)) {
+		next_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
+
+	} else {
+		next_wp_ned = home_pos.isAllFinite() ? global_ned_proj_ref.project(home_pos(0), home_pos(1)) : Vector2f(NAN,
+				NAN); // Enables corner slow down with RTL
+	}
+}
+
+float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, Vector2f &next_wp_ned)
+{
+	// Sanitize inputs
+	if (!prev_wp_ned.isAllFinite() || !curr_wp_ned.isAllFinite() || !next_wp_ned.isAllFinite()) {
+		return NAN;
+	}
+
+	const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned;
+	const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned;
+
+	// Waypoint overlap
+	if (curr_to_next_wp_ned.norm() < FLT_EPSILON || curr_to_prev_wp_ned.norm() < FLT_EPSILON) {
+		return NAN;
+	}
+
+	float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
+	cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
+	return acosf(cosin);
+}
+
+} // RoverControl

src/lib/rover_control/RoverControl.hpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RoverControl.hpp
+ *
+ * Functions that are shared among the different rover modules.
+ * Also includes the parameters that are shared among the rover modules.
+ */
+
+#pragma once
+#include <lib/slew_rate/SlewRate.hpp>
+#include <lib/slew_rate/SlewRateYaw.hpp>
+#include <lib/pid/PID.hpp>
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <lib/geo/geo.h>
+
+using namespace matrix;
+namespace RoverControl
+{
+/**
+ * Applies acceleration/deceleration slew rate to a throttle setpoint.
+ * @param motor_setpoint Throttle setpoint with applied slew rate [-1, 1] (Updated by this function)
+ * @param throttle_setpoint Throttle setpoint pre slew rate [-1, 1]
+ * @param current_motor_setpoint Currently applied motor input [-1, 1]
+ * @param max_accel Maximum allowed acceleration [m/s^2]
+ * @param max_decel Maximum allowed deceleration [m/s^2]
+ * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]
+ * @param dt Time since last update [s]
+ * @return Motor Setpoint [-1, 1]
+ */
+float throttleControl(SlewRate<float> &motor_setpoint, float throttle_setpoint, float current_motor_setpoint,
+		      float max_accel, float max_decel, float max_thr_spd, float dt);
+
+/**
+ * Applies yaw rate slew rate to a yaw setpoint and calculates the necessary yaw rate setpoint
+ * using a PID controller.
+ * @param adjusted_yaw_setpoint Yaw setpoint with applied slew rate [-1, 1] (Updated by this function)
+ * @param pid_yaw Yaw PID (Updated by this function)
+ * @param yaw_slew_rate Yaw slew rate [rad/s]
+ * @param vehicle_yaw Measured vehicle yaw [rad]
+ * @param yaw_setpoint Yaw setpoint [rad]
+ * @param dt Time since last update [s]
+ * @return Yaw rate setpoint [rad/s]
+ */
+float attitudeControl(SlewRateYaw<float> &adjusted_yaw_setpoint, PID &pid_yaw, float yaw_slew_rate,
+		      float vehicle_yaw, float yaw_setpoint, float dt);
+/**
+ * Applies acceleration/deceleration slew rate to a speed setpoint and calculates the necessary throttle setpoint
+ * using a feed forward term and PID controller.
+ * @param speed_with_rate_limit Speed setpoint with applied slew rate [-1, 1] (Updated by this function)
+ * @param pid_speed Speed PID (Updated by this function)
+ * @param speed_setpoint Speed setpoint [m/s]
+ * @param vehicle_speed Measured vehicle speed [m/s]
+ * @param max_accel Maximum allowed acceleration [m/s^2]
+ * @param max_decel Maximum allowed deceleration [m/s^2]
+ * @param max_thr_speed Speed at maximum throttle [m/s]
+ * @param dt Time since last update [s]
+ * @return Throttle setpoint [-1, 1]
+ */
+float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float speed_setpoint,
+		   float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt);
+
+/**
+ * Projects positionSetpointTriplet waypoints from global to ned frame.
+ * @param curr_wp_ned Current waypoint in NED frame (Updated by this function)
+ * @param prev_wp_ned Previous waypoint in NED frame (Updated by this function)
+ * @param next_wp_ned Next waypoint in NED frame (Updated by this function)
+ * @param position_setpoint_triplet Position Setpoint Triplet
+ * @param curr_pos Current position of the rover in global frame
+ * @param home_pos Home position in global frame
+ * @param global_ned_proj_ref Global to ned projection
+ */
+void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
+				  position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
+				  MapProjection &global_ned_proj_ref);
+
+/**
+ * Calculate and return the angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+ * @param prev_wp_ned Previous waypoint in NED frame
+ * @param curr_wp_ned Current waypoint in NED frame
+ * @param next_wp_ned Next waypoint in NED frame
+ * @return Waypoint transition angle [rad]
+ */
+float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, Vector2f &next_wp_ned);
+
+}

src/lib/rover_control/RoverControlTest.cpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/******************************************************************
+ * Test code for the Pure Pursuit algorithm
+ * Run this test only using "make tests TESTFILTER=RoverControl"
+******************************************************************/
+
+#include <gtest/gtest.h>
+#include "RoverControl.hpp"
+
+TEST(calcWaypointTransitionAngle, invalidInputs)
+{
+	Vector2f prev_wp_ned(NAN, NAN);
+	Vector2f curr_wp_ned(10.f, 10.f);
+	Vector2f next_wp_ned(10.f, 10.f);
+	float prevInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	prev_wp_ned = Vector2f(10.f, 10.f);
+	curr_wp_ned = Vector2f(NAN, NAN);
+	float currInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	curr_wp_ned = Vector2f(10.f, 10.f);
+	next_wp_ned = Vector2f(NAN, NAN);
+	float nextInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	EXPECT_FALSE(PX4_ISFINITE(prevInvalid));
+	EXPECT_FALSE(PX4_ISFINITE(currInvalid));
+	EXPECT_FALSE(PX4_ISFINITE(nextInvalid));
+
+}
+
+TEST(calcWaypointTransitionAngle, validInputs)
+{
+	// P -- C -- N
+	Vector2f prev_wp_ned(0.f, 0.f);
+	Vector2f curr_wp_ned(10.f, 0.f);
+	Vector2f next_wp_ned(20.f, 0.f);
+	const float angle1 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	EXPECT_FLOAT_EQ(angle1, M_PI_F);
+
+	/**
+	 *	    N
+	 *	  /
+	 * P -- C
+	 */
+	prev_wp_ned = Vector2f(0.f, 0.f);
+	curr_wp_ned = Vector2f(10.f, 0.f);
+	next_wp_ned = Vector2f(20.f, 10.f);
+	const float angle2 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	EXPECT_FLOAT_EQ(angle2, M_PI_F - M_PI_4_F);
+
+	/**
+	 *	N
+	 *	|
+	 * P -- C
+	 */
+	prev_wp_ned = Vector2f(0.f, 0.f);
+	curr_wp_ned = Vector2f(10.f, 0.f);
+	next_wp_ned = Vector2f(10.f, 10.f);
+	const float angle3 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	EXPECT_FLOAT_EQ(angle3, M_PI_2_F);
+
+	/**
+	 * N
+	 *    \
+	 * P -- C
+	 */
+	prev_wp_ned = Vector2f(0.f, 0.f);
+	curr_wp_ned = Vector2f(10.f, 0.f);
+	next_wp_ned = Vector2f(0.f, 10.f);
+	const float angle4 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	EXPECT_FLOAT_EQ(angle4, M_PI_4_F);
+
+	// P/C -- N
+	prev_wp_ned = Vector2f(0.f, 0.f);
+	curr_wp_ned = Vector2f(0.f, 0.f);
+	next_wp_ned = Vector2f(10.f, 0.f);
+	const float angle5 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	EXPECT_FALSE(PX4_ISFINITE(angle5));
+
+	// P -- C/N
+	prev_wp_ned = Vector2f(0.f, 0.f);
+	curr_wp_ned = Vector2f(10.f, 0.f);
+	next_wp_ned = Vector2f(10.f, 0.f);
+	const float angle6 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned);
+	EXPECT_FALSE(PX4_ISFINITE(angle6));
+}

src/lib/rover_control/module.yaml

@@ -0,0 +1,191 @@
+module_name: Rover Control
+
+parameters:
+    - group: Rover Control
+      definitions:
+        RO_MAX_THR_SPEED:
+          description:
+            short: Speed the rover drives at maximum throttle
+            long: Used to linearly map speeds [m/s] to throttle values [-1. 1].
+          type: float
+          unit: m/s
+          min: 0
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: 0
+
+        RO_ACCEL_LIM:
+          description:
+            short: Acceleration limit
+            long: |
+              Set to -1 to disable.
+              For mecanum rovers this limit is used for longitudinal and lateral acceleration.
+          type: float
+          unit: m/s^2
+          min: -1
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: -1
+
+        RO_DECEL_LIM:
+          description:
+            short: Deceleration limit
+            long: |
+              Set to -1 to disable.
+              Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes.
+              For mecanum rovers this limit is used for longitudinal and lateral deceleration.
+          type: float
+          unit: m/s^2
+          min: -1
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: -1
+
+        RO_JERK_LIM:
+          description:
+            short: Jerk limit
+            long: |
+              Set to -1 to disable.
+              Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes.
+              For mecanum rovers this limit is used for longitudinal and lateral jerk.
+          type: float
+          unit: m/s^3
+          min: -1
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: -1
+
+        RO_YAW_RATE_TH:
+          description:
+            short: Yaw rate measurement threshold
+            long: The minimum threshold for the yaw rate measurement not to be interpreted as zero.
+          type: float
+          unit: deg/s
+          min: 0
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: 3
+
+        RO_SPEED_TH:
+          description:
+            short: Speed measurement threshold
+            long: The minimum threshold for the speed measurement not to be interpreted as zero.
+          type: float
+          unit: m/s
+          min: 0
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: 0.1
+
+        RO_YAW_STICK_DZ:
+          description:
+            short: Yaw stick deadzone
+            long: Percentage of stick input range that will be interpreted as zero around the stick centered value.
+          type: float
+          min: 0
+          max: 1
+          increment: 0.01
+          decimal: 2
+          default: 0.1
+
+    - group: Rover Rate Control
+      definitions:
+        RO_YAW_RATE_P:
+          description:
+            short: Proportional gain for closed loop yaw rate controller
+          type: float
+          min: 0
+          max: 100
+          increment: 0.01
+          decimal: 3
+          default: 0
+
+        RO_YAW_RATE_I:
+          description:
+            short: Integral gain for closed loop yaw rate controller
+          type: float
+          min: 0
+          max: 100
+          increment: 0.01
+          decimal: 3
+          default: 0
+
+        RO_YAW_RATE_LIM:
+          description:
+            short: Yaw rate limit
+            long: |
+              Used to cap yaw rate setpoints and map controller inputs to yaw rate setpoints
+              in Acro, Stabilized and Position mode.
+          type: float
+          unit: deg/s
+          min: 0
+          max: 10000
+          increment: 0.01
+          decimal: 2
+          default: 0
+
+        RO_YAW_ACCEL_LIM:
+          description:
+            short: Yaw acceleration limit
+            long: Set to -1 to disable.
+          type: float
+          unit: deg/s^2
+          min: -1
+          max: 10000
+          increment: 0.01
+          decimal: 2
+          default: -1
+
+    - group: Rover Attitude Control
+      definitions:
+        RO_YAW_P:
+          description:
+            short: Proportional gain for closed loop yaw controller
+          type: float
+          min: 0
+          max: 100
+          increment: 0.01
+          decimal: 3
+          default: 0
+
+    - group: Rover Velocity Control
+      definitions:
+        RO_SPEED_P:
+          description:
+            short: Proportional gain for ground speed controller
+          type: float
+          min: 0
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: 0
+
+        RO_SPEED_I:
+          description:
+            short: Integral gain for ground speed controller
+          type: float
+          min: 0
+          max: 100
+          increment: 0.001
+          decimal: 3
+          default: 0
+
+        RO_SPEED_LIM:
+          description:
+            short: Speed limit
+            long: |
+              Used to cap speed setpoints and map controller inputs to speed setpoints
+              in Position mode.
+          type: float
+          unit: m/s
+          min: -1
+          max: 100
+          increment: 0.01
+          decimal: 2
+          default: -1

src/modules/logger/logged_topics.cpp

@@ -102,9 +102,13 @@ void LoggedTopics::add_default_topics()
 	add_topic("position_setpoint_triplet", 200);
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
-	add_optional_topic("rover_ackermann_guidance_status", 100);
+	add_optional_topic("rover_attitude_setpoint", 100);
-	add_optional_topic("rover_ackermann_setpoint", 100);
+	add_optional_topic("rover_attitude_status", 100);
-	add_optional_topic("rover_ackermann_status", 100);
+	add_optional_topic("rover_velocity_status", 100);
+	add_optional_topic("rover_rate_setpoint", 100);
+	add_optional_topic("rover_rate_status", 100);
+	add_optional_topic("rover_steering_setpoint", 100);
+	add_optional_topic("rover_throttle_setpoint", 100);
 	add_optional_topic("rover_differential_guidance_status", 100);
 	add_optional_topic("rover_differential_setpoint", 100);
 	add_optional_topic("rover_differential_status", 100);

src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp

@@ -0,0 +1,205 @@
+/****************************************************************************
+ *
+ *   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 "AckermannAttControl.hpp"
+
+using namespace time_literals;
+
+AckermannAttControl::AckermannAttControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_rover_rate_setpoint_pub.advertise();
+	_rover_throttle_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
+	_rover_attitude_status_pub.advertise();
+	updateParams();
+}
+
+void AckermannAttControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) {
+		_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
+	}
+
+	_pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f);
+	_pid_yaw.setIntegralLimit(0.f);
+	_pid_yaw.setOutputLimit(_max_yaw_rate);
+	_adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate);
+}
+
+void AckermannAttControl::updateAttControl()
+{
+	hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	if (_vehicle_control_mode_sub.updated()) {
+		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
+	}
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+		matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
+	}
+
+	if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
+		// Estimate forward speed based on throttle
+		if (_actuator_motors_sub.updated()) {
+			actuator_motors_s actuator_motors;
+			_actuator_motors_sub.copy(&actuator_motors);
+			_estimated_speed_body_x = math::interpolate<float> (actuator_motors.control[0], -1.f, 1.f,
+						  -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
+		}
+
+		if (_vehicle_control_mode.flag_control_manual_enabled) {
+			generateAttitudeSetpoint();
+		}
+
+		generateRateSetpoint();
+
+	} else { // Reset pid and slew rate when attitude control is not active
+		_pid_yaw.resetIntegral();
+		_adjusted_yaw_setpoint.setForcedValue(0.f);
+	}
+
+	// Publish attitude controller status (logging only)
+	rover_attitude_status_s rover_attitude_status;
+	rover_attitude_status.timestamp = _timestamp;
+	rover_attitude_status.measured_yaw = _vehicle_yaw;
+	rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState();
+	_rover_attitude_status_pub.publish(rover_attitude_status);
+
+}
+
+void AckermannAttControl::generateAttitudeSetpoint()
+{
+	const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
+				       && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
+
+	if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode
+		manual_control_setpoint_s manual_control_setpoint{};
+
+		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+
+			rover_throttle_setpoint_s rover_throttle_setpoint{};
+			rover_throttle_setpoint.timestamp = _timestamp;
+			rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
+			rover_throttle_setpoint.throttle_body_y = 0.f;
+			_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+
+			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(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control
+				_stab_yaw_ctl = false;
+				_adjusted_yaw_setpoint.setForcedValue(0.f);
+				rover_rate_setpoint_s rover_rate_setpoint{};
+				rover_rate_setpoint.timestamp = _timestamp;
+				rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * yaw_rate_setpoint;
+				_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+
+			} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
+				if (!_stab_yaw_ctl) {
+					_stab_yaw_setpoint = _vehicle_yaw;
+					_stab_yaw_ctl = true;
+				}
+
+				rover_attitude_setpoint_s rover_attitude_setpoint{};
+				rover_attitude_setpoint.timestamp = _timestamp;
+				rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
+				_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+			}
+
+
+		}
+
+	}
+}
+
+void AckermannAttControl::generateRateSetpoint()
+{
+	if (_rover_attitude_setpoint_sub.updated()) {
+		_rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint);
+	}
+
+	if (_rover_rate_setpoint_sub.updated()) {
+		_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
+	}
+
+	// Check if a new rate setpoint was already published from somewhere else
+	if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update
+	    && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) {
+		return;
+	}
+
+	// Calculate yaw rate limit for slew rate
+	float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) /
+				      _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
+	float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate);
+
+	float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate,
+				  _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt);
+
+	_last_rate_setpoint_update = _timestamp;
+	rover_rate_setpoint_s rover_rate_setpoint{};
+	rover_rate_setpoint.timestamp = _timestamp;
+	rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
+	_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+}
+
+bool AckermannAttControl::runSanityChecks()
+{
+	bool ret = true;
+
+	if (_param_ro_max_thr_speed.get() < FLT_EPSILON) {
+		ret = false;
+	}
+
+	if (_param_ra_wheel_base.get() < FLT_EPSILON) {
+		ret = false;
+	}
+
+	if (_param_ra_max_str_ang.get() < FLT_EPSILON) {
+		ret = false;
+	}
+
+	if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
+		ret = false;
+	}
+
+	return ret;
+}

src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp

@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   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>
+
+// Library includes
+#include <lib/rover_control/RoverControl.hpp>
+#include <lib/pid/PID.hpp>
+#include <lib/slew_rate/SlewRateYaw.hpp>
+#include <math.h>
+#include <matrix/matrix/math.hpp>
+
+// 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/vehicle_control_mode.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/rover_attitude_status.h>
+#include <uORB/topics/rover_attitude_setpoint.h>
+#include <uORB/topics/actuator_motors.h>
+
+/**
+ * @brief Class for ackermann attitude control.
+ */
+class AckermannAttControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for AckermannAttControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	AckermannAttControl(ModuleParams *parent);
+	~AckermannAttControl() = default;
+
+	/**
+	 * @brief Update attitude controller.
+	 */
+	void updateAttControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode).
+	 */
+	void generateAttitudeSetpoint();
+
+	/**
+	 * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
+	 */
+	void generateRateSetpoint();
+
+	/**
+	 * @brief Check if the necessary parameters are set.
+	 * @return True if all checks pass.
+	 */
+	bool runSanityChecks();
+
+	// uORB subscriptions
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
+	uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
+	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
+	vehicle_control_mode_s _vehicle_control_mode{};
+	rover_attitude_setpoint_s _rover_attitude_setpoint{};
+	rover_rate_setpoint_s _rover_rate_setpoint{};
+
+	// 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_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
+
+	// Variables
+	float _vehicle_yaw{0.f};
+	hrt_abstime _timestamp{0};
+	hrt_abstime _last_rate_setpoint_update{0};
+	float _dt{0.f};
+	float _max_yaw_rate{0.f};
+	float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint,  _estimated_speed_body_x]
+					       between [0, 0] and [1, _param_ro_max_thr_speed].*/
+	float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode
+	bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode
+
+	// Controllers
+	PID _pid_yaw;
+	SlewRateYaw<float> _adjusted_yaw_setpoint;
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
+		(ParamFloat<px4::params::RA_WHEEL_BASE>)    _param_ra_wheel_base,
+		(ParamFloat<px4::params::RA_MAX_STR_ANG>)   _param_ra_max_str_ang,
+		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
+		(ParamFloat<px4::params::RO_YAW_P>)         _param_ro_yaw_p,
+		(ParamFloat<px4::params::RO_YAW_STICK_DZ>)  _param_ro_yaw_stick_dz
+	)
+};

src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#   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
@@ -31,9 +31,9 @@
 #
 ############################################################################
 
-px4_add_library(RoverAckermannControl
+px4_add_library(AckermannAttControl
-	RoverAckermannControl.cpp
+	AckermannAttControl.cpp
 )
 
-target_link_libraries(RoverAckermannControl PUBLIC PID)
+target_link_libraries(AckermannAttControl PUBLIC PID)
-target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_include_directories(AckermannAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp

@@ -0,0 +1,418 @@
+/****************************************************************************
+ *
+ *   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 "AckermannPosVelControl.hpp"
+
+using namespace time_literals;
+
+AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_rover_rate_setpoint_pub.advertise();
+	_rover_throttle_setpoint_pub.advertise();
+	_rover_attitude_setpoint_pub.advertise();
+	_rover_velocity_status_pub.advertise();
+	updateParams();
+}
+
+void AckermannPosVelControl::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()
+{
+	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();
+		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);
+
+	} 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()
+{
+	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);
+		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;
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+		_vehicle_status_sub.copy(&vehicle_status);
+		_nav_state = vehicle_status.nav_state;
+	}
+
+}
+
+void AckermannPosVelControl::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 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();
+	}
+}
+
+void AckermannPosVelControl::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 = matrix::sign(_vehicle_speed_body_x) * 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 float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
+								2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
+			const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) *
+							     vector_scaling * _pos_ctl_course_direction;
+			float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
+					     _curr_pos_ned, fabsf(_speed_body_x_setpoint));
+			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 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());
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = _timestamp;
+		rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
+						       _curr_pos_ned, fabsf(_speed_body_x_setpoint));
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
+	} else {
+		_speed_body_x_setpoint = 0.f;
+	}
+}
+
+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()
+{
+	updateAutoSubscriptions();
+
+	// Distances to waypoints
+	const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
+					       2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2));
+	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));
+
+	if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival
+		_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
+	}
+
+	if (_mission_finished) {
+		_speed_body_x_setpoint = 0.f;
+
+	} 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(), _nav_state,
+					 _waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get());
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = _timestamp;
+		rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned,
+						       _curr_pos_ned, fabsf(_speed_body_x_setpoint));
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+	}
+}
+
+void AckermannPosVelControl::updateAutoSubscriptions()
+{
+	if (_home_position_sub.updated()) {
+		home_position_s home_position{};
+		_home_position_sub.copy(&home_position);
+		_home_position = Vector2d(home_position.lat, home_position.lon);
+	}
+
+	if (_position_setpoint_triplet_sub.updated()) {
+		updateWaypointsAndAcceptanceRadius();
+	}
+
+	if (_mission_result_sub.updated()) {
+		mission_result_s mission_result{};
+		_mission_result_sub.copy(&mission_result);
+		_mission_finished = mission_result.finished;
+	}
+}
+
+void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius()
+{
+	position_setpoint_triplet_s position_setpoint_triplet{};
+	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+
+	RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
+			_curr_pos_ned, _home_position, _global_ned_proj_ref);
+
+	_prev_waypoint_transition_angle = _waypoint_transition_angle;
+	_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
+
+	// Update acceptance radius
+	_prev_acceptance_radius = _acceptance_radius;
+
+	if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
+		_acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
+				     _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get());
+
+	} else {
+		_acceptance_radius = _param_nav_acc_rad.get();
+	}
+
+	// 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();
+}
+
+float AckermannPosVelControl::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)
+{
+	// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
+	float acceptance_radius = default_acceptance_radius;
+	const float theta = waypoint_transition_angle / 2.f;
+	const float min_turning_radius = wheel_base / sinf(max_steer_angle);
+	const float acceptance_radius_temp = min_turning_radius / tanf(theta);
+	const float acceptance_radius_temp_scaled = acceptance_radius_gain *
+			acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
+	acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
+			    acceptance_radius_max);
+
+	// Publish updated acceptance radius
+	position_controller_status_s pos_ctrl_status{};
+	pos_ctrl_status.acceptance_radius = acceptance_radius;
+	pos_ctrl_status.timestamp = _timestamp;
+	_position_controller_status_pub.publish(pos_ctrl_status);
+	return acceptance_radius;
+}
+
+float AckermannPosVelControl::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 nav_state,
+		const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed)
+{
+	// Catch improper values
+	if (miss_speed_min < -FLT_EPSILON  || miss_speed_min > cruising_speed) {
+		return cruising_speed;
+	}
+
+	// Cornering slow down effect
+	if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) {
+		const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f);
+		const float cornering_speed = _max_yaw_rate * turning_circle;
+		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
+
+	} else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) {
+		const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
+		const float cornering_speed = _max_yaw_rate * turning_circle;
+		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
+
+	}
+
+	// Straight line speed
+	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
+		float straight_line_speed{0.f};
+
+		if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+			straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
+					      max_decel, distance_to_curr_wp, 0.f);
+
+		} else {
+			const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
+			float cornering_speed = _max_yaw_rate * turning_circle;
+			cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed);
+			straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
+					      max_decel, distance_to_curr_wp - acc_rad, cornering_speed);
+		}
+
+		return math::min(straight_line_speed, cruising_speed);
+
+	} else {
+		return cruising_speed;
+	}
+
+}
+
+bool AckermannPosVelControl::runSanityChecks()
+{
+	bool ret = true;
+
+	if (_param_ro_max_thr_speed.get() < FLT_EPSILON) {
+		ret = false;
+	}
+
+	if (_param_ra_wheel_base.get() < FLT_EPSILON) {
+		ret = false;
+	}
+
+	if (_param_ra_max_str_ang.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_posVel_control_conf_invalid_speed_lim"), events::Log::Error,
+					    "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_yaw_rate_limit.get());
+		}
+
+	}
+
+	_prev_param_check_passed = ret;
+	return ret;
+}

src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp

@@ -0,0 +1,257 @@
+/****************************************************************************
+ *
+ *   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 <lib/pure_pursuit/PurePursuit.hpp>
+#include <lib/geo/geo.h>
+#include <math.h>
+
+// 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/vehicle_control_mode.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/offboard_control_mode.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/home_position.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for ackermann position control.
+ */
+class AckermannPosVelControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for AckermannPosVelControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	AckermannPosVelControl(ModuleParams *parent);
+	~AckermannPosVelControl() = default;
+
+	/**
+	 * @brief Update position controller.
+	 */
+	void updatePosVelControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Update uORB subscriptions used in position controller.
+	 */
+	void updateSubscriptions();
+
+	/**
+	 * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or
+	 * 	  trajcetorySetpoint (Offboard position or velocity control) or
+	 * 	  positionSetpointTriplet (Auto Mode).
+	 */
+	void generateAttitudeSetpoint();
+
+	/**
+	 * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint 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.
+	 */
+	void autoPositionMode();
+
+	/**
+	 * @brief Update uORB subscriptions used for auto modes.
+	 */
+	void updateAutoSubscriptions();
+
+	/**
+	 * @brief Update global/NED waypoint coordinates and acceptance radius.
+	 */
+	void updateWaypointsAndAcceptanceRadius();
+
+	/**
+	 * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment
+	 * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle.
+	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	 * @param default_acceptance_radius Default acceptance radius for waypoints [m].
+	 * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-].
+	 * @param acceptance_radius_max Maximum value for the acceptance radius [m].
+	 * @param wheel_base Rover wheelbase [m].
+	 * @param max_steer_angle Rover maximum steer angle [rad].
+	 * @return Updated acceptance radius [m].
+	 */
+	float updateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius,
+				     float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle);
+
+	/**
+	 * @brief Calculate the speed setpoint. During cornering the speed is restricted based on the radius of the corner.
+	 * On straight lines it is based on a speed trajectory such that the rover will arrive at the next corner with the
+	 * desired cornering speed under consideration of the maximum deceleration and jerk.
+	 * @param cruising_speed Cruising speed [m/s].
+	 * @param miss_speed_min Minimum speed setpoint [m/s].
+	 * @param distance_to_prev_wp Distance to the previous waypoint [m].
+	 * @param distance_to_curr_wp Distance to the current waypoint [m].
+	 * @param acc_rad Acceptance radius of the current waypoint [m].
+	 * @param prev_acc_rad Acceptance radius of the previous waypoint [m].
+	 * @param max_decel Maximum allowed deceleration [m/s^2].
+	 * @param max_jerk Maximum allowed jerk [m/s^3].
+	 * @param nav_state Current nav_state of the rover.
+	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	 * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	 * @param max_speed Maximum speed setpoint [m/s]
+	 * @return Speed setpoint [m/s].
+	 */
+	float calcSpeedSetpoint(float cruising_speed, float miss_speed_min, float distance_to_prev_wp,
+				float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int nav_state,
+				float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed);
+
+	/**
+	 * @brief Check if the necessary parameters are set.
+	 * @return True if all checks pass.
+	 */
+	bool runSanityChecks();
+
+	// uORB subscriptions
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+	vehicle_control_mode_s _vehicle_control_mode{};
+	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)};
+
+	// Variables
+	hrt_abstime _timestamp{0};
+	Quatf _vehicle_attitude_quaternion{};
+	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 _nav_state{0};
+	bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode.
+	bool _mission_finished{false};
+	bool _prev_param_check_passed{true};
+
+	// Waypoint variables
+	Vector2d _home_position{};
+	Vector2f _curr_wp_ned{};
+	Vector2f _prev_wp_ned{};
+	Vector2f _next_wp_ned{};
+	float _acceptance_radius{0.5f};
+	float _prev_acceptance_radius{0.5f};
+	float _cruising_speed{0.f};
+	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
+	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
+	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,
+		(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::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max,
+		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
+		(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,
+		(ParamFloat<px4::params::RA_WHEEL_BASE>)    _param_ra_wheel_base,
+		(ParamFloat<px4::params::NAV_ACC_RAD>)      _param_nav_acc_rad
+
+	)
+};

src/modules/rover_ackermann/AckermannPosVelControl/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(AckermannPosVelControl
+	AckermannPosVelControl.cpp
+)
+
+target_link_libraries(AckermannPosVelControl PUBLIC PID)
+target_include_directories(AckermannPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp

@@ -0,0 +1,223 @@
+/****************************************************************************
+ *
+ *   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 "AckermannRateControl.hpp"
+
+using namespace time_literals;
+
+AckermannRateControl::AckermannRateControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	_rover_rate_setpoint_pub.advertise();
+	_rover_throttle_setpoint_pub.advertise();
+	_rover_steering_setpoint_pub.advertise();
+	_rover_rate_status_pub.advertise();
+	updateParams();
+}
+
+void AckermannRateControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
+	_pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f);
+	_pid_yaw_rate.setIntegralLimit(1.f);
+	_pid_yaw_rate.setOutputLimit(1.f);
+	_yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F);
+}
+
+void AckermannRateControl::updateRateControl()
+{
+	hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	if (_vehicle_control_mode_sub.updated()) {
+		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
+	}
+
+	if (_vehicle_angular_velocity_sub.updated()) {
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
+		_vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ?
+				    vehicle_angular_velocity.xyz[2] : 0.f;
+	}
+
+	if (_vehicle_control_mode.flag_control_rates_enabled  && _vehicle_control_mode.flag_armed && runSanityChecks()) {
+		// Estimate forward speed based on throttle
+		if (_actuator_motors_sub.updated()) {
+			actuator_motors_s actuator_motors;
+			_actuator_motors_sub.copy(&actuator_motors);
+			_estimated_speed_body_x = math::interpolate<float>(actuator_motors.control[0], -1.f, 1.f,
+						  -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
+			_estimated_speed_body_x = fabsf(_estimated_speed_body_x) >  _param_ro_speed_th.get() ? _estimated_speed_body_x : 0.f;
+		}
+
+		if (_vehicle_control_mode.flag_control_manual_enabled) {
+			generateRateSetpoint();
+		}
+
+		generateSteeringSetpoint();
+
+	} else { // Reset controller and slew rate when rate control is not active
+		_pid_yaw_rate.resetIntegral();
+		_yaw_rate_setpoint.setForcedValue(0.f);
+	}
+
+	// Publish rate controller status (logging only)
+	rover_rate_status_s rover_rate_status;
+	rover_rate_status.timestamp = _timestamp;
+	rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate;
+	rover_rate_status.adjusted_yaw_rate_setpoint = _yaw_rate_setpoint.getState();
+	rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
+	_rover_rate_status_pub.publish(rover_rate_status);
+
+}
+
+void AckermannRateControl::generateRateSetpoint()
+{
+	const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
+				       && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled;
+
+	if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode
+		manual_control_setpoint_s manual_control_setpoint{};
+
+		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+			rover_throttle_setpoint_s rover_throttle_setpoint{};
+			rover_throttle_setpoint.timestamp = _timestamp;
+			rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
+			rover_throttle_setpoint.throttle_body_y = 0.f;
+			_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
+			rover_rate_setpoint_s rover_rate_setpoint{};
+			rover_rate_setpoint.timestamp = _timestamp;
+			rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * math::interpolate<float>
+								(manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+		}
+
+	}
+}
+
+void AckermannRateControl::generateSteeringSetpoint()
+{
+	if (_rover_rate_setpoint_sub.updated()) {
+		_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
+
+	}
+
+	float steering_setpoint{0.f};
+
+	if (fabsf(_estimated_speed_body_x) > FLT_EPSILON) {
+		// Set up feasible yaw rate setpoint
+		float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) /
+					      _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
+		float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate);
+		float constrained_yaw_rate = math::constrain(_rover_rate_setpoint.yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit);
+
+		if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured
+			if (fabsf(_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate -
+					_vehicle_yaw_rate)) {
+				_yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate);
+			}
+
+			_yaw_rate_setpoint.update(constrained_yaw_rate, _dt);
+
+		} else {
+			_yaw_rate_setpoint.setForcedValue(constrained_yaw_rate);
+		}
+
+		// Feed forward
+		steering_setpoint = atanf(_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed_body_x);
+
+		// Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability)
+		if (_estimated_speed_body_x > FLT_EPSILON) {
+			_pid_yaw_rate.setSetpoint(_yaw_rate_setpoint.getState());
+			steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, _dt);
+		}
+
+	} else {
+		_pid_yaw_rate.resetIntegral();
+	}
+
+	rover_steering_setpoint_s rover_steering_setpoint{};
+	rover_steering_setpoint.timestamp = _timestamp;
+	rover_steering_setpoint.normalized_steering_angle = math::interpolate<float>(steering_setpoint,
+			-_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint
+	_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
+}
+
+bool AckermannRateControl::runSanityChecks()
+{
+	bool ret = true;
+
+	if (_param_ro_max_thr_speed.get() < FLT_EPSILON) {
+		ret = false;
+
+		if (_prev_param_check_passed) {
+			events::send<float>(events::ID("ackermann_rate_control_conf_invalid_max_thr_speed"), events::Log::Error,
+					    "Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get());
+		}
+
+	}
+
+	if (_param_ra_wheel_base.get() < FLT_EPSILON) {
+		ret = false;
+
+		if (_prev_param_check_passed) {
+			events::send<float>(events::ID("ackermann_rate_control_conf_invalid_wheel_base"), events::Log::Error,
+					    "Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get());
+		}
+
+	}
+
+	if (_param_ra_max_str_ang.get() < FLT_EPSILON) {
+		ret = false;
+
+		if (_prev_param_check_passed) {
+			events::send<float>(events::ID("ackermann_rate_control_conf_invalid_max_str_ang"), events::Log::Error,
+					    "Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get());
+		}
+
+	}
+
+	if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
+		ret = false;
+
+		if (_prev_param_check_passed) {
+			events::send<float>(events::ID("ackermann_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error,
+					    "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get());
+		}
+
+	}
+
+	_prev_param_check_passed = ret;
+	return ret;
+}

src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp

@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   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 <lib/slew_rate/SlewRate.hpp>
+#include <math.h>
+
+// 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/vehicle_control_mode.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/rover_steering_setpoint.h>
+#include <uORB/topics/rover_rate_status.h>
+#include <uORB/topics/actuator_motors.h>
+
+/**
+ * @brief Class for ackermann rate control.
+ */
+class AckermannRateControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for AckermannRateControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	AckermannRateControl(ModuleParams *parent);
+	~AckermannRateControl() = default;
+
+	/**
+	 * @brief Update rate controller.
+	 */
+	void updateRateControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+
+	/**
+	 * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode).
+	 */
+	void generateRateSetpoint();
+
+	/**
+	 * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.
+	 */
+	void generateSteeringSetpoint();
+
+	/**
+	 * @brief Check if the necessary parameters are set.
+	 * @return True if all checks pass.
+	 */
+	bool runSanityChecks();
+
+	// uORB subscriptions
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
+	vehicle_control_mode_s _vehicle_control_mode{};
+	rover_rate_setpoint_s _rover_rate_setpoint{};
+
+	// uORB publications
+	uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
+	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
+	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
+	uORB::Publication<rover_rate_status_s> _rover_rate_status_pub{ORB_ID(rover_rate_status)};
+
+	// Variables
+	float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint,  _estimated_speed_body_x]
+					       between [0, 0] and [1, _param_ro_max_thr_speed].*/
+	float _max_yaw_rate{0.f};
+	float _vehicle_yaw_rate{0.f};
+	hrt_abstime _timestamp{0};
+	float _dt{0.f}; // Time since last update [s].
+	bool _prev_param_check_passed{true};
+
+	// Controllers
+	PID _pid_yaw_rate;
+	SlewRate<float> _yaw_rate_setpoint{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
+		(ParamFloat<px4::params::RA_WHEEL_BASE>)    _param_ra_wheel_base,
+		(ParamFloat<px4::params::RA_MAX_STR_ANG>)   _param_ra_max_str_ang,
+		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
+		(ParamFloat<px4::params::RO_YAW_RATE_TH>)   _param_ro_yaw_rate_th,
+		(ParamFloat<px4::params::RO_YAW_RATE_P>)    _param_ro_yaw_rate_p,
+		(ParamFloat<px4::params::RO_YAW_RATE_I>)    _param_ro_yaw_rate_i,
+		(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
+		(ParamFloat<px4::params::RO_SPEED_TH>)      _param_ro_speed_th
+	)
+};

src/modules/rover_ackermann/AckermannRateControl/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(AckermannRateControl
+	AckermannRateControl.cpp
+)
+
+target_link_libraries(AckermannRateControl PUBLIC PID)
+target_include_directories(AckermannRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_ackermann/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#   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
@@ -31,8 +31,9 @@
 #
 ############################################################################
 
-add_subdirectory(RoverAckermannGuidance)
+add_subdirectory(AckermannRateControl)
-add_subdirectory(RoverAckermannControl)
+add_subdirectory(AckermannAttControl)
+add_subdirectory(AckermannPosVelControl)
 
 px4_add_module(
 	MODULE modules__rover_ackermann
@@ -41,11 +42,13 @@ px4_add_module(
 		RoverAckermann.cpp
 		RoverAckermann.hpp
 	DEPENDS
-		RoverAckermannGuidance
+		AckermannRateControl
-		RoverAckermannControl
+		AckermannAttControl
+		AckermannPosVelControl
 		px4_work_queue
-		SlewRate
+		rover_control
-		pure_pursuit
 	MODULE_CONFIG
 		module.yaml
 )
+
+set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/rover_control/module.yaml)

src/modules/rover_ackermann/Kconfig

@@ -1,6 +1,5 @@
 menuconfig MODULES_ROVER_ACKERMANN
 	bool "rover_ackermann"
 	default n
-	depends on MODULES_CONTROL_ALLOCATOR
 	---help---
-		Enable support for control of ackermann drive rovers
+		Enable support for rover_ackermann

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *   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
@@ -33,14 +33,14 @@
 
 #include "RoverAckermann.hpp"
 
+using namespace time_literals;
+
 RoverAckermann::RoverAckermann() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	_rover_ackermann_setpoint_pub.advertise();
+	_rover_throttle_setpoint_pub.advertise();
-	_ax_filter.setAlpha(0.05);
+	_rover_steering_setpoint_pub.advertise();
-	_ay_filter.setAlpha(0.05);
-	_az_filter.setAlpha(0.05);
 	updateParams();
 }
 
@@ -53,176 +53,116 @@ bool RoverAckermann::init()
 void RoverAckermann::updateParams()
 {
 	ModuleParams::updateParams();
+
+	if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) {
+		_servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get());
+	}
+
+	if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
+		_motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
+	}
 }
 
 void RoverAckermann::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	updateSubscriptions();
-
-	// Generate and publish speed and steering setpoints
-	hrt_abstime timestamp = hrt_absolute_time();
-
-	switch (_nav_state) {
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				rover_ackermann_setpoint_s rover_ackermann_setpoint{};
-				rover_ackermann_setpoint.timestamp =  timestamp;
-				rover_ackermann_setpoint.forward_speed_setpoint =  NAN;
-				rover_ackermann_setpoint.forward_speed_setpoint_normalized =  manual_control_setpoint.throttle;
-				rover_ackermann_setpoint.steering_setpoint = NAN;
-				rover_ackermann_setpoint.steering_setpoint_normalized = manual_control_setpoint.roll;
-				rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN;
-				_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
-			}
-
-		} break;
-
-	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				rover_ackermann_setpoint_s rover_ackermann_setpoint{};
-				rover_ackermann_setpoint.timestamp =  timestamp;
-				rover_ackermann_setpoint.forward_speed_setpoint =  NAN;
-				rover_ackermann_setpoint.forward_speed_setpoint_normalized =  manual_control_setpoint.throttle;
-				rover_ackermann_setpoint.steering_setpoint = NAN;
-				rover_ackermann_setpoint.steering_setpoint_normalized = NAN;
-				rover_ackermann_setpoint.lateral_acceleration_setpoint = math::interpolate(manual_control_setpoint.roll, -1.f, 1.f,
-						-_param_ra_max_lat_accel.get(), _param_ra_max_lat_accel.get());
-				_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
-			}
-
-		} break;
-
-	case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				rover_ackermann_setpoint_s rover_ackermann_setpoint{};
-				rover_ackermann_setpoint.timestamp = timestamp;
-				rover_ackermann_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-						-1.f, 1.f, -_param_ra_max_speed.get(), _param_ra_max_speed.get());
-				rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN;
-				rover_ackermann_setpoint.steering_setpoint = NAN;
-				rover_ackermann_setpoint.steering_setpoint_normalized = NAN;
-				rover_ackermann_setpoint.lateral_acceleration_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll,
-						STICK_DEADZONE), -1.f, 1.f, -_param_ra_max_lat_accel.get(), _param_ra_max_lat_accel.get());
-
-				if (fabsf(rover_ackermann_setpoint.lateral_acceleration_setpoint) > FLT_EPSILON
-				    || fabsf(rover_ackermann_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
-					_course_control = false;
-
-				} 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 float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
-										2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
-					const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(
-							rover_ackermann_setpoint.forward_speed_setpoint) *
-									     vector_scaling * _pos_ctl_course_direction;
-					// Calculate steering setpoint
-					const float steering_setpoint = _ackermann_guidance.calcDesiredSteering(_posctl_pure_pursuit,
-									target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, _param_ra_wheel_base.get(),
-									rover_ackermann_setpoint.forward_speed_setpoint, _vehicle_yaw, _param_ra_max_steer_angle.get(), _armed);
-					rover_ackermann_setpoint.lateral_acceleration_setpoint = powf(_vehicle_forward_speed,
-							2.f) * tanf(steering_setpoint) / _param_ra_wheel_base.get();
-				}
-
-				_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
-			}
-
-		} break;
-
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-		_ackermann_guidance.computeGuidance(_vehicle_forward_speed, _vehicle_yaw, _nav_state, _armed);
-		break;
-
-	default: // Unimplemented nav states will stop the rover
-		rover_ackermann_setpoint_s rover_ackermann_setpoint{};
-		rover_ackermann_setpoint.timestamp =  timestamp;
-		rover_ackermann_setpoint.forward_speed_setpoint =  NAN;
-		rover_ackermann_setpoint.forward_speed_setpoint_normalized =  0.f;
-		rover_ackermann_setpoint.steering_setpoint = NAN;
-		rover_ackermann_setpoint.steering_setpoint_normalized = 0.f;
-		rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN;
-		_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
-		break;
-	}
-
-	if (!_armed) {
-		_ackermann_control.resetControllers();
-	}
-
-	_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw, _vehicle_lateral_acceleration);
-
-}
-
-void RoverAckermann::updateSubscriptions()
 {
 	if (_parameter_update_sub.updated()) {
 		updateParams();
 	}
 
-	if (_vehicle_status_sub.updated()) {
+	const hrt_abstime timestamp_prev = _timestamp;
-		vehicle_status_s vehicle_status;
+	_timestamp = hrt_absolute_time();
-		_vehicle_status_sub.copy(&vehicle_status);
+	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
-		if (vehicle_status.nav_state != _nav_state) { // Reset on mode change
+	_ackermann_pos_vel_control.updatePosVelControl();
-			_ackermann_control.resetControllers();
+	_ackermann_att_control.updateAttControl();
-			_course_control = false;
+	_ackermann_rate_control.updateRateControl();
-		}
 
-		_nav_state = vehicle_status.nav_state;
+	if (_vehicle_control_mode_sub.updated()) {
-		_armed = vehicle_status.arming_state == 2;
+		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
 	}
 
-	if (_vehicle_attitude_sub.updated()) {
+	const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-		vehicle_attitude_s vehicle_attitude{};
+					      && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled
-		_vehicle_attitude_sub.copy(&vehicle_attitude);
+					      && !_vehicle_control_mode.flag_control_rates_enabled;
-		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+
-		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
+	if (full_manual_mode_enabled) { // Manual mode
+		generateSteeringSetpoint();
 	}
 
-	if (_vehicle_local_position_sub.updated()) {
+	generateActuatorSetpoint();
-		vehicle_local_position_s vehicle_local_position{};
-		_vehicle_local_position_sub.copy(&vehicle_local_position);
 
-		if (PX4_ISFINITE(vehicle_local_position.ax)) {
+}
-			_ax_filter.update(vehicle_local_position.ax);
-		}
 
-		if (PX4_ISFINITE(vehicle_local_position.ay)) {
+void RoverAckermann::generateSteeringSetpoint()
-			_ay_filter.update(vehicle_local_position.ay);
+{
-		}
+	manual_control_setpoint_s manual_control_setpoint{};
 
-		if (PX4_ISFINITE(vehicle_local_position.az)) {
+	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-			_az_filter.update(vehicle_local_position.az);
+		rover_steering_setpoint_s rover_steering_setpoint{};
-		}
+		rover_steering_setpoint.timestamp = _timestamp;
-
+		rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll;
-		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+		_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
-		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		rover_throttle_setpoint_s rover_throttle_setpoint{};
-		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		rover_throttle_setpoint.timestamp = _timestamp;
-		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
+		rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
-		Vector3f acceleration_in_local_frame(_ax_filter.getState(), _ay_filter.getState(), _az_filter.getState());
+		rover_throttle_setpoint.throttle_body_y = 0.f;
-		Vector3f acceleration_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(acceleration_in_local_frame);
+		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-		_vehicle_lateral_acceleration = acceleration_in_body_frame(1);
 	}
 }
 
+void RoverAckermann::generateActuatorSetpoint()
+{
+	if (_rover_throttle_setpoint_sub.updated()) {
+		_rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint);
+	}
+
+	if (_actuator_motors_sub.updated()) {
+		actuator_motors_s actuator_motors{};
+		_actuator_motors_sub.copy(&actuator_motors);
+		_current_motor_setpoint = actuator_motors.control[0];
+	}
+
+	if (_vehicle_control_mode.flag_armed) {
+		actuator_motors_s actuator_motors{};
+		actuator_motors.reversible_flags = _param_r_rev.get();
+		actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint,
+					     _rover_throttle_setpoint.throttle_body_x, _current_motor_setpoint, _param_ro_accel_limit.get(),
+					     _param_ro_decel_limit.get(),
+					     _param_ro_max_thr_speed.get(), _dt);
+		actuator_motors.timestamp = _timestamp;
+		_actuator_motors_pub.publish(actuator_motors);
+	}
+
+	if (_rover_steering_setpoint_sub.updated()) {
+		_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
+	}
+
+	if (_actuator_servos_sub.updated()) {
+		actuator_servos_s actuator_servos{};
+		_actuator_servos_sub.copy(&actuator_servos);
+		_current_servo_setpoint = actuator_servos.control[0];
+	}
+
+	if (_param_ra_str_rate_limit.get() > FLT_EPSILON
+	    && _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured
+		if (fabsf(_servo_setpoint.getState() - _current_servo_setpoint) > fabsf(
+			    _rover_steering_setpoint.normalized_steering_angle -
+			    _current_servo_setpoint)) {
+			_servo_setpoint.setForcedValue(_current_servo_setpoint);
+		}
+
+		_servo_setpoint.update(_rover_steering_setpoint.normalized_steering_angle, _dt);
+
+	} else {
+		_servo_setpoint.setForcedValue(_rover_steering_setpoint.normalized_steering_angle);
+	}
+
+	actuator_servos_s actuator_servos{};
+	actuator_servos.control[0] = _servo_setpoint.getState();
+	actuator_servos.timestamp = _timestamp;
+	_actuator_servos_pub.publish(actuator_servos);
+}
+
 int RoverAckermann::task_spawn(int argc, char *argv[])
 {
 	RoverAckermann *instance = new RoverAckermann();

src/modules/rover_ackermann/RoverAckermann.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *   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
@@ -39,35 +39,28 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+// Libraries
 #include <lib/pure_pursuit/PurePursuit.hpp>
+#include <lib/rover_control/RoverControl.hpp>
+#include <lib/slew_rate/SlewRate.hpp>
 
 // uORB includes
-#include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/rover_ackermann_setpoint.h>
+#include <uORB/topics/actuator_servos.h>
-#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/rover_steering_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/rover_throttle_setpoint.h>
-
+#include <uORB/topics/vehicle_control_mode.h>
-
+#include <uORB/topics/manual_control_setpoint.h>
-// Standard library includes
-#include <math.h>
-#include <matrix/matrix/math.hpp>
-#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 
 // Local includes
-#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
+#include "AckermannRateControl/AckermannRateControl.hpp"
-#include "RoverAckermannControl/RoverAckermannControl.hpp"
+#include "AckermannAttControl/AckermannAttControl.hpp"
-
+#include "AckermannPosVelControl/AckermannPosVelControl.hpp"
-using namespace time_literals;
-
-// Constants
-static constexpr float STICK_DEADZONE =
-	0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
-static constexpr float SPEED_THRESHOLD =
-	0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
 
 class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	public px4::ScheduledWorkItem
@@ -90,56 +83,65 @@ public:
 
 	bool init();
 
-
 protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
 	void updateParams() override;
 
 private:
 	void Run() override;
 
 	/**
-	 * @brief Update uORB subscriptions.
+	 * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode).
 	 */
-	void updateSubscriptions();
+	void generateSteeringSetpoint();
+
+	/**
+	 * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
+	 */
+	void generateActuatorSetpoint();
 
 	// uORB subscriptions
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
+	vehicle_control_mode_s _vehicle_control_mode{};
+	rover_steering_setpoint_s _rover_steering_setpoint{};
+	rover_throttle_setpoint_s _rover_throttle_setpoint{};
 
 	// uORB publications
-	uORB::Publication<rover_ackermann_setpoint_s> _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)};
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
+	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
+	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
 
 	// Class instances
-	RoverAckermannGuidance _ackermann_guidance{this};
+	AckermannRateControl _ackermann_rate_control{this};
-	RoverAckermannControl _ackermann_control{this};
+	AckermannAttControl _ackermann_att_control{this};
-	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
+	AckermannPosVelControl _ackermann_pos_vel_control{this};
 
 	// Variables
-	matrix::Quatf _vehicle_attitude_quaternion{};
+	hrt_abstime _timestamp{0};
-	int _nav_state{0};
+	float _dt{0.f};
-	float _vehicle_forward_speed{0.f};
+	float _current_servo_setpoint{0.f};
-	float _vehicle_yaw{0.f};
+	float _current_motor_setpoint{0.f};
-	bool _armed{false};
+
-	bool _course_control{false};
+	// Controllers
-	Vector2f _pos_ctl_course_direction{};
+	SlewRate<float> _servo_setpoint{0.f};
-	Vector2f _pos_ctl_start_position_ned{};
+	SlewRate<float> _motor_setpoint{0.f};
-	Vector2f _curr_pos_ned{};
-	float _vehicle_lateral_acceleration{0.f};
-	AlphaFilter<float> _ax_filter;
-	AlphaFilter<float> _ay_filter;
-	AlphaFilter<float> _az_filter;
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
-		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
+		(ParamFloat<px4::params::RA_STR_RATE_LIM>) _param_ra_str_rate_limit,
-		(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
+		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
-		(ParamFloat<px4::params::RA_MAX_LAT_ACCEL>) _param_ra_max_lat_accel,
+		(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
-		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
+		(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
-
+		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
 	)
-
 };

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp

@@ -1,228 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "RoverAckermannControl.hpp"
-
-#include <mathlib/math/Limits.hpp>
-
-using namespace matrix;
-
-RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParams(parent)
-{
-	updateParams();
-	_rover_ackermann_status_pub.advertise();
-}
-
-void RoverAckermannControl::updateParams()
-{
-	ModuleParams::updateParams();
-
-	_pid_throttle.setGains(_param_ra_speed_p.get(), _param_ra_speed_i.get(), 0.f);
-	_pid_throttle.setIntegralLimit(1.f);
-	_pid_throttle.setOutputLimit(1.f);
-
-	_pid_lat_accel.setGains(_param_ra_lat_accel_p.get(), _param_ra_lat_accel_i.get(), 0.f);
-	_pid_lat_accel.setIntegralLimit(1.f);
-	_pid_lat_accel.setOutputLimit(1.f);
-
-	// Update slew rates
-	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
-		_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get());
-	}
-
-	if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
-		_steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) /
-						      _param_ra_max_steer_angle.get());
-	}
-}
-
-void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_speed, const float vehicle_yaw,
-		const float vehicle_lateral_acceleration)
-{
-	// Timestamps
-	hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5_s) * 1e-6f;
-
-	// Update ackermann setpoint
-	_rover_ackermann_setpoint_sub.update(&_rover_ackermann_setpoint);
-
-	// Speed control
-	float forward_speed_normalized{0.f};
-
-	if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) {
-		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint,
-					   vehicle_forward_speed, dt, false);
-
-	} else if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint
-		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint_normalized,
-					   vehicle_forward_speed, dt, true);
-
-	}
-
-	// Closed loop lateral acceleration control (overrides steering setpoint)
-	if (PX4_ISFINITE(_rover_ackermann_setpoint.lateral_acceleration_setpoint)) {
-		float vehicle_forward_speed_temp{0.f};
-
-		if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) { // Use valid measurement if available
-			vehicle_forward_speed_temp = vehicle_forward_speed;
-
-		} else if (PX4_ISFINITE(forward_speed_normalized) && _param_ra_max_thr_speed.get() > FLT_EPSILON) {
-			vehicle_forward_speed_temp = math::interpolate<float>(forward_speed_normalized,
-						     -1.f, 1.f, -_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get());
-		}
-
-		if (fabsf(vehicle_forward_speed_temp) > FLT_EPSILON) {
-			float steering_setpoint = atanf(_param_ra_wheel_base.get() *
-							_rover_ackermann_setpoint.lateral_acceleration_setpoint / powf(
-								vehicle_forward_speed_temp, 2.f));
-
-			if (sign(vehicle_forward_speed_temp) ==
-			    1) { // Only do closed loop control when driving forwards (backwards driving is non-minimum phase and can therefor introduce instability)
-				_pid_lat_accel.setSetpoint(_rover_ackermann_setpoint.lateral_acceleration_setpoint);
-				steering_setpoint += _pid_lat_accel.update(vehicle_lateral_acceleration, dt);
-			}
-
-			_rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(),
-					_param_ra_max_steer_angle.get());
-
-		} else {
-			_rover_ackermann_setpoint.steering_setpoint = 0.f;
-		}
-	}
-
-	// Steering control
-	float steering_normalized{0.f};
-
-	if (PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint)) {
-		steering_normalized = math::interpolate<float>(_rover_ackermann_setpoint.steering_setpoint,
-				      -_param_ra_max_steer_angle.get(),
-				      _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint
-
-	} else { // Use normalized setpoint
-		steering_normalized = PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint_normalized) ? math::constrain(
-					      _rover_ackermann_setpoint.steering_setpoint_normalized, -1.f, 1.f) : 0.f;
-	}
-
-	if (_param_ra_max_steering_rate.get() > FLT_EPSILON
-	    && _param_ra_max_steer_angle.get() > FLT_EPSILON) { // Apply slew rate
-		_steering_with_rate_limit.update(steering_normalized, dt);
-
-	} else {
-		_steering_with_rate_limit.setForcedValue(steering_normalized);
-	}
-
-	// Publish rover Ackermann status (logging)
-	_rover_ackermann_status.timestamp = _timestamp;
-	_rover_ackermann_status.measured_forward_speed = vehicle_forward_speed;
-	_rover_ackermann_status.steering_setpoint_normalized = steering_normalized;
-	_rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState();
-	_rover_ackermann_status.measured_lateral_acceleration = vehicle_lateral_acceleration;
-	_rover_ackermann_status.pid_throttle_integral = _pid_throttle.getIntegral();
-	_rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.getIntegral();
-	_rover_ackermann_status_pub.publish(_rover_ackermann_status);
-
-	// Publish to motor
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get();
-	actuator_motors.control[0] = forward_speed_normalized;
-	actuator_motors.timestamp = _timestamp;
-	_actuator_motors_pub.publish(actuator_motors);
-
-	// Publish to servo
-	actuator_servos_s actuator_servos{};
-	actuator_servos.control[0] = _steering_with_rate_limit.getState();
-	actuator_servos.timestamp = _timestamp;
-	_actuator_servos_pub.publish(actuator_servos);
-
-}
-
-float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint,
-		const float vehicle_forward_speed, const float dt, const bool normalized)
-{
-	float slew_rate_normalization{1.f};
-
-	if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized
-		slew_rate_normalization = _param_ra_max_thr_speed.get() > FLT_EPSILON ? _param_ra_max_thr_speed.get() : 0.f;
-	}
-
-	// Apply acceleration and deceleration limit
-	if (fabsf(forward_speed_setpoint) >= fabsf(_forward_speed_setpoint_with_accel_limit.getState())) {
-		if (_param_ra_max_accel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
-			_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / slew_rate_normalization);
-			_forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
-
-		} else {
-			_forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
-
-		}
-
-	} else if (_param_ra_max_decel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
-		_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_decel.get() / slew_rate_normalization);
-		_forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
-
-	} else {
-		_forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
-	}
-
-	// Calculate normalized forward speed setpoint
-	float forward_speed_normalized{0.f};
-
-	if (normalized) {
-		forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState();
-
-	} else { // Closed loop speed control
-		_rover_ackermann_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState();
-
-		if (_param_ra_max_thr_speed.get() > FLT_EPSILON) { // Feedforward
-			forward_speed_normalized = math::interpolate<float>(_forward_speed_setpoint_with_accel_limit.getState(),
-						   -_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get(),
-						   -1.f, 1.f);
-		}
-
-		_pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState());
-		forward_speed_normalized += _pid_throttle.update(vehicle_forward_speed, dt);
-	}
-
-	return math::constrain(forward_speed_normalized, -1.f, 1.f);
-
-}
-
-void RoverAckermannControl::resetControllers()
-{
-	_pid_throttle.resetIntegral();
-	_pid_lat_accel.resetIntegral();
-	_forward_speed_setpoint_with_accel_limit.setForcedValue(0.f);
-	_steering_with_rate_limit.setForcedValue(0.f);
-
-}

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp

@@ -1,137 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-// PX4 includes
-#include <px4_platform_common/module_params.h>
-
-// uORB includes
-#include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/rover_ackermann_setpoint.h>
-#include <uORB/topics/rover_ackermann_status.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/actuator_servos.h>
-
-// Standard libraries
-#include <lib/pid/PID.hpp>
-#include <matrix/matrix/math.hpp>
-#include <matrix/math.hpp>
-#include <mathlib/mathlib.h>
-#include <math.h>
-#include <lib/slew_rate/SlewRate.hpp>
-
-using namespace matrix;
-using namespace time_literals;
-
-/**
- * @brief Class for ackermann rover guidance.
- */
-class RoverAckermannControl : public ModuleParams
-{
-public:
-	/**
-	 * @brief Constructor for RoverAckermannControl.
-	 * @param parent The parent ModuleParams object.
-	 */
-	RoverAckermannControl(ModuleParams *parent);
-	~RoverAckermannControl() = default;
-
-	/**
-	 * @brief Compute motor commands based on setpoints
-	 * @param vehicle_forward_speed Measured speed in body x direction. Positiv: forwards, Negativ: backwards [m/s]
-	 * @param vehicle_yaw Measured yaw [rad]
-	 * @param vehicle_lateral_acceleration Measured acceleration in body y direction. Positiv: right, Negativ: left [m/s^s]
-	 */
-	void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw, float vehicle_lateral_acceleration);
-
-	/**
-	 * @brief Reset PID controllers and slew rates
-	 */
-	void resetControllers();
-
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
-	/**
-	 * @brief Compute normalized forward speed setpoint by applying slew rates
-	 * to the forward speed setpoint and doing closed loop speed control if enabled.
-	 * @param forward_speed_setpoint Forward speed setpoint [m/s].
-	 * @param vehicle_forward_speed Actual forward speed [m/s].
-	 * @param dt Time since last update [s].
-	 * @param normalized Indicates if the forward speed setpoint is already normalized.
-	 * @return Normalized forward speed setpoint with applied slew rates [-1, 1].
-	 */
-	float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float dt, bool normalized);
-
-	// uORB subscriptions
-	uORB::Subscription _rover_ackermann_setpoint_sub{ORB_ID(rover_ackermann_setpoint)};
-	rover_ackermann_setpoint_s _rover_ackermann_setpoint{};
-
-	// uORB publications
-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
-	uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
-	uORB::Publication<rover_ackermann_status_s> _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)};
-	rover_ackermann_status_s _rover_ackermann_status{};
-
-	// Variables
-	hrt_abstime _timestamp{0};
-
-	// Controllers
-	PID _pid_throttle; // The PID controller for the closed loop speed control
-	PID _pid_lat_accel; // The PID controller for the closed loop speed control
-	SlewRate<float> _steering_with_rate_limit{0.f};
-	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
-
-	// Parameters
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RA_SPEED_P>) _param_ra_speed_p,
-		(ParamFloat<px4::params::RA_SPEED_I>) _param_ra_speed_i,
-		(ParamFloat<px4::params::RA_LAT_ACCEL_P>) _param_ra_lat_accel_p,
-		(ParamFloat<px4::params::RA_LAT_ACCEL_I>) _param_ra_lat_accel_i,
-		(ParamFloat<px4::params::RA_MAX_ACCEL>) _param_ra_max_accel,
-		(ParamFloat<px4::params::RA_MAX_DECEL>) _param_ra_max_decel,
-		(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
-		(ParamFloat<px4::params::RA_MAX_THR_SPEED>) _param_ra_max_thr_speed,
-		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
-		(ParamFloat<px4::params::RA_MAX_STR_RATE>) _param_ra_max_steering_rate,
-		(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
-	)
-};

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp

@@ -1,309 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "RoverAckermannGuidance.hpp"
-
-#include <mathlib/math/Limits.hpp>
-
-using namespace matrix;
-using namespace time_literals;
-
-RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent)
-{
-	_rover_ackermann_guidance_status_pub.advertise();
-	updateParams();
-}
-
-void RoverAckermannGuidance::updateParams()
-{
-	ModuleParams::updateParams();
-
-	if (_param_ra_wheel_base.get() > FLT_EPSILON && _param_ra_max_lat_accel.get() > FLT_EPSILON
-	    && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
-		_min_speed = sqrt(_param_ra_wheel_base.get() * _param_ra_max_lat_accel.get() / tanf(_param_ra_max_steer_angle.get()));
-	}
-}
-
-void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed,
-		const float vehicle_yaw, const int nav_state, const bool armed)
-{
-	updateSubscriptions();
-
-	// Distances to waypoints
-	const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
-					  _prev_wp(0), _prev_wp(1));
-	const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
-					  _curr_wp(0), _curr_wp(1));
-
-	// Catch return to launch
-	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
-		_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
-	}
-
-	// Guidance logic
-	if (_mission_finished) { // Mission is finished
-		_desired_steering = 0.f;
-		_desired_speed = 0.f;
-
-	} else if (_nav_cmd == 93) { // Catch delay command
-		_desired_speed = 0.f;
-
-	} else { // Regular guidance algorithm
-
-		_desired_speed = calcDesiredSpeed(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp,
-						  _acceptance_radius,
-						  _prev_acceptance_radius, _param_ra_max_decel.get(), _param_ra_max_jerk.get(), nav_state, _waypoint_transition_angle,
-						  _prev_waypoint_transition_angle, _param_ra_max_speed.get());
-
-		_desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
-							_param_ra_wheel_base.get(), _desired_speed, vehicle_yaw, _param_ra_max_steer_angle.get(), armed);
-
-	}
-
-	// Publish ackermann controller status (logging)
-	hrt_abstime timestamp = hrt_absolute_time();
-	_rover_ackermann_guidance_status.timestamp = timestamp;
-	_rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status);
-
-	// Publish speed and steering setpoints
-	rover_ackermann_setpoint_s rover_ackermann_setpoint{};
-	rover_ackermann_setpoint.timestamp = timestamp;
-	rover_ackermann_setpoint.forward_speed_setpoint = _desired_speed;
-	rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN;
-	rover_ackermann_setpoint.steering_setpoint = NAN;
-	rover_ackermann_setpoint.steering_setpoint_normalized = NAN;
-	rover_ackermann_setpoint.lateral_acceleration_setpoint = powf(vehicle_forward_speed,
-			2.f) * tanf(_desired_steering) / _param_ra_wheel_base.get();
-	_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
-
-}
-
-void RoverAckermannGuidance::updateSubscriptions()
-{
-	if (_vehicle_global_position_sub.updated()) {
-		vehicle_global_position_s vehicle_global_position{};
-		_vehicle_global_position_sub.copy(&vehicle_global_position);
-		_curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
-	}
-
-	if (_local_position_sub.updated()) {
-		vehicle_local_position_s local_position{};
-		_local_position_sub.copy(&local_position);
-
-		if (!_global_ned_proj_ref.isInitialized()
-		    || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
-			_global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
-		}
-
-		_curr_pos_ned = Vector2f(local_position.x, local_position.y);
-	}
-
-	if (_home_position_sub.updated()) {
-		home_position_s home_position{};
-		_home_position_sub.copy(&home_position);
-		_home_position = Vector2d(home_position.lat, home_position.lon);
-	}
-
-	if (_position_setpoint_triplet_sub.updated()) {
-		updateWaypointsAndAcceptanceRadius();
-	}
-
-	if (_mission_result_sub.updated()) {
-		mission_result_s mission_result{};
-		_mission_result_sub.copy(&mission_result);
-		_mission_finished = mission_result.finished;
-	}
-
-	if (_navigator_mission_item_sub.updated()) {
-		navigator_mission_item_s navigator_mission_item{};
-		_navigator_mission_item_sub.copy(&navigator_mission_item);
-		_nav_cmd = navigator_mission_item.nav_cmd;
-	}
-}
-
-void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
-{
-	position_setpoint_triplet_s position_setpoint_triplet{};
-	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
-
-	// Global waypoint coordinates
-	_prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid
-	_curr_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if current waypoint is invalid
-	_next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL
-
-	if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
-	    && PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
-		_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
-
-	}
-
-	if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
-	    && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) {
-		_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
-
-	}
-
-	if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
-	    && PX4_ISFINITE(position_setpoint_triplet.next.lon)) {
-		_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
-
-	}
-
-	// NED waypoint coordinates
-	_curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));
-	_prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
-	_next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));
-
-	// Distances
-	const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned;
-	const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned;
-	float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
-	cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
-	_prev_waypoint_transition_angle = _waypoint_transition_angle;
-	_waypoint_transition_angle = acosf(cosin);
-
-	// Update acceptance radius
-	_prev_acceptance_radius = _acceptance_radius;
-
-	if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
-		_acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
-				     _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get());
-
-	} else {
-		_acceptance_radius = _param_nav_acc_rad.get();
-	}
-
-	// Waypoint cruising speed
-	_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
-				  position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get()) : _param_ra_max_speed.get();
-}
-
-float RoverAckermannGuidance::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)
-{
-	// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
-	float acceptance_radius = default_acceptance_radius;
-	const float theta = waypoint_transition_angle / 2.f;
-	const float min_turning_radius = wheel_base / sinf(max_steer_angle);
-	const float acceptance_radius_temp = min_turning_radius / tanf(theta);
-	const float acceptance_radius_temp_scaled = acceptance_radius_gain *
-			acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
-	acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
-			    acceptance_radius_max);
-
-	// Publish updated acceptance radius
-	position_controller_status_s pos_ctrl_status{};
-	pos_ctrl_status.acceptance_radius = acceptance_radius;
-	pos_ctrl_status.timestamp = hrt_absolute_time();
-	_position_controller_status_pub.publish(pos_ctrl_status);
-	return acceptance_radius;
-}
-
-float RoverAckermannGuidance::calcDesiredSpeed(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 nav_state,
-		const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed)
-{
-	// Catch improper values
-	if (miss_speed_min < -FLT_EPSILON  || miss_speed_min > cruising_speed) {
-		return cruising_speed;
-	}
-
-	// Cornering slow down effect
-	if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) {
-		const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f);
-		const float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get());
-		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
-
-	} else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) {
-		const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
-		const float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get());
-		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
-
-	}
-
-	// Straight line speed
-	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
-		float straight_line_speed{0.f};
-
-		if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
-			straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-					      max_decel, distance_to_curr_wp, 0.f);
-
-		} else {
-			const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
-			float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get());
-			cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed);
-			straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-					      max_decel, distance_to_curr_wp - acc_rad, cornering_speed);
-		}
-
-		return math::min(straight_line_speed, cruising_speed);
-
-	} else {
-		return cruising_speed;
-	}
-
-}
-
-float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned,
-		const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed,
-		const float vehicle_yaw, const float max_steering, const bool armed)
-{
-	const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned,
-				      desired_speed);
-	const float lookahead_distance = pure_pursuit.getLookaheadDistance();
-	const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw);
-	// For logging
-	_rover_ackermann_guidance_status.lookahead_distance = lookahead_distance;
-	_rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F);
-
-	float desired_steering{0.f};
-
-	if (!armed) {
-		return desired_steering;
-	}
-
-	if (math::abs_t(heading_error) <= M_PI_2_F) {
-		desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance);
-
-
-	} else {
-		desired_steering = atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error -
-					 sign(heading_error) * M_PI_2_F)) / lookahead_distance);
-	}
-
-	return math::constrain(desired_steering, -max_steering, max_steering);
-
-}

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -1,215 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-// PX4 includes
-#include <px4_platform_common/module_params.h>
-#include <lib/pure_pursuit/PurePursuit.hpp>
-
-// uORB includes
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/rover_ackermann_guidance_status.h>
-#include <uORB/topics/rover_ackermann_setpoint.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/position_controller_status.h>
-#include <uORB/topics/mission_result.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/navigator_mission_item.h>
-
-// Standard library includes
-#include <matrix/matrix/math.hpp>
-#include <matrix/math.hpp>
-#include <mathlib/mathlib.h>
-#include <lib/geo/geo.h>
-#include <math.h>
-#include <lib/pid/PID.hpp>
-
-using namespace matrix;
-
-/**
- * @brief Class for ackermann drive guidance.
- */
-class RoverAckermannGuidance : public ModuleParams
-{
-public:
-	/**
-	 * @brief Constructor for RoverAckermannGuidance.
-	 * @param parent The parent ModuleParams object.
-	 */
-	RoverAckermannGuidance(ModuleParams *parent);
-	~RoverAckermannGuidance() = default;
-
-	/**
-	 * @brief Compute and publish speed and steering setpoints based on the mission plan.
-	 * @param vehicle_forward_speed Forward speed of the vehicle [m/s]
-	 * @param vehicle_yaw Yaw of the vehicle [rad]
-	 * @param nav_state Vehicle navigation state.
-	 * @param armed Vehicle arm state.
-	 */
-	void computeGuidance(float vehicle_forward_speed, float vehicle_yaw, int nav_state, bool armed);
-
-	/**
-	 * @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to
-	 * reach the point calculated using the pure pursuit algorithm (see PurePursuit.hpp).
-	 * @param pure_pursuit Pure pursuit class instance.
-	 * @param curr_wp_ned Current waypoint in NED frame [m].
-	 * @param prev_wp_ned Previous waypoint in NED frame [m].
-	 * @param curr_pos_ned Current position of the vehicle in NED frame [m].
-	 * @param wheel_base Rover wheelbase [m].
-	 * @param desired_speed Desired speed for the rover [m/s].
-	 * @param vehicle_yaw Current yaw of the rover [rad].
-	 * @param max_steering Maximum steering angle of the rover [rad].
-	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
-	 * @param armed Vehicle arm status
-	 * @return Steering setpoint for the rover [rad].
-	 */
-	float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
-				  const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering, bool armed);
-
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
-	/**
-	 * @brief Update uORB subscriptions
-	 */
-	void updateSubscriptions();
-
-	/**
-	 * @brief Update global/NED waypoint coordinates and acceptance radius.
-	 */
-	void updateWaypointsAndAcceptanceRadius();
-
-	/**
-	 * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment
-	 * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle.
-	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
-	 * @param default_acceptance_radius Default acceptance radius for waypoints [m].
-	 * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-].
-	 * @param acceptance_radius_max Maximum value for the acceptance radius [m].
-	 * @param wheel_base Rover wheelbase [m].
-	 * @param max_steer_angle Rover maximum steer angle [rad].
-	 * @return Updated acceptance radius [m].
-	 */
-	float updateAcceptanceRadius(const float waypoint_transition_angle, float default_acceptance_radius,
-				     float acceptance_radius_gain,
-				     float acceptance_radius_max, float wheel_base, float max_steer_angle);
-
-
-	/**
-	 * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse
-	 * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a speed trajectory
-	 * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the
-	 * maximum acceleration and jerk.
-	 * @param cruising_speed Cruising speed [m/s].
-	 * @param miss_speed_min Minimum speed setpoint [m/s].
-	 * @param distance_to_prev_wp Distance to the previous waypoint [m].
-	 * @param distance_to_curr_wp Distance to the current waypoint [m].
-	 * @param acc_rad Acceptance radius of the current waypoint [m].
-	 * @param prev_acc_rad Acceptance radius of the previous waypoint [m].
-	 * @param max_accel Maximum allowed acceleration [m/s^2].
-	 * @param max_jerk Maximum allowed jerk [m/s^3].
-	 * @param nav_state Current nav_state of the rover.
-	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
-	 * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
-	 * @param max_speed Maximum speed setpoint [m/s]
-	 * @return Speed setpoint [m/s].
-	 */
-	float calcDesiredSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp,
-			       float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state,
-			       float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed);
-
-	// uORB subscriptions
-	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
-	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
-	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
-	uORB::Subscription _navigator_mission_item_sub{ORB_ID(navigator_mission_item)};
-
-	// uORB publications
-	uORB::Publication<rover_ackermann_setpoint_s> _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)};
-	uORB::Publication<rover_ackermann_guidance_status_s> _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)};
-	uORB::Publication<position_controller_status_s>	_position_controller_status_pub{ORB_ID(position_controller_status)};
-	rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{};
-
-	// Class instances
-	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
-	PurePursuit _pure_pursuit{this}; // Pure pursuit library
-
-	// Rover variables
-	float _desired_steering{0.f};
-	float _desired_speed{0.f};
-	Vector2d _curr_pos{};
-	Vector2f _curr_pos_ned{};
-	bool _mission_finished{false};
-
-	// Waypoint variables
-	Vector2d _home_position{};
-	Vector2f _curr_wp_ned{};
-	Vector2f _prev_wp_ned{};
-	Vector2f _next_wp_ned{};
-	Vector2d _curr_wp{};
-	Vector2d _prev_wp{};
-	Vector2d _next_wp{};
-	float _acceptance_radius{0.5f};
-	float _prev_acceptance_radius{0.5f};
-	float _cruising_speed{0.f};
-	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]
-	uint _nav_cmd{0};
-	float _min_speed{0.f};
-
-	// Parameters
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
-		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
-		(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_SPEED_P>) _param_ra_p_speed,
-		(ParamFloat<px4::params::RA_SPEED_I>) _param_ra_i_speed,
-		(ParamFloat<px4::params::RA_MAX_SPEED>) _param_ra_max_speed,
-		(ParamFloat<px4::params::RA_MAX_JERK>) _param_ra_max_jerk,
-		(ParamFloat<px4::params::RA_MAX_DECEL>) _param_ra_max_decel,
-		(ParamFloat<px4::params::RA_MAX_LAT_ACCEL>) _param_ra_max_lat_accel,
-		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
-	)
-};

src/modules/rover_ackermann/module.yaml

@@ -6,26 +6,37 @@ parameters:
       RA_WHEEL_BASE:
         description:
           short: Wheel base
-          long: Distance from the front to the rear axle
+          long: Distance from the front to the rear axle.
         type: float
         unit: m
-        min: 0.001
+        min: 0
         max: 100
         increment: 0.001
         decimal: 3
-        default: 0.5
+        default: 0
 
       RA_MAX_STR_ANG:
         description:
           short: Maximum steering angle
-          long: The maximum angle that the rover can steer
         type: float
         unit: rad
-        min: 0.1
+        min: 0
         max: 1.5708
         increment: 0.01
         decimal: 2
-        default: 0.5236
+        default: 0
+
+      RA_STR_RATE_LIM:
+        description:
+          short: Steering rate limit
+          long: Set to -1 to disable.
+        type: float
+        unit: deg/s
+        min: -1
+        max: 1000
+        increment: 0.01
+        decimal: 2
+        default: -1
 
       RA_ACC_RAD_MAX:
         description:
@@ -41,7 +52,7 @@ parameters:
         max: 100
         increment: 0.01
         decimal: 2
-        default: 3
+        default: -1
 
       RA_ACC_RAD_GAIN:
         description:
@@ -55,142 +66,4 @@ parameters:
         max: 100
         increment: 0.01
         decimal: 2
-        default: 2
-
-      RA_SPEED_P:
-        description:
-          short: Proportional gain for ground speed controller
-        type: float
-        min: 0
-        max: 100
-        increment: 0.01
-        decimal: 2
         default: 1
-
-      RA_SPEED_I:
-        description:
-          short: Integral gain for ground speed controller
-        type: float
-        min: 0
-        max: 100
-        increment: 0.001
-        decimal: 3
-        default: 1
-
-      RA_MAX_SPEED:
-        description:
-          short: Maximum allowed speed
-          long: |
-            This is the maximum allowed speed setpoint when driving in a mode that uses
-            closed loop speed control.
-        type: float
-        unit: m/s
-        min: -1
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: -1
-
-      RA_MAX_THR_SPEED:
-        description:
-          short: Speed the rover drives at maximum throttle
-          long: |
-            This parameter is used to calculate the feedforward term of the closed loop speed control which linearly
-            maps desired speeds to normalized motor commands [-1. 1].
-            A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode.
-            Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower.
-        type: float
-        unit: m/s
-        min: -1
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: -1
-
-      RA_MAX_ACCEL:
-        description:
-          short: Maximum acceleration for the rover
-          long: |
-            This is used for the acceleration slew rate.
-        type: float
-        unit: m/s^2
-        min: -1
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: -1
-
-      RA_MAX_DECEL:
-        description:
-          short: Maximum deceleration for the rover
-          long: |
-            This is used for the deceleration slew rate, the feed-forward term
-            for the speed controller during missions and the corner slow down effect.
-            Note: For the corner slow down effect RA_MAX_JERK also has to be set.
-        type: float
-        unit: m/s^2
-        min: -1
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: -1
-
-      RA_MAX_JERK:
-        description:
-          short: Maximum jerk
-          long: |
-            Limit for forwards acc/deceleration change.
-            This is used for the corner slow down effect.
-            Note: RA_MAX_DECEL also has to be set for this to be enabled.
-        type: float
-        unit: m/s^3
-        min: -1
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: -1
-
-      RA_MAX_STR_RATE:
-        description:
-          short: Maximum steering rate for the rover
-        type: float
-        unit: deg/s
-        min: -1
-        max: 1000
-        increment: 0.01
-        decimal: 2
-        default: -1
-
-      RA_MAX_LAT_ACCEL:
-        description:
-          short: Maximum allowed lateral acceleration
-          long: |
-            This parameter is used to cap the lateral acceleration and map controller inputs to desired lateral acceleration
-            in Acro, Stabilized and Position mode.
-        type: float
-        unit: m/s^2
-        min: 0.01
-        max: 1000
-        increment: 0.01
-        decimal: 2
-        default: 0.01
-
-      RA_LAT_ACCEL_P:
-        description:
-          short: Proportional gain for lateral acceleration controller
-        type: float
-        min: 0
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: 0
-
-      RA_LAT_ACCEL_I:
-        description:
-          short: Integral gain for lateral acceleration controller
-        type: float
-        min: 0
-        max: 100
-        increment: 0.001
-        decimal: 3
-        default: 0

src/modules/rover_pos_control/rover_pos_control_params.c

@@ -57,7 +57,7 @@
  * @min 0.0
  * @decimal 3
  * @increment 0.01
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f);
 
@@ -73,7 +73,7 @@ PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f);
  * @max 50.0
  * @decimal 1
  * @increment 0.1
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f);
 
@@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f);
  * @max 50.0
  * @decimal 1
  * @increment 0.5
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f);
 
@@ -103,7 +103,7 @@ PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f);
  * @max 0.9
  * @decimal 2
  * @increment 0.05
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f);
 
@@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f);
  * @max 1.0
  * @decimal 2
  * @increment 0.01
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f);
 
@@ -133,7 +133,7 @@ PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f);
  * @max 1.0
  * @decimal 2
  * @increment 0.01
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f);
 
@@ -148,7 +148,7 @@ PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f);
  * @max 1.0
  * @decimal 2
  * @increment 0.01
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f);
 
@@ -160,7 +160,7 @@ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f);
  * @max 1
  * @value 0 open loop control
  * @value 1 close the loop with gps speed
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1);
 
@@ -174,7 +174,7 @@ PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1);
  * @max 50.0
  * @decimal 3
  * @increment 0.005
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f);
 
@@ -188,7 +188,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f);
  * @max 50.0
  * @decimal 3
  * @increment 0.005
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_I, 3.0f);
 
@@ -202,7 +202,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_I, 3.0f);
  * @max 50.0
  * @decimal 3
  * @increment 0.005
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.001f);
 
@@ -216,7 +216,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.001f);
  * @max 50.0
  * @decimal 3
  * @increment 0.005
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f);
 
@@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f);
  * @max 50.0
  * @decimal 3
  * @increment 0.005
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
 
@@ -243,7 +243,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
  * @max 40
  * @decimal 1
  * @increment 0.5
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
 
@@ -256,7 +256,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
  * @max 40
  * @decimal 1
  * @increment 0.5
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
 
@@ -271,7 +271,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
  * @max 3.14159
  * @decimal 3
  * @increment 0.01
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
 
@@ -282,6 +282,6 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
  * @min 0.0
  * @max 400
  * @decimal 1
- * @group Rover Position Control
+ * @group Rover Position Control (Deprecated)
  */
 PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f);