ackermann: add lateral acceleration setpoint, acro/position mode and updates to auto modes

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

@@ -23,8 +23,6 @@ param set-default RA_MAX_THR_SPEED 6
 param set-default RA_MAX_STR_ANG 0.5236
 param set-default RA_MAX_STR_RATE 360
 param set-default RA_MISS_SPD_DEF 5
-param set-default RA_MISS_SPD_GAIN 5
-param set-default RA_MISS_SPD_MIN 1
 param set-default RA_SPEED_I 0.01
 param set-default RA_SPEED_P 0.1
 param set-default RA_WHEEL_BASE 0.321

msg/RoverAckermannSetpoint.msg

@@ -1,8 +1,9 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 forward_speed_setpoint 		  # [m/s] Desired forward speed for the rover
-float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward speed for the rover
-float32 steering_setpoint      		  # [rad/s] Desired steering for the rover
-float32 steering_setpoint_normalized      # [-1, 1] Desired normalized steering for the rover
+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

@@ -4,6 +4,8 @@ float32 measured_forward_speed       	      # [m/s] Measured speed in body x dir
 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

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -38,6 +38,9 @@ RoverAckermann::RoverAckermann() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	_rover_ackermann_setpoint_pub.advertise();
+	_ax_filter.setAlpha(0.05);
+	_ay_filter.setAlpha(0.05);
+	_az_filter.setAlpha(0.05);
 	updateParams();
 }
 
@@ -76,6 +79,68 @@ void RoverAckermann::Run()
 				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);
 			}
 
@@ -93,6 +158,7 @@ void RoverAckermann::Run()
 		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;
 	}
@@ -101,7 +167,7 @@ void RoverAckermann::Run()
 		_ackermann_control.resetControllers();
 	}
 
-	_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw);
+	_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw, _vehicle_lateral_acceleration);
 
 }
 
@@ -114,6 +180,12 @@ void RoverAckermann::updateSubscriptions()
 	if (_vehicle_status_sub.updated()) {
 		vehicle_status_s vehicle_status;
 		_vehicle_status_sub.copy(&vehicle_status);
+
+		if (vehicle_status.nav_state != _nav_state) { // Reset on mode change
+			_ackermann_control.resetControllers();
+			_course_control = false;
+		}
+
 		_nav_state = vehicle_status.nav_state;
 		_armed = vehicle_status.arming_state == 2;
 	}
@@ -128,9 +200,26 @@ void RoverAckermann::updateSubscriptions()
 	if (_vehicle_local_position_sub.updated()) {
 		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)) {
+			_ay_filter.update(vehicle_local_position.ay);
+		}
+
+		if (PX4_ISFINITE(vehicle_local_position.az)) {
+			_az_filter.update(vehicle_local_position.az);
+		}
+
+		_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_forward_speed = velocity_in_body_frame(0);
+		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
+		Vector3f acceleration_in_local_frame(_ax_filter.getState(), _ay_filter.getState(), _az_filter.getState());
+		Vector3f acceleration_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(acceleration_in_local_frame);
+		_vehicle_lateral_acceleration = acceleration_in_body_frame(1);
 	}
 }
 

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/pure_pursuit/PurePursuit.hpp>
 
 // uORB includes
 #include <uORB/Publication.hpp>
@@ -54,6 +55,7 @@
 // Standard library includes
 #include <math.h>
 #include <matrix/matrix/math.hpp>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 
 // Local includes
 #include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
@@ -61,6 +63,12 @@
 
 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
 {
@@ -107,6 +115,7 @@ private:
 	// Class instances
 	RoverAckermannGuidance _ackermann_guidance{this};
 	RoverAckermannControl _ackermann_control{this};
+	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
 
 	// Variables
 	matrix::Quatf _vehicle_attitude_quaternion{};
@@ -114,5 +123,23 @@ private:
 	float _vehicle_forward_speed{0.f};
 	float _vehicle_yaw{0.f};
 	bool _armed{false};
+	bool _course_control{false};
+	Vector2f _pos_ctl_course_direction{};
+	Vector2f _pos_ctl_start_position_ned{};
+	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,
+		(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_LAT_ACCEL>) _param_ra_max_lat_accel,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
+
+	)
 
 };

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp

@@ -50,8 +50,15 @@ void RoverAckermannControl::updateParams()
 	ModuleParams::updateParams();
 
 	pid_set_parameters(&_pid_throttle,
-			   _param_ra_p_speed.get(), // Proportional gain
-			   _param_ra_i_speed.get(), // Integral gain
+			   _param_ra_speed_p.get(), // Proportional gain
+			   _param_ra_speed_i.get(), // Integral gain
+			   0, // Derivative gain
+			   1, // Integral limit
+			   1); // Output limit
+
+	pid_set_parameters(&_pid_lat_accel,
+			   _param_ra_lat_accel_p.get(), // Proportional gain
+			   _param_ra_lat_accel_i.get(), // Integral gain
 			   0, // Derivative gain
 			   1, // Integral limit
 			   1); // Output limit
@@ -67,12 +74,13 @@ void RoverAckermannControl::updateParams()
 	}
 }
 
-void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_speed, const float vehicle_yaw)
+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, 5000_ms) * 1e-6f;
+	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);
@@ -90,6 +98,37 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe
 
 	}
 
+	// 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)
+				steering_setpoint += pid_calculate(&_pid_lat_accel, _rover_ackermann_setpoint.lateral_acceleration_setpoint,
+								   vehicle_lateral_acceleration, 0, 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};
 
@@ -116,7 +155,9 @@ void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_spe
 	_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.integral;
+	_rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.integral;
 	_rover_ackermann_status_pub.publish(_rover_ackermann_status);
 
 	// Publish to motor
@@ -188,4 +229,8 @@ float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_spe
 void RoverAckermannControl::resetControllers()
 {
 	pid_reset_integral(&_pid_throttle);
+	pid_reset_integral(&_pid_lat_accel);
+	_forward_speed_setpoint_with_accel_limit.setForcedValue(0.f);
+	_steering_with_rate_limit.setForcedValue(0.f);
+
 }

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp

@@ -71,8 +71,11 @@ public:
 
 	/**
 	 * @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);
+	void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw, float vehicle_lateral_acceleration);
 
 	/**
 	 * @brief Reset PID controllers and slew rates
@@ -112,19 +115,23 @@ private:
 
 	// Controllers
 	PID_t _pid_throttle; // The PID controller for the closed loop speed control
+	PID_t _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_p_speed,
-		(ParamFloat<px4::params::RA_SPEED_I>) _param_ra_i_speed,
+		(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

@@ -48,6 +48,10 @@ 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,
@@ -60,8 +64,6 @@ void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed,
 					  _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));
-	// const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
-	// 				  _next_wp(0), _next_wp(1));
 
 	// Catch return to launch
 	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
@@ -78,8 +80,8 @@ void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed,
 
 	} else { // Regular guidance algorithm
 
-		_desired_speed = calcDesiredSpeed(_cruising_speed, _param_ra_miss_spd_min.get(),
-						  _param_ra_miss_spd_gain.get(), distance_to_prev_wp, distance_to_curr_wp, _acceptance_radius,
+		_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());
 
@@ -98,8 +100,10 @@ void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed,
 	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 = _desired_steering;
+	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);
 
 }
@@ -231,24 +235,24 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const float waypoint_transi
 }
 
 float RoverAckermannGuidance::calcDesiredSpeed(const float cruising_speed, const float miss_speed_min,
-		const float miss_speed_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad,
+		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 < 0.f  || miss_speed_min > cruising_speed || miss_speed_gain < FLT_EPSILON) {
+	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 cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - prev_waypoint_transition_angle,
-					      0.f, M_PI_F, 0.f, 1.f);
+		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 cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
-					      M_PI_F, 0.f, 1.f);
+		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);
 
 	}
@@ -262,8 +266,8 @@ float RoverAckermannGuidance::calcDesiredSpeed(const float cruising_speed, const
 					      max_decel, distance_to_curr_wp, 0.f);
 
 		} else {
-			float cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, M_PI_F,
-						0.f, 1.f);
+			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);

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -83,6 +83,24 @@ public:
 	 */
 	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.
@@ -123,7 +141,6 @@ private:
 	 * maximum acceleration and jerk.
 	 * @param cruising_speed Cruising speed [m/s].
 	 * @param miss_speed_min Minimum speed setpoint [m/s].
-	 * @param miss_speed_gain Tuning parameter for the slow down effect during cornering [-].
 	 * @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].
@@ -136,28 +153,10 @@ private:
 	 * @param max_speed Maximum speed setpoint [m/s]
 	 * @return Speed setpoint [m/s].
 	 */
-	float calcDesiredSpeed(float cruising_speed, float miss_speed_min, float miss_speed_gain, float distance_to_prev_wp,
+	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);
 
-	/**
-	 * @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);
-
 	// uORB subscriptions
 	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
@@ -197,6 +196,7 @@ private:
 	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 	float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
 	uint _nav_cmd{0};
+	float _min_speed{0.f};
 
 	// Parameters
 	DEFINE_PARAMETERS(
@@ -205,13 +205,12 @@ private:
 		(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_MISS_SPD_DEF>) _param_ra_miss_spd_def,
-		(ParamFloat<px4::params::RA_MISS_SPD_MIN>) _param_ra_miss_spd_min,
-		(ParamFloat<px4::params::RA_MISS_SPD_GAIN>) _param_ra_miss_spd_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

@@ -68,34 +68,6 @@ parameters:
         decimal: 2
         default: 2
 
-      RA_MISS_SPD_MIN:
-        description:
-          short: Minimum rover speed during a mission
-          long: |
-            The speed off the rover is reduced based on the corner it has to take
-            to smooth the trajectory (Set to -1 to disable)
-        type: float
-        unit: m/s
-        min: -1
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: 1
-
-      RA_MISS_SPD_GAIN:
-        description:
-          short: Tuning parameter for the speed 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 speed reduction during cornering.
-        type: float
-        min: 0.05
-        max: 100
-        increment: 0.01
-        decimal: 2
-        default: 5
-
       RA_SPEED_P:
         description:
           short: Proportional gain for ground speed controller
@@ -168,8 +140,7 @@ parameters:
           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, RA_MISS_VEL_GAIN and
-            RA_MISS_VEL_MIN also have to be set.
+            Note: For the corner slow down effect RA_MAX_JERK also has to be set.
         type: float
         unit: m/s^2
         min: -1
@@ -184,8 +155,7 @@ parameters:
           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.
+            Note: RA_MAX_ACCEL also has to be set for this to be enabled.
         type: float
         unit: m/s^3
         min: -1
@@ -204,3 +174,37 @@ parameters:
         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