ackermann: new features

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

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 lookahead_distance # [m] Lookahead distance of pure the 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 crosstrack_error # [m] Shortest distance from the vehicle to the path
+float32 desired_speed 		# [m/s] Rover desired ground speed
+float32 lookahead_distance 	# [m] Lookahead distance of pure the 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 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,15 @@ bool RoverAckermann::init()
 void RoverAckermann::updateParams()
 {
 	ModuleParams::updateParams();
+
+	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()
@@ -72,9 +82,17 @@ void RoverAckermann::Run()
 		_nav_state = vehicle_status.nav_state;
 	}
 
+	if (_local_position_sub.updated()) {
+		vehicle_local_position_s local_position{};
+		_local_position_sub.copy(&local_position);
+		const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
+		_actual_speed = rover_velocity.norm();
+	}
+
 	// Navigation modes
 	switch (_nav_state) {
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
+	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
 			manual_control_setpoint_s manual_control_setpoint{};
 
 			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
@@ -85,7 +103,8 @@ void RoverAckermann::Run()
 		} break;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-		_motor_setpoint = _ackermann_guidance.purePursuit();
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+		_motor_setpoint = _ackermann_guidance.purePursuit(_nav_state);
 		break;
 
 	default: // Unimplemented nav states will stop the rover
@@ -94,19 +113,49 @@ void RoverAckermann::Run()
 		break;
 	}
 
-	hrt_abstime now = hrt_absolute_time();
+	// Timestamps
+	hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	const float dt = (_timestamp - timestamp_prev) * 1e-6f;
+
+	// Acceleration slew rate
+	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
+	    && _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO
+	    && 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
+	    && _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO) {
+		_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 wheel motors
+	// Publish to motor
 	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.control[0] = _throttle_with_accel_limit.getState();
+	actuator_motors.timestamp = _timestamp;
 	_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.control[0] = _steering_with_rate_limit.getState();
+	actuator_servos.timestamp = _timestamp;
 	_actuator_servos_pub.publish(actuator_servos);
 }
 

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,20 +93,30 @@ class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
 	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};
 
 	// 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};
 
 	// 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()) {
@@ -98,18 +100,33 @@ 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 == NAVIGATION_STATE_AUTO_RTL
+	    && get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _next_wp(0),
+					     _next_wp(1)) < _param_ra_acc_rad_def.get()) { // 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_acceptance_radius * _param_ra_miss_vel_red.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());
 
-		if (distance_to_prev_wp <= _prev_acc_rad) { // Cornering speed
-			const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acc_rad;
+		} else if (distance_to_curr_wp <= _acceptance_radius * _param_ra_miss_vel_red.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
@@ -145,7 +162,6 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit()
 
 	// Publish ackermann controller status (logging)
 	_rover_ackermann_guidance_status.timestamp = now;
-	_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);
@@ -187,7 +203,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

@@ -43,7 +43,6 @@
 #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/position_controller_status.h>
 #include <uORB/topics/mission_result.h>
@@ -57,6 +56,7 @@
 #include <lib/pid/pid.h>
 
 using namespace matrix;
+static constexpr uint8_t NAVIGATION_STATE_AUTO_RTL = 5;
 
 /**
  * @brief Class for ackermann drive guidance.
@@ -78,8 +78,9 @@ class RoverAckermannGuidance : public ModuleParams
 
 	/**
 	 * @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
@@ -166,8 +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};
-	bool _mission_finished{false};
+	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_MISS_VEL_RED>) _param_ra_miss_vel_red,
 		(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

@@ -133,14 +133,31 @@ 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
         max: 100
         increment: 0.01
         decimal: 2
         default: 5
 
+      RA_MISS_VEL_RED:
+        description:
+          short: Tuning for the start of the velocity reduction during cornering
+          long: |
+            The rover will start slowing down at a distance of acceptance_radius * RA_MISS_VEL_RED
+            to the WP s.t. it could reach the reduced velocity before it enters the corner.
+            This can help to reduce the risk of the rover rolling over during corners.
+            Higher value -> Rover starts the velocity reduction further from the waypoint.
+        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
@@ -175,3 +192,25 @@ parameters:
         increment: 0.01
         decimal: 2
         default: -1
+
+      RA_MAX_ACCEL:
+        description:
+          short: Maximum acceleration for the rover
+        type: float
+        unit: m/s^2
+        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