ackermann: new features and improvements

added return mode support, slew rates for actuators, new ackermann specific message, improved cornering slow down effect and fixed logging issue.

ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps

@@ -1,9 +1,6 @@
 #!/bin/sh
 # Standard apps for a ackermann drive rover.
 
-# Start the attitude and position estimator.
-ekf2 start &
-
 # Start rover ackermann drive controller.
 rover_ackermann start
 

msg/CMakeLists.txt

@@ -180,6 +180,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	RoverAckermannGuidanceStatus.msg
+	RoverAckermannStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg

msg/RoverAckermannGuidanceStatus.msg

@@ -1,10 +1,9 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 actual_speed # [m/s] Rover ground speed
+float32 desired_speed 		# [m/s] Rover desired ground speed
-float32 desired_speed # [m/s] Rover desired ground speed
+float32 lookahead_distance 	# [m] Lookahead distance of pure the pursuit controller
-float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
+float32 heading_error 		# [deg] Heading error of the pure pursuit controller
-float32 heading_error # [deg] Heading error of the pure pursuit controller
+float32 pid_throttle_integral 	# [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions
-float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions
+float32 crosstrack_error 	# [m] Shortest distance from the vehicle to the path
-float32 crosstrack_error # [m] Shortest distance from the vehicle to the path
 
 # TOPICS rover_ackermann_guidance_status

msg/RoverAckermannStatus.msg

@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 throttle_setpoint 	# [-1, 1] Normalized throttle setpoint
+float32 steering_setpoint 	# [-1, 1] Normalized steering setpoint
+float32 actual_speed       	# [m/s] Rover ground speed
+
+# TOPICS rover_ackermann_status

src/modules/logger/logged_topics.cpp

@@ -103,7 +103,8 @@ void LoggedTopics::add_default_topics()
 	add_topic("position_setpoint_triplet", 200);
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
-	add_topic("rover_ackermann_guidance_status", 100);
+	add_optional_topic("rover_ackermann_guidance_status", 100);
+	add_optional_topic("rover_ackermann_status", 100);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);
 	add_optional_topic("sensor_airflow", 100);

src/modules/rover_ackermann/CMakeLists.txt

@@ -42,6 +42,7 @@ px4_add_module(
 	DEPENDS
 		RoverAckermannGuidance
 		px4_work_queue
+		SlewRate
 	MODULE_CONFIG
 		module.yaml
 )

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -40,6 +40,7 @@ RoverAckermann::RoverAckermann() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
+	_rover_ackermann_status_pub.advertise();
 	updateParams();
 }
 
@@ -52,6 +53,16 @@ bool RoverAckermann::init()
 void RoverAckermann::updateParams()
 {
 	ModuleParams::updateParams();
+
+	// Update slew rates
+	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
+		_throttle_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 RoverAckermann::Run()
@@ -59,6 +70,7 @@ void RoverAckermann::Run()
 	if (should_exit()) {
 		ScheduleClear();
 		exit_and_cleanup();
+		return;
 	}
 
 	// uORB subscriber updates
@@ -70,44 +82,98 @@ void RoverAckermann::Run()
 		vehicle_status_s vehicle_status;
 		_vehicle_status_sub.copy(&vehicle_status);
 		_nav_state = vehicle_status.nav_state;
+		_armed = vehicle_status.arming_state == 2;
 	}
 
-	// Navigation modes
+	if (_local_position_sub.updated()) {
-	switch (_nav_state) {
+		vehicle_local_position_s local_position{};
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
+		_local_position_sub.copy(&local_position);
-			manual_control_setpoint_s manual_control_setpoint{};
+		const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
+		_actual_speed = rover_velocity.norm();
+	}
 
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+	// Timestamps
-				_motor_setpoint.steering = manual_control_setpoint.roll;
+	hrt_abstime timestamp_prev = _timestamp;
-				_motor_setpoint.throttle =  manual_control_setpoint.throttle;
+	_timestamp = hrt_absolute_time();
-			}
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
-		} break;
+	if (_armed) {
+		// Navigation modes
+		switch (_nav_state) {
+		case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
+				manual_control_setpoint_s manual_control_setpoint{};
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+				if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-		_motor_setpoint = _ackermann_guidance.purePursuit();
+					_motor_setpoint.steering = manual_control_setpoint.roll;
-		break;
+					_motor_setpoint.throttle =  manual_control_setpoint.throttle;
+				}
 
-	default: // Unimplemented nav states will stop the rover
+			} break;
+
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+			_motor_setpoint = _ackermann_guidance.purePursuit(_nav_state);
+			break;
+
+		default: // Unimplemented nav states will stop the rover
+			_motor_setpoint.steering = 0.f;
+			_motor_setpoint.throttle =  0.f;
+			break;
+		}
+
+		// Sanitize actuator commands
+		if (!PX4_ISFINITE(_motor_setpoint.steering)) {
+			_motor_setpoint.steering = 0.f;
+		}
+
+		if (!PX4_ISFINITE(_motor_setpoint.throttle)) {
+			_motor_setpoint.throttle = 0.f;
+		}
+
+		// Acceleration slew rate
+		if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
+		    && fabsf(_motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
+			_throttle_with_accel_limit.update(_motor_setpoint.throttle, dt);
+
+		} else {
+			_throttle_with_accel_limit.setForcedValue(_motor_setpoint.throttle);
+		}
+
+		// Steering slew rate
+		if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
+			_steering_with_rate_limit.update(_motor_setpoint.steering, dt);
+
+		} else {
+			_steering_with_rate_limit.setForcedValue(_motor_setpoint.steering);
+		}
+
+		// Publish rover ackermann status (logging)
+		rover_ackermann_status_s rover_ackermann_status{};
+		rover_ackermann_status.timestamp = _timestamp;
+		rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
+		rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
+		rover_ackermann_status.actual_speed = _actual_speed;
+		_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] = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
+		actuator_motors.timestamp = _timestamp;
+		_actuator_motors_pub.publish(actuator_motors);
+
+		// Publish to servo
+		actuator_servos_s actuator_servos{};
+		actuator_servos.control[0] = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
+		actuator_servos.timestamp = _timestamp;
+		_actuator_servos_pub.publish(actuator_servos);
+
+	} else { // Reset on disarm
 		_motor_setpoint.steering = 0.f;
 		_motor_setpoint.throttle =  0.f;
-		break;
+		_throttle_with_accel_limit.setForcedValue(0.f);
+		_steering_with_rate_limit.setForcedValue(0.f);
 	}
-
-	hrt_abstime now = hrt_absolute_time();
-
-	// Publish to wheel motors
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get();
-	actuator_motors.control[0] = _motor_setpoint.throttle;
-	actuator_motors.timestamp = now;
-	_actuator_motors_pub.publish(actuator_motors);
-
-	// Publish to servo
-	actuator_servos_s actuator_servos{};
-	actuator_servos.control[0] = _motor_setpoint.steering;
-	actuator_servos.timestamp = now;
-	_actuator_servos_pub.publish(actuator_servos);
 }
 
 int RoverAckermann::task_spawn(int argc, char *argv[])
@@ -147,7 +213,7 @@ int RoverAckermann::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-Rover state machine.
+Rover ackermann module.
 )DESCR_STR");
 
 	PRINT_MODULE_USAGE_NAME("rover_ackermann", "controller");

src/modules/rover_ackermann/RoverAckermann.hpp

@@ -39,6 +39,7 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/slew_rate/SlewRate.hpp>
 
 // uORB includes
 #include <uORB/Publication.hpp>
@@ -49,6 +50,9 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/actuator_servos.h>
+#include <uORB/topics/rover_ackermann_status.h>
+#include <uORB/topics/vehicle_local_position.h>
+
 
 // Standard library includes
 #include <math.h>
@@ -89,10 +93,12 @@ private:
 	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 _local_position_sub{ORB_ID(vehicle_local_position)};
 
 	// 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)};
 
 	// Instances
 	RoverAckermannGuidance _ackermann_guidance{this};
@@ -100,9 +106,18 @@ private:
 	// Variables
 	int _nav_state{0};
 	RoverAckermannGuidance::motor_setpoint _motor_setpoint;
+	hrt_abstime _timestamp{0};
+	float _actual_speed{0.f};
+	SlewRate<float> _steering_with_rate_limit{0.f};
+	SlewRate<float> _throttle_with_accel_limit{0.f};
+	bool _armed{false};
 
 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(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_MAX_SPEED>) _param_ra_max_speed,
+		(ParamFloat<px4::params::RA_MAX_ACCEL>) _param_ra_max_accel,
+		(ParamFloat<px4::params::RA_MAX_STR_RATE>) _param_ra_max_steering_rate
 	)
 };

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp

@@ -40,6 +40,7 @@ using namespace time_literals;
 
 RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent)
 {
+	_rover_ackermann_guidance_status_pub.advertise();
 	updateParams();
 	pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
 }
@@ -55,13 +56,14 @@ void RoverAckermannGuidance::updateParams()
 			   1); // Output limit
 }
 
-RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit()
+RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const int nav_state)
 {
 	// Initializations
 	float desired_speed{0.f};
 	float desired_steering{0.f};
 	float vehicle_yaw{0.f};
 	float actual_speed{0.f};
+	bool mission_finished{false};
 
 	// uORB subscriber updates
 	if (_vehicle_global_position_sub.updated()) {
@@ -84,6 +86,12 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit()
 		actual_speed = rover_velocity.norm();
 	}
 
+	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()) {
 		updateWaypoints();
 	}
@@ -98,21 +106,58 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit()
 	if (_mission_result_sub.updated()) {
 		mission_result_s mission_result{};
 		_mission_result_sub.copy(&mission_result);
-		_mission_finished = mission_result.finished;
+		mission_finished = mission_result.finished;
+	}
+
+	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
+	    && get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _next_wp(0),
+					     _next_wp(1)) < _acceptance_radius) { // Return to launch
+		mission_finished = true;
 	}
 
 	// Guidance logic
-	if (!_mission_finished) {
+	if (!mission_finished) {
 		// Calculate desired speed
 		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));
 
-		if (distance_to_prev_wp <= _prev_acc_rad) { // Cornering speed
+		if (_param_ra_miss_vel_min.get() > 0.f  && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get()
-			const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acc_rad;
+		    && _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect
-			desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
+			if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) {
+				const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acceptance_radius;
+				desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
 
-		} else { // Default mission speed
+			} else if (distance_to_curr_wp <= _acceptance_radius && _acceptance_radius > FLT_EPSILON) {
+				const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius;
+				desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
+
+			} else { // Straight line speed
+				if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_jerk.get() > FLT_EPSILON
+				    && _acceptance_radius > FLT_EPSILON) {
+					float max_velocity{0.f};
+
+					if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+						max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(),
+								_param_ra_max_accel.get(), distance_to_curr_wp, 0.f);
+
+					} else {
+						const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius;
+						max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(),
+								_param_ra_max_accel.get(), distance_to_curr_wp - _acceptance_radius, cornering_speed);
+					}
+
+					desired_speed = math::constrain(max_velocity, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
+
+				} else {
+					desired_speed = _param_ra_miss_vel_def.get();
+				}
+			}
+
+		} else {
 			desired_speed = _param_ra_miss_vel_def.get();
 		}
 
@@ -124,9 +169,9 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit()
 	}
 
 	// Throttle PID
-	hrt_abstime now = hrt_absolute_time();
+	hrt_abstime timestamp_prev = _timestamp;
-	const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
+	_timestamp = hrt_absolute_time();
-	_time_stamp_last = now;
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 	float throttle = 0.f;
 
 	if (desired_speed < FLT_EPSILON) {
@@ -144,8 +189,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit()
 	}
 
 	// Publish ackermann controller status (logging)
-	_rover_ackermann_guidance_status.timestamp = now;
+	_rover_ackermann_guidance_status.timestamp = _timestamp;
-	_rover_ackermann_guidance_status.actual_speed = actual_speed;
 	_rover_ackermann_guidance_status.desired_speed = desired_speed;
 	_rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral;
 	_rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status);
@@ -165,7 +209,12 @@ void RoverAckermannGuidance::updateWaypoints()
 	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
 
 	// Global waypoint coordinates
-	_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
+	if (position_setpoint_triplet.current.valid) {
+		_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
+
+	} else {
+		_curr_wp = Vector2d(0, 0);
+	}
 
 	if (position_setpoint_triplet.previous.valid) {
 		_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
@@ -178,7 +227,7 @@ void RoverAckermannGuidance::updateWaypoints()
 		_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
 
 	} else {
-		_next_wp = _curr_wp;
+		_next_wp = _home_position; // Enables corner slow down with RTL
 	}
 
 	// Local waypoint coordinates
@@ -187,9 +236,15 @@ void RoverAckermannGuidance::updateWaypoints()
 	_next_wp_local = _global_local_proj_ref.project(_next_wp(0), _next_wp(1));
 
 	// Update acceptance radius
-	_prev_acc_rad = _acceptance_radius;
+	_prev_acceptance_radius = _acceptance_radius;
-	_acceptance_radius = updateAcceptanceRadius(_curr_wp_local, _prev_wp_local, _next_wp_local, _param_ra_acc_rad_def.get(),
+
-			     _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get());
+	if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
+		_acceptance_radius = updateAcceptanceRadius(_curr_wp_local, _prev_wp_local, _next_wp_local, _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();
+	}
 }
 
 float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -43,10 +43,11 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_attitude.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>
 
 // Standard library includes
 #include <matrix/matrix/math.hpp>
@@ -78,8 +79,9 @@ public:
 
 	/**
 	 * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering.
+	 * @param nav_state Vehicle navigation state
 	 */
-	motor_setpoint purePursuit();
+	motor_setpoint purePursuit(const int nav_state);
 
 	/**
 	 * @brief Update global/local waypoint coordinates and acceptance radius
@@ -143,6 +145,7 @@ private:
 	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
 
 	// uORB publications
 	uORB::Publication<rover_ackermann_guidance_status_s> _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)};
@@ -156,18 +159,18 @@ private:
 	Vector2d _curr_pos{};
 	Vector2f _curr_pos_local{};
 	PID_t _pid_throttle;
-	hrt_abstime _time_stamp_last{0};
+	hrt_abstime _timestamp{0};
 
 	// Waypoint variables
 	Vector2d _curr_wp{};
 	Vector2d _next_wp{};
 	Vector2d _prev_wp{};
+	Vector2d _home_position{};
 	Vector2f _curr_wp_local{};
 	Vector2f _prev_wp_local{};
 	Vector2f _next_wp_local{};
 	float _acceptance_radius{0.5f};
-	float _prev_acc_rad{0.f};
+	float _prev_acceptance_radius{0.5f};
-	bool _mission_finished{false};
 
 	// Parameters
 	DEFINE_PARAMETERS(
@@ -177,7 +180,6 @@ private:
 		(ParamFloat<px4::params::RA_LOOKAHD_MAX>) _param_ra_lookahd_max,
 		(ParamFloat<px4::params::RA_LOOKAHD_MIN>) _param_ra_lookahd_min,
 		(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
-		(ParamFloat<px4::params::RA_ACC_RAD_DEF>) _param_ra_acc_rad_def,
 		(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
 		(ParamFloat<px4::params::RA_MISS_VEL_DEF>) _param_ra_miss_vel_def,
 		(ParamFloat<px4::params::RA_MISS_VEL_MIN>) _param_ra_miss_vel_min,
@@ -185,6 +187,8 @@ private:
 		(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_ACCEL>) _param_ra_max_accel,
 		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
 	)
 };

src/modules/rover_ackermann/module.yaml

@@ -66,31 +66,20 @@ parameters:
 
       RA_ACC_RAD_MAX:
         description:
-          short: Maximum acceptance radius
+          short: Maximum acceptance radius for the waypoints
           long: |
             The controller scales the acceptance radius based on the angle between
-            the previous, current and next waypoint. Used as tuning parameter.
+            the previous, current and next waypoint.
             Higher value -> smoother trajectory at the cost of how close the rover gets
-            to the waypoint (Set equal to RA_ACC_RAD_DEF to disable corner cutting).
+            to the waypoint (Set to -1 to disable corner cutting).
         type: float
         unit: m
-        min: 0.1
+        min: -1
         max: 100
         increment: 0.01
         decimal: 2
         default: 3
 
-      RA_ACC_RAD_DEF:
-        description:
-          short: Default acceptance radius
-        type: float
-        unit: m
-        min: 0.1
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: 0.5
-
       RA_ACC_RAD_GAIN:
         description:
           short: Tuning parameter for corner cutting
@@ -110,21 +99,21 @@ parameters:
           short: Default rover velocity during a mission
         type: float
         unit: m/s
-        min: 0.1
+        min: 0
         max: 100
         increment: 0.01
         decimal: 2
-        default: 3
+        default: 2
 
       RA_MISS_VEL_MIN:
         description:
           short: Minimum rover velocity during a mission
           long: |
             The velocity off the rover is reduced based on the corner it has to take
-            to smooth the trajectory (To disable this feature set it equal to RA_MISSION_VEL_DEF)
+            to smooth the trajectory (Set to -1 to disable)
         type: float
         unit: m/s
-        min: 0.1
+        min: -1
         max: 100
         increment: 0.01
         decimal: 2
@@ -133,9 +122,12 @@ parameters:
       RA_MISS_VEL_GAIN:
         description:
           short: Tuning parameter for the velocity reduction during cornering
-          long: Lower value -> More velocity reduction during cornering
+          long: |
+            The cornering speed is equal to the inverse of the acceptance radius
+            of the WP multiplied with this factor.
+            Lower value -> More velocity reduction during cornering.
         type: float
-        min: 0.1
+        min: 0.05
         max: 100
         increment: 0.01
         decimal: 2
@@ -175,3 +167,46 @@ parameters:
         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, 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, RA_MISS_VEL_GAIN and
+            RA_MISS_VEL_MIN also have 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_ACCEL, RA_MISS_VEL_GAIN and RA_MISS_VEL_MIN also have 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