mecanum: add slew rates to yaw, yaw rate and speed setpoints

msg/RoverMecanumStatus.msg

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

src/modules/rover_mecanum/RoverMecanum.hpp

@@ -66,7 +66,7 @@ static constexpr float YAW_RATE_THRESHOLD =
 static constexpr float SPEED_THRESHOLD =
 	0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still
 static constexpr float STICK_DEADZONE =
-	0.3f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
+	0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
 
 using namespace time_literals;
 

src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp

@@ -61,9 +61,19 @@ void RoverMecanumControl::updateParams()
 	_pid_lateral_throttle.setOutputLimit(1.f);
 
 	_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
+	_max_yaw_accel = _param_rm_max_yaw_accel.get() * M_DEG_TO_RAD_F;
 	_pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f);
 	_pid_yaw.setIntegralLimit(_max_yaw_rate);
 	_pid_yaw.setOutputLimit(_max_yaw_rate);
+
+	// Update slew rates
+	if (_max_yaw_rate > FLT_EPSILON) {
+		_yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate);
+	}
+
+	if (_max_yaw_accel > FLT_EPSILON) {
+		_yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel);
+	}
 }
 
 void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
@@ -79,98 +89,191 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
 
 	// Closed loop yaw control
 	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) {
+		_yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint), dt);
+		_rover_mecanum_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState());
 		_pid_yaw.setSetpoint(
-			matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw));  // error as setpoint to take care of wrapping
+			matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() -
+					vehicle_yaw));  // error as setpoint to take care of wrapping
 		_rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt);
+		_rover_mecanum_status.clyaw_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint;
 
 	} else {
 		_pid_yaw.resetIntegral();
+		_yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw);
 	}
 
 	// Yaw rate control
 	float speed_diff_normalized{0.f};
 
-	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { 	// Closed loop yaw rate control
+	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
-		if (_param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward
+		speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate,
-			const float speed_diff = _rover_mecanum_setpoint.yaw_rate_setpoint * _param_rm_wheel_track.get();
+					_param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
-			speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rm_max_thr_yaw_r.get(),
+					_pid_yaw_rate, false);
-						_param_rm_max_thr_yaw_r.get(), -1.f, 1.f);
+		_rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState();
-		}
-
-		_pid_yaw_rate.setSetpoint(_rover_mecanum_setpoint.yaw_rate_setpoint);
-		speed_diff_normalized = math::constrain(speed_diff_normalized +
-							_pid_yaw_rate.update(vehicle_yaw_rate, dt),
-							-1.f, 1.f); // Feedback
 
 	} else { // Use normalized setpoint
-		speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.speed_diff_setpoint_normalized) ? math::constrain(
+		speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized,
-						_rover_mecanum_setpoint.speed_diff_setpoint_normalized, -1.f, 1.f) : 0.f;
+					vehicle_yaw_rate,
+					_param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
+					_pid_yaw_rate, true);
 	}
 
 	// Speed control
-	float forward_throttle{0.f};
+	float forward_speed_normalized{0.f};
-	float lateral_throttle{0.f};
+	float lateral_speed_normalized{0.f};
 
 	if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint)
 	    && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control
-		// Closed loop forward speed control
+		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint,
-		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+					   vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle,
-			forward_throttle = math::interpolate<float>(_rover_mecanum_setpoint.forward_speed_setpoint,
+					   _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false);
-					   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
+		lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint,
-		}
+					   vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle,
+					   _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false);
+		_rover_mecanum_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState();
+		_rover_mecanum_status.adjusted_lateral_speed_setpoint = _lateral_speed_setpoint_with_accel_limit.getState();
 
-		_pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint);
-		forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt);
 
-		// Closed loop lateral speed control
+	} else if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized)
-		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+		   && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized)) { // Use normalized setpoint
-			lateral_throttle = math::interpolate<float>(_rover_mecanum_setpoint.lateral_speed_setpoint,
+		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint_normalized,
-					   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
+					   vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle,
-		}
+					   _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true);
+		lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized,
+					   vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle,
+					   _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true);
 
-		_pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint);
-		lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt);
-
-	} else { // Use normalized setpoint
-		forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain(
-					   _rover_mecanum_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
-		lateral_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized) ? math::constrain(
-					   _rover_mecanum_setpoint.lateral_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
 	}
 
 	// Publish rover mecanum status (logging)
-	rover_mecanum_status_s rover_mecanum_status{};
+	_rover_mecanum_status.timestamp = _timestamp;
-	rover_mecanum_status.timestamp = _timestamp;
+	_rover_mecanum_status.measured_forward_speed = vehicle_forward_speed;
-	rover_mecanum_status.measured_forward_speed = vehicle_forward_speed;
+	_rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed;
-	rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed;
+	_rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate;
-	rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint;
+	_rover_mecanum_status.measured_yaw = vehicle_yaw;
-	rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate;
+	_rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
-	rover_mecanum_status.measured_yaw = vehicle_yaw;
+	_rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral();
-	rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
+	_rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral();
-	rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral();
+	_rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral();
-	rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral();
+	_rover_mecanum_status_pub.publish(_rover_mecanum_status);
-	rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral();
-	_rover_mecanum_status_pub.publish(rover_mecanum_status);
 
 	// Publish to motors
 	actuator_motors_s actuator_motors{};
 	actuator_motors.reversible_flags = _param_r_rev.get();
-	computeInverseKinematics(forward_throttle, lateral_throttle,
+	computeInverseKinematics(forward_speed_normalized, lateral_speed_normalized,
 				 speed_diff_normalized).copyTo(actuator_motors.control);
 	actuator_motors.timestamp = _timestamp;
 	_actuator_motors_pub.publish(actuator_motors);
 
 }
 
-matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_throttle, float lateral_throttle,
+float RoverMecanumControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate,
+		const float max_thr_yaw_r,
+		const float max_yaw_accel, const float wheel_track, const float dt, SlewRate<float> &yaw_rate_with_accel_limit,
+		PID &pid_yaw_rate, 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 = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f;
+	}
+
+	if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+		yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization);
+		yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt);
+
+	} else {
+		yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint);
+	}
+
+	// Transform yaw rate into speed difference
+	float speed_diff_normalized{0.f};
+
+	if (normalized) {
+		speed_diff_normalized = yaw_rate_with_accel_limit.getState();
+
+	} else {
+		if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
+			const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track /
+						 2.f;
+			speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_yaw_r,
+						max_thr_yaw_r, -1.f, 1.f);
+		}
+
+		_pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState());
+		speed_diff_normalized = math::constrain(speed_diff_normalized +
+							_pid_yaw_rate.update(vehicle_yaw_rate, dt),
+							-1.f, 1.f); // Feedback
+
+
+	}
+
+	return math::constrain(speed_diff_normalized, -1.f, 1.f);
+
+}
+
+float RoverMecanumControl::calcNormalizedSpeedSetpoint(const float speed_setpoint,
+		const float vehicle_speed, const float max_thr_spd, SlewRate<float> &speed_setpoint_with_accel_limit,
+		PID &pid_throttle, const float max_accel, const float max_decel, 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 = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f;
+	}
+
+	// Apply acceleration and deceleration limit
+	if (fabsf(speed_setpoint) >= fabsf(speed_setpoint_with_accel_limit.getState())) {
+		if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+			speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization);
+			speed_setpoint_with_accel_limit.update(speed_setpoint, dt);
+
+		} else {
+			speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint);
+
+		}
+
+	} else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+		speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization);
+		speed_setpoint_with_accel_limit.update(speed_setpoint, dt);
+
+	} else {
+		speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint);
+	}
+
+	// Calculate normalized forward speed setpoint
+	float forward_speed_normalized{0.f};
+
+	if (normalized) {
+		forward_speed_normalized = speed_setpoint_with_accel_limit.getState();
+
+	} else { // Closed loop speed control
+
+		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+			forward_speed_normalized = math::interpolate<float>(speed_setpoint_with_accel_limit.getState(),
+						   -max_thr_spd, max_thr_spd,
+						   -1.f, 1.f);
+		}
+
+		pid_throttle.setSetpoint(speed_setpoint_with_accel_limit.getState());
+		forward_speed_normalized += pid_throttle.update(vehicle_speed, dt); // Feedback
+
+	}
+
+	return math::constrain(forward_speed_normalized, -1.f, 1.f);
+
+}
+
+matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_speed_normalized,
+		float lateral_speed_normalized,
 		float speed_diff)
 {
 	// Prioritize ratio between forward and lateral speed over either magnitude
-	float combined_speed =  fabsf(forward_throttle) + fabsf(lateral_throttle);
+	float combined_speed =  fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized);
 
 	if (combined_speed > 1.f) {
-		forward_throttle /= combined_speed;
+		forward_speed_normalized /= combined_speed;
-		lateral_throttle /= combined_speed;
+		lateral_speed_normalized /= combined_speed;
 		combined_speed = 1.f;
 	}
 
@@ -179,20 +282,21 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr
 
 	if (total_speed > 1.f) {
 		const float excess_velocity = fabsf(total_speed - 1.f);
-		const float forward_throttle_temp = forward_throttle - sign(forward_throttle) * 0.5f * excess_velocity;
+		const float forward_throttle_temp = forward_speed_normalized - sign(forward_speed_normalized) * 0.5f * excess_velocity;
-		const float lateral_throttle_temp = lateral_throttle - sign(lateral_throttle) * 0.5f * excess_velocity;
+		const float lateral_throttle_temp = lateral_speed_normalized - sign(lateral_speed_normalized) * 0.5f * excess_velocity;
 
-		if (sign(forward_throttle_temp) == sign(forward_throttle) && sign(lateral_throttle) == sign(lateral_throttle_temp)) {
+		if (sign(forward_throttle_temp) == sign(forward_speed_normalized)
-			forward_throttle = forward_throttle_temp;
+		    && sign(lateral_speed_normalized) == sign(lateral_throttle_temp)) {
-			lateral_throttle = lateral_throttle_temp;
+			forward_speed_normalized = forward_throttle_temp;
+			lateral_speed_normalized = lateral_throttle_temp;
 
 		} else {
-			forward_throttle = lateral_throttle = 0.f;
+			forward_speed_normalized = lateral_speed_normalized = 0.f;
 		}
 	}
 
 	// Calculate motor commands
-	const float input_data[3] = {forward_throttle, lateral_throttle, speed_diff};
+	const float input_data[3] = {forward_speed_normalized, lateral_speed_normalized, speed_diff};
 	const Matrix<float, 3, 1> input(input_data);
 	const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f};
 	const Matrix<float, 4, 3> m(m_data);

src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp

@@ -43,7 +43,8 @@
 #include <uORB/topics/rover_mecanum_setpoint.h>
 #include <uORB/topics/rover_mecanum_status.h>
 #include <uORB/topics/actuator_motors.h>
-
+#include <lib/slew_rate/SlewRate.hpp>
+#include <lib/slew_rate/SlewRateYaw.hpp>
 
 // Standard libraries
 #include <lib/pid/PID.hpp>
@@ -89,6 +90,39 @@ protected:
 	void updateParams() override;
 
 private:
+	/**
+	 * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates.
+	 * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]].
+	 * @param vehicle_yaw_rate Measured yaw rate [rad/s].
+	 * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s].
+	 * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2].
+	 * @param wheel_track Wheel track [m].
+	 * @param dt Time since last update [s].
+	 * @param yaw_rate_with_accel_limit Yaw rate slew rate.
+	 * @param pid_yaw_rate Yaw rate PID
+	 * @param normalized Indicates if the forward speed setpoint is already normalized.
+	 * @return Normalized speed differece setpoint with applied slew rates [-1, 1].
+	 */
+	float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel,
+				      float wheel_track, float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized);
+	/**
+	 * @brief Compute normalized speed setpoint and apply slew rates.
+	 * to the speed setpoint and doing closed loop speed control if enabled.
+	 * @param speed_setpoint Speed setpoint [m/s].
+	 * @param vehicle_speed Actual speed [m/s].
+	 * @param max_thr_spd Speed the rover drives at maximum throttle [m/s].
+	 * @param speed_setpoint_with_accel_limit Speed slew rate.
+	 * @param pid_throttle Throttle PID
+	 * @param max_accel Maximum acceleration [m/s^2]
+	 * @param max_decel Maximum deceleration [m/s^2]
+	 * @param dt Time since last update [s].
+	 * @param normalized Indicates if the speed setpoint is already normalized.
+	 * @return Normalized speed setpoint with applied slew rates [-1, 1].
+	 */
+	float calcNormalizedSpeedSetpoint(float speed_setpoint, float vehicle_forward_speed, float max_thr_spd,
+					  SlewRate<float> &speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel,
+					  float dt, bool normalized);
+
 	/**
 	 * @brief Turn normalized speed setpoints into normalized motor commands.
 	 *
@@ -105,30 +139,39 @@ private:
 	// uORB publications
 	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
 	uORB::Publication<rover_mecanum_status_s> _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)};
+	rover_mecanum_status_s _rover_mecanum_status{};
 
 	// Variables
 	rover_mecanum_setpoint_s _rover_mecanum_setpoint{};
 	hrt_abstime _timestamp{0};
 	float _max_yaw_rate{0.f};
+	float _max_yaw_accel{0.f};
 
 	// Controllers
 	PID _pid_forward_throttle; // PID for the closed loop forward speed control
 	PID _pid_lateral_throttle; // PID for the closed loop lateral speed control
 	PID _pid_yaw; // PID for the closed loop yaw control
 	PID _pid_yaw_rate; // PID for the closed loop yaw rate control
+	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
+	SlewRate<float> _lateral_speed_setpoint_with_accel_limit{0.f};
+	SlewRate<float> _yaw_rate_with_accel_limit{0.f};
+	SlewRateYaw<float> _yaw_setpoint_with_yaw_rate_limit;
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
+		(ParamFloat<px4::params::RM_WHEEL_TRACK>)   _param_rm_wheel_track,
-		(ParamFloat<px4::params::RM_MAX_THR_SPD>) _param_rm_max_thr_spd,
+		(ParamFloat<px4::params::RM_MAX_THR_SPD>)   _param_rm_max_thr_spd,
+		(ParamFloat<px4::params::RM_MAX_ACCEL>)     _param_rm_max_accel,
+		(ParamFloat<px4::params::RM_MAX_DECEL>)     _param_rm_max_decel,
 		(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
-		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>)  _param_rm_max_yaw_rate,
-		(ParamFloat<px4::params::RM_YAW_RATE_P>) _param_rm_yaw_rate_p,
+		(ParamFloat<px4::params::RM_MAX_YAW_ACCEL>) _param_rm_max_yaw_accel,
-		(ParamFloat<px4::params::RM_YAW_RATE_I>) _param_rm_yaw_rate_i,
+		(ParamFloat<px4::params::RM_YAW_RATE_P>)    _param_rm_yaw_rate_p,
-		(ParamFloat<px4::params::RM_SPEED_P>) _param_rm_p_gain_speed,
+		(ParamFloat<px4::params::RM_YAW_RATE_I>)    _param_rm_yaw_rate_i,
-		(ParamFloat<px4::params::RM_SPEED_I>) _param_rm_i_gain_speed,
+		(ParamFloat<px4::params::RM_SPEED_P>)       _param_rm_p_gain_speed,
-		(ParamFloat<px4::params::RM_YAW_P>) _param_rm_p_gain_yaw,
+		(ParamFloat<px4::params::RM_SPEED_I>)       _param_rm_i_gain_speed,
-		(ParamFloat<px4::params::RM_YAW_I>) _param_rm_i_gain_yaw,
+		(ParamFloat<px4::params::RM_YAW_P>)         _param_rm_p_gain_yaw,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamFloat<px4::params::RM_YAW_I>)         _param_rm_i_gain_yaw,
+		(ParamInt<px4::params::CA_R_REV>)           _param_r_rev
 	)
 };

src/modules/rover_mecanum/module.yaml

@@ -29,6 +29,22 @@ parameters:
         decimal: 2
         default: 90
 
+      RM_MAX_YAW_ACCEL:
+        description:
+          short: Maximum allowed yaw acceleration for the rover
+          long: |
+            This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints
+            to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change.
+            This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track.
+            Set to -1 to disable.
+        type: float
+        unit: deg/s^2
+        min: -1
+        max: 1000
+        increment: 0.01
+        decimal: 2
+        default: -1
+
       RM_MAX_THR_YAW_R:
         description:
           short: Yaw rate turning left/right wheels at max speed in opposite directions
@@ -160,6 +176,21 @@ parameters:
         decimal: 2
         default: 0.5
 
+      RM_MAX_DECEL:
+        description:
+          short: Maximum deceleration
+          long: |
+            Maximum decelaration is used to limit the deceleration of the rover.
+            Set to -1 to disable, causing the rover to decelerate as fast as possible.
+            Caution: This disables the slow down effect in auto modes.
+        type: float
+        unit: m/s^2
+        min: -1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: -1
+
       RM_MISS_VEL_GAIN:
         description:
           short: Tuning parameter for the velocity reduction during waypoint transition