New features for the ackermann rover module

ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames

@@ -6,6 +6,7 @@
 # @class Copter
 #
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v5x exclude
 # @board bitcraze_crazyflie exclude
 #
 # @maintainer Iain Galloway <[email protected]>

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 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,13 +53,24 @@ 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()
 {
 	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
-	switch (_nav_state) {
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
-			manual_control_setpoint_s manual_control_setpoint{};
+	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();
+	}
 
-			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-				_motor_setpoint.steering = manual_control_setpoint.roll;
-				_motor_setpoint.throttle =  manual_control_setpoint.throttle;
-			}
+	// Timestamps
+	hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	if (_armed) {
+		// Navigation modes
+		switch (_nav_state) {
+		case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
+				manual_control_setpoint_s manual_control_setpoint{};
+
+				if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+					_motor_setpoint.steering = manual_control_setpoint.roll;
+					_motor_setpoint.throttle =  manual_control_setpoint.throttle;
+				}
+
+			} 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;
+		}
 
-		} break;
+		// Sanitize actuator commands
+		if (!PX4_ISFINITE(_motor_setpoint.steering)) {
+			_motor_setpoint.steering = 0.f;
+		}
 
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
-		_motor_setpoint = _ackermann_guidance.purePursuit();
-		break;
+		if (!PX4_ISFINITE(_motor_setpoint.throttle)) {
+			_motor_setpoint.throttle = 0.f;
+		}
 
-	default: // Unimplemented nav states will stop the rover
-		_motor_setpoint.steering = 0.f;
-		_motor_setpoint.throttle =  0.f;
-		break;
-	}
+		// 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);
+		}
 
-	hrt_abstime now = hrt_absolute_time();
+		// 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);
 
-	// 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);
+		} else {
+			_steering_with_rate_limit.setForcedValue(_motor_setpoint.steering);
+		}
 
-	// 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);
+		// 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;
+		_throttle_with_accel_limit.setForcedValue(0.f);
+		_steering_with_rate_limit.setForcedValue(0.f);
+	}
 }
 
 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,20 +93,31 @@ 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};
+	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
 	)
 };