ackermann: improved cornering logic

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp

@@ -114,9 +114,18 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
 		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
-			const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acc_rad;
+		if (distance_to_prev_wp <= _prev_acceptance_radius * _param_ra_vel_red_start.get()
+		    && _prev_acceptance_radius > FLT_EPSILON) { // Cornering speed
+			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 if (distance_to_curr_wp <= _acceptance_radius * _param_ra_vel_red_start.get()
+			   && _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 { // Default mission speed
@@ -193,7 +202,7 @@ 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());
 }

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -167,7 +167,7 @@ class RoverAckermannGuidance : public ModuleParams
 	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};
 
 	// Parameters
 	DEFINE_PARAMETERS(
@@ -182,6 +182,7 @@ class RoverAckermannGuidance : public ModuleParams
 		(ParamFloat<px4::params::RA_MISS_VEL_DEF>) _param_ra_miss_vel_def,
 		(ParamFloat<px4::params::RA_MISS_VEL_MIN>) _param_ra_miss_vel_min,
 		(ParamFloat<px4::params::RA_MISS_VEL_GAIN>) _param_ra_miss_vel_gain,
+		(ParamFloat<px4::params::RA_VEL_RED_START>) _param_ra_vel_red_start,
 		(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,

src/modules/rover_ackermann/module.yaml

@@ -141,6 +141,21 @@ parameters:
         decimal: 2
         default: 5
 
+      RA_VEL_RED_START:
+        description:
+          short: Scaling factor on the distance to the WP at which the cornering slow down effect starts
+          long: |
+            This is a multiplicative factor on the acceptance radius of the waypoints.
+            A value higher than one means that the rover will start to slow down before it reaches the
+            acceptance radius of the waypoint. This means the rover could already reach the reduced velocity
+            before it enters the corner, which can help reduce the risk of the rover rolling over.
+        type: float
+        min: 1.0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1.3
+
       RA_SPEED_P:
         description:
           short: Proportional gain for ground speed controller