ackermann: restructure module

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

@@ -16,15 +16,17 @@ param set-default NAV_ACC_RAD 0.5
 param set-default RA_ACC_RAD_GAIN 2
 param set-default RA_ACC_RAD_MAX 3
 param set-default RA_MAX_ACCEL 1.5
+param set-default RA_MAX_DECEL 10
 param set-default RA_MAX_JERK 15
-param set-default RA_MAX_SPEED 3
+param set-default RA_MAX_SPEED 5
+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_VEL_DEF 3
+param set-default RA_MISS_SPD_DEF 5
-param set-default RA_MISS_VEL_GAIN 5
+param set-default RA_MISS_SPD_GAIN 5
-param set-default RA_MISS_VEL_MIN 1
+param set-default RA_MISS_SPD_MIN 1
 param set-default RA_SPEED_I 0.01
-param set-default RA_SPEED_P 2
+param set-default RA_SPEED_P 0.1
 param set-default RA_WHEEL_BASE 0.321
 
 # Pure Pursuit parameters

msg/CMakeLists.txt

@@ -185,6 +185,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	RoverAckermannGuidanceStatus.msg
+	RoverAckermannSetpoint.msg
 	RoverAckermannStatus.msg
 	RoverDifferentialGuidanceStatus.msg
 	RoverDifferentialSetpoint.msg

msg/RoverAckermannGuidanceStatus.msg

@@ -1,8 +1,6 @@
 uint64 timestamp # time since system start (microseconds)
 
-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
 
 # TOPICS rover_ackermann_guidance_status

msg/RoverAckermannSetpoint.msg

@@ -0,0 +1,8 @@
+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
+
+# TOPICS rover_ackermann_setpoint

msg/RoverAckermannStatus.msg

@@ -1,7 +1,9 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 throttle_setpoint 	# [-1, 1] Normalized throttle setpoint
+float32 measured_forward_speed       	      # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ
-float32 steering_setpoint 	# [-1, 1] Normalized steering setpoint
+float32 adjusted_forward_speed_setpoint       # [m/s] Speed setpoint after applying slew rate
-float32 actual_speed       	# [m/s] Rover ground speed
+float32 steering_setpoint_normalized          # [-1, 1] Normalized steering setpoint
+float32 adjusted_steering_setpoint_normalized # [-1, 1] Normalized steering setpoint after applying slew rate
+float32 pid_throttle_integral                 # Integral of the PID for the closed loop speed controller
 
 # TOPICS rover_ackermann_status

src/modules/logger/logged_topics.cpp

@@ -104,6 +104,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
 	add_optional_topic("rover_ackermann_guidance_status", 100);
+	add_optional_topic("rover_ackermann_setpoint", 100);
 	add_optional_topic("rover_ackermann_status", 100);
 	add_optional_topic("rover_differential_guidance_status", 100);
 	add_optional_topic("rover_differential_setpoint", 100);

src/modules/rover_ackermann/CMakeLists.txt

@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(RoverAckermannGuidance)
+add_subdirectory(RoverAckermannControl)
 
 px4_add_module(
 	MODULE modules__rover_ackermann
@@ -41,6 +42,7 @@ px4_add_module(
 		RoverAckermann.hpp
 	DEPENDS
 		RoverAckermannGuidance
+		RoverAckermannControl
 		px4_work_queue
 		SlewRate
 		pure_pursuit

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -33,14 +33,11 @@
 
 #include "RoverAckermann.hpp"
 
-using namespace time_literals;
-using namespace matrix;
-
 RoverAckermann::RoverAckermann() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	_rover_ackermann_status_pub.advertise();
+	_rover_ackermann_setpoint_pub.advertise();
 	updateParams();
 }
 
@@ -53,16 +50,6 @@ 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()
@@ -75,45 +62,47 @@ void RoverAckermann::Run()
 
 	updateSubscriptions();
 
-	// Timestamps
+	// Generate and publish speed and steering setpoints
-	hrt_abstime timestamp_prev = _timestamp;
+	hrt_abstime timestamp = hrt_absolute_time();
-	_timestamp = hrt_absolute_time();
-	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
-	// Generate motor setpoints
-	if (_armed) {
 	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;
+				rover_ackermann_setpoint_s rover_ackermann_setpoint{};
-					_motor_setpoint.throttle =  manual_control_setpoint.throttle;
+				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 = manual_control_setpoint.roll;
+				_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
 			}
 
 		} break;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
-			_motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state);
+		_ackermann_guidance.computeGuidance(_vehicle_forward_speed, _vehicle_yaw, _nav_state, _armed);
 		break;
 
 	default: // Unimplemented nav states will stop the rover
-			_motor_setpoint.steering = 0.f;
+		rover_ackermann_setpoint_s rover_ackermann_setpoint{};
-			_motor_setpoint.throttle =  0.f;
+		rover_ackermann_setpoint.timestamp =  timestamp;
-			_throttle_with_accel_limit.setForcedValue(0.f);
+		rover_ackermann_setpoint.forward_speed_setpoint =  NAN;
-			_steering_with_rate_limit.setForcedValue(0.f);
+		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_pub.publish(rover_ackermann_setpoint);
 		break;
 	}
 
-	} else { // Reset on disarm
+	if (!_armed) {
-		_motor_setpoint.steering = 0.f;
+		_ackermann_control.resetControllers();
-		_motor_setpoint.throttle =  0.f;
-		_throttle_with_accel_limit.setForcedValue(0.f);
-		_steering_with_rate_limit.setForcedValue(0.f);
 	}
 
-	publishMotorSetpoints(applySlewRates(_motor_setpoint, dt));
+	_ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw);
+
 }
 
 void RoverAckermann::updateSubscriptions()
@@ -129,69 +118,20 @@ void RoverAckermann::updateSubscriptions()
 		_armed = vehicle_status.arming_state == 2;
 	}
 
-	if (_local_position_sub.updated()) {
+	if (_vehicle_attitude_sub.updated()) {
-		vehicle_local_position_s local_position{};
+		vehicle_attitude_s vehicle_attitude{};
-		_local_position_sub.copy(&local_position);
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
-		const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_actual_speed = rover_velocity.norm();
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
-	}
-}
-motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt)
-{
-	// Sanitize actuator commands
-	if (!PX4_ISFINITE(motor_setpoint.steering)) {
-		motor_setpoint.steering = 0.f;
 	}
 
-	if (!PX4_ISFINITE(motor_setpoint.throttle)) {
+	if (_vehicle_local_position_sub.updated()) {
-		motor_setpoint.throttle = 0.f;
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		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);
 	}
-
-	// 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);
-	}
-
-	motor_setpoint_struct motor_setpoint_temp{};
-	motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
-	motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
-	return motor_setpoint_temp;
-}
-
-void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates)
-{
-	// 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] = motor_setpoint_with_slew_rates.throttle;
-	actuator_motors.timestamp = _timestamp;
-	_actuator_motors_pub.publish(actuator_motors);
-
-	// Publish to servo
-	actuator_servos_s actuator_servos{};
-	actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering;
-	actuator_servos.timestamp = _timestamp;
-	_actuator_servos_pub.publish(actuator_servos);
 }
 
 int RoverAckermann::task_spawn(int argc, char *argv[])

src/modules/rover_ackermann/RoverAckermann.hpp

@@ -39,27 +39,25 @@
 #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>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/rover_ackermann_setpoint.h>
-#include <uORB/topics/actuator_servos.h>
-#include <uORB/topics/rover_ackermann_status.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 
 // Standard library includes
 #include <math.h>
+#include <matrix/matrix/math.hpp>
 
 // Local includes
 #include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
-using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint;
+#include "RoverAckermannControl/RoverAckermannControl.hpp"
 
 using namespace time_literals;
 
@@ -96,49 +94,25 @@ private:
 	 */
 	void updateSubscriptions();
 
-	/**
-	 * @brief Apply slew rates to motor setpoints.
-	 * @param motor_setpoint Normalized steering and throttle setpoints.
-	 * @param dt Time since last update [s].
-	 * @return Motor setpoint with applied slew rates.
-	 */
-	motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt);
-
-	/**
-	 * @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus.
-	 * @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates.
-	 */
-	void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates);
-
 	// uORB subscriptions
 	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::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 
 	// uORB publications
-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<rover_ackermann_setpoint_s> _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)};
-	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)};
 
 	// Class instances
 	RoverAckermannGuidance _ackermann_guidance{this};
+	RoverAckermannControl _ackermann_control{this};
 
 	// Variables
+	matrix::Quatf _vehicle_attitude_quaternion{};
 	int _nav_state{0};
-	motor_setpoint_struct _motor_setpoint;
+	float _vehicle_forward_speed{0.f};
-	hrt_abstime _timestamp{0};
+	float _vehicle_yaw{0.f};
-	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,
-		(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/RoverAckermannControl/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(RoverAckermannControl
+	RoverAckermannControl.cpp
+)
+
+target_link_libraries(RoverAckermannControl PUBLIC pid)
+target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp

@@ -0,0 +1,191 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "RoverAckermannControl.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_ackermann_status_pub.advertise();
+	pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
+
+}
+
+void RoverAckermannControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	pid_set_parameters(&_pid_throttle,
+			   _param_ra_p_speed.get(), // Proportional gain
+			   _param_ra_i_speed.get(), // Integral gain
+			   0, // Derivative gain
+			   1, // Integral limit
+			   1); // Output limit
+
+	// Update slew rates
+	if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) {
+		_forward_speed_setpoint_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 RoverAckermannControl::computeMotorCommands(const float vehicle_forward_speed, const float vehicle_yaw)
+{
+	// Timestamps
+	hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	// Update ackermann setpoint
+	_rover_ackermann_setpoint_sub.update(&_rover_ackermann_setpoint);
+
+	// Speed control
+	float forward_speed_normalized{0.f};
+
+	if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) {
+		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint,
+					   vehicle_forward_speed, dt, false);
+
+	} else if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint
+		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint_normalized,
+					   vehicle_forward_speed, dt, true);
+
+	}
+
+	// Steering control
+	float steering_normalized{0.f};
+
+	if (PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint)) {
+		steering_normalized = math::interpolate<float>(_rover_ackermann_setpoint.steering_setpoint,
+				      -_param_ra_max_steer_angle.get(),
+				      _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint
+
+	} else { // Use normalized setpoint
+		steering_normalized = PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint_normalized) ? math::constrain(
+					      _rover_ackermann_setpoint.steering_setpoint_normalized, -1.f, 1.f) : 0.f;
+	}
+
+	if (_param_ra_max_steering_rate.get() > FLT_EPSILON
+	    && _param_ra_max_steer_angle.get() > FLT_EPSILON) { // Apply slew rate
+		_steering_with_rate_limit.update(steering_normalized, dt);
+
+	} else {
+		_steering_with_rate_limit.setForcedValue(steering_normalized);
+	}
+
+	// Publish rover Ackermann status (logging)
+	_rover_ackermann_status.timestamp = _timestamp;
+	_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.pid_throttle_integral = _pid_throttle.integral;
+	_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] = forward_speed_normalized;
+	actuator_motors.timestamp = _timestamp;
+	_actuator_motors_pub.publish(actuator_motors);
+
+	// Publish to servo
+	actuator_servos_s actuator_servos{};
+	actuator_servos.control[0] = _steering_with_rate_limit.getState();
+	actuator_servos.timestamp = _timestamp;
+	_actuator_servos_pub.publish(actuator_servos);
+
+}
+
+float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint,
+		const float vehicle_forward_speed, const float dt, const bool normalized)
+{
+	float slew_rate_normalization{1.f};
+
+	if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized
+		slew_rate_normalization = _param_ra_max_thr_speed.get() > FLT_EPSILON ? _param_ra_max_thr_speed.get() : 0.f;
+	}
+
+	// Apply acceleration and deceleration limit
+	if (fabsf(forward_speed_setpoint) >= fabsf(_forward_speed_setpoint_with_accel_limit.getState())) {
+		if (_param_ra_max_accel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+			_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / slew_rate_normalization);
+			_forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
+
+		} else {
+			_forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
+
+		}
+
+	} else if (_param_ra_max_decel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+		_forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_decel.get() / slew_rate_normalization);
+		_forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
+
+	} else {
+		_forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
+	}
+
+	// Calculate normalized forward speed setpoint
+	float forward_speed_normalized{0.f};
+
+	if (normalized) {
+		forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState();
+
+	} else { // Closed loop speed control
+		_rover_ackermann_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState();
+
+		if (_param_ra_max_thr_speed.get() > FLT_EPSILON) { // Feedforward
+			forward_speed_normalized = math::interpolate<float>(_forward_speed_setpoint_with_accel_limit.getState(),
+						   -_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get(),
+						   -1.f, 1.f);
+		}
+
+		forward_speed_normalized += pid_calculate(&_pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
+					    vehicle_forward_speed, 0, dt); // Feedback
+	}
+
+	return math::constrain(forward_speed_normalized, -1.f, 1.f);
+
+}
+
+void RoverAckermannControl::resetControllers()
+{
+	pid_reset_integral(&_pid_throttle);
+}

src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/rover_ackermann_setpoint.h>
+#include <uORB/topics/rover_ackermann_status.h>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/actuator_servos.h>
+
+// Standard libraries
+#include <lib/pid/pid.h>
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <math.h>
+#include <lib/slew_rate/SlewRate.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+/**
+ * @brief Class for ackermann rover guidance.
+ */
+class RoverAckermannControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for RoverAckermannControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	RoverAckermannControl(ModuleParams *parent);
+	~RoverAckermannControl() = default;
+
+	/**
+	 * @brief Compute motor commands based on setpoints
+	 */
+	void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw);
+
+	/**
+	 * @brief Reset PID controllers and slew rates
+	 */
+	void resetControllers();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Compute normalized forward speed setpoint by applying slew rates
+	 * to the forward speed setpoint and doing closed loop speed control if enabled.
+	 * @param forward_speed_setpoint Forward speed setpoint [m/s].
+	 * @param vehicle_forward_speed Actual forward speed [m/s].
+	 * @param dt Time since last update [s].
+	 * @param normalized Indicates if the forward speed setpoint is already normalized.
+	 * @return Normalized forward speed setpoint with applied slew rates [-1, 1].
+	 */
+	float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float dt, bool normalized);
+
+	// uORB subscriptions
+	uORB::Subscription _rover_ackermann_setpoint_sub{ORB_ID(rover_ackermann_setpoint)};
+	rover_ackermann_setpoint_s _rover_ackermann_setpoint{};
+
+	// 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)};
+	rover_ackermann_status_s _rover_ackermann_status{};
+
+	// Variables
+	hrt_abstime _timestamp{0};
+
+	// Controllers
+	PID_t _pid_throttle; // 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_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,
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
+};

src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt

@@ -35,5 +35,4 @@ px4_add_library(RoverAckermannGuidance
 	RoverAckermannGuidance.cpp
 )
 
-target_link_libraries(RoverAckermannGuidance PUBLIC pid)
 target_include_directories(RoverAckermannGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp

@@ -42,35 +42,30 @@ RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModulePar
 {
 	_rover_ackermann_guidance_status_pub.advertise();
 	updateParams();
-	pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f);
 }
 
 void RoverAckermannGuidance::updateParams()
 {
 	ModuleParams::updateParams();
-	pid_set_parameters(&_pid_throttle,
+
-			   _param_ra_p_speed.get(), // Proportional gain
-			   _param_ra_i_speed.get(), // Integral gain
-			   0, // Derivative gain
-			   1, // Integral limit
-			   1); // Output limit
 }
 
-RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state)
+void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed,
+		const float vehicle_yaw, const int nav_state, const bool armed)
 {
 	updateSubscriptions();
 
 	// Distances to waypoints
-	_distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
+	const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
 					  _prev_wp(0), _prev_wp(1));
-	_distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
+	const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
 					  _curr_wp(0), _curr_wp(1));
-	_distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
+	// const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
-			       _next_wp(0), _next_wp(1));
+	// 				  _next_wp(0), _next_wp(1));
 
 	// Catch return to launch
 	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
-		_mission_finished = _distance_to_next_wp < _acceptance_radius;
+		_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
 	}
 
 	// Guidance logic
@@ -78,39 +73,35 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
 		_desired_steering = 0.f;
 		_desired_speed = 0.f;
 
-	} else if (_distance_to_curr_wp < _acceptance_radius) { // Catch delay command
+	} else if (_nav_cmd == 93) { // Catch delay command
 		_desired_speed = 0.f;
 
 	} else { // Regular guidance algorithm
 
-		_desired_speed = calcDesiredSpeed(_wp_max_desired_vel, _param_ra_miss_vel_min.get(),
+		_desired_speed = calcDesiredSpeed(_cruising_speed, _param_ra_miss_spd_min.get(),
-						  _param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius,
+						  _param_ra_miss_spd_gain.get(), distance_to_prev_wp, distance_to_curr_wp, _acceptance_radius,
-						  _prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state);
+						  _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());
 
 		_desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
-							_param_ra_wheel_base.get(), _desired_speed, _vehicle_yaw, _param_ra_max_steer_angle.get());
+							_param_ra_wheel_base.get(), _desired_speed, vehicle_yaw, _param_ra_max_steer_angle.get(), armed);
 
 	}
 
-	// Calculate throttle setpoint
-	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 throttle = calcThrottleSetpoint(_pid_throttle, _desired_speed, _actual_speed, _param_ra_max_speed.get(),
-			       dt);
-
 	// Publish ackermann controller status (logging)
-	_rover_ackermann_guidance_status.timestamp = _timestamp;
+	hrt_abstime timestamp = hrt_absolute_time();
-	_rover_ackermann_guidance_status.desired_speed = _desired_speed;
+	_rover_ackermann_guidance_status.timestamp = timestamp;
-	_rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral;
 	_rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status);
 
-	// Return motor setpoints
+	// Publish speed and steering setpoints
-	motor_setpoint motor_setpoint_temp;
+	rover_ackermann_setpoint_s rover_ackermann_setpoint{};
-	motor_setpoint_temp.steering = math::interpolate<float>(_desired_steering, -_param_ra_max_steer_angle.get(),
+	rover_ackermann_setpoint.timestamp = timestamp;
-				       _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint
+	rover_ackermann_setpoint.forward_speed_setpoint = _desired_speed;
-	motor_setpoint_temp.throttle = throttle;
+	rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN;
-	return motor_setpoint_temp;
+	rover_ackermann_setpoint.steering_setpoint = _desired_steering;
+	rover_ackermann_setpoint.steering_setpoint_normalized = NAN;
+	_rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint);
+
 }
 
 void RoverAckermannGuidance::updateSubscriptions()
@@ -131,8 +122,6 @@ void RoverAckermannGuidance::updateSubscriptions()
 		}
 
 		_curr_pos_ned = Vector2f(local_position.x, local_position.y);
-		const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
-		_actual_speed = rover_velocity.norm();
 	}
 
 	if (_home_position_sub.updated()) {
@@ -145,18 +134,17 @@ void RoverAckermannGuidance::updateSubscriptions()
 		updateWaypointsAndAcceptanceRadius();
 	}
 
-	if (_vehicle_attitude_sub.updated()) {
-		vehicle_attitude_s vehicle_attitude{};
-		_vehicle_attitude_sub.copy(&vehicle_attitude);
-		matrix::Quatf vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
-	}
-
 	if (_mission_result_sub.updated()) {
 		mission_result_s mission_result{};
 		_mission_result_sub.copy(&mission_result);
 		_mission_finished = mission_result.finished;
 	}
+
+	if (_navigator_mission_item_sub.updated()) {
+		navigator_mission_item_s navigator_mission_item{};
+		_navigator_mission_item_sub.copy(&navigator_mission_item);
+		_nav_cmd = navigator_mission_item.nav_cmd;
+	}
 }
 
 void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
@@ -166,7 +154,7 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
 
 	// Global waypoint coordinates
 	_prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid
-	_curr_wp = Vector2d(0, 0);
+	_curr_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if current waypoint is invalid
 	_next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL
 
 	if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
@@ -192,46 +180,47 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
 	_prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
 	_next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));
 
+	// Distances
+	const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned;
+	const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned;
+	float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
+	cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
+	_prev_waypoint_transition_angle = _waypoint_transition_angle;
+	_waypoint_transition_angle = acosf(cosin);
+
 	// Update acceptance radius
 	_prev_acceptance_radius = _acceptance_radius;
 
 	if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
-		_acceptance_radius = updateAcceptanceRadius(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, _param_nav_acc_rad.get(),
+		_acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _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();
 	}
 
+	// Waypoint cruising speed
 	if (position_setpoint_triplet.current.cruising_speed > 0.f) {
-		_wp_max_desired_vel = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get());
+		_cruising_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get());
 
 	} else {
-		_wp_max_desired_vel = _param_ra_miss_vel_def.get();
+		_cruising_speed = _param_ra_miss_spd_def.get();
 	}
 }
 
-float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
+float RoverAckermannGuidance::updateAcceptanceRadius(const float waypoint_transition_angle,
-		const Vector2f &next_wp_ned, const float default_acceptance_radius, const float acceptance_radius_gain,
+		const float default_acceptance_radius, const float acceptance_radius_gain,
 		const float acceptance_radius_max, const float wheel_base, const float max_steer_angle)
 {
-	// Setup variables
-	const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned;
-	const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned;
-	float acceptance_radius = default_acceptance_radius;
-
 	// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
-	if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) {
+	float acceptance_radius = default_acceptance_radius;
-		float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
+	const float theta = waypoint_transition_angle / 2.f;
-		cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
-		const float theta = acosf(cosin) / 2.f;
 	const float min_turning_radius = wheel_base / sinf(max_steer_angle);
 	const float acceptance_radius_temp = min_turning_radius / tanf(theta);
 	const float acceptance_radius_temp_scaled = acceptance_radius_gain *
 			acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
 	acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
 			    acceptance_radius_max);
-	}
 
 	// Publish updated acceptance radius
 	position_controller_status_s pos_ctrl_status{};
@@ -241,51 +230,56 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned
 	return acceptance_radius;
 }
 
-float RoverAckermannGuidance::calcDesiredSpeed(const float miss_vel_def, const float miss_vel_min,
+float RoverAckermannGuidance::calcDesiredSpeed(const float cruising_speed, const float miss_speed_min,
-		const float miss_vel_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad,
+		const float miss_speed_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad,
-		const float prev_acc_rad, const float max_accel, const float max_jerk, const int nav_state)
+		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_vel_min < 0.f  || miss_vel_min > miss_vel_def || miss_vel_gain < FLT_EPSILON) {
+	if (miss_speed_min < 0.f  || miss_speed_min > cruising_speed || miss_speed_gain < FLT_EPSILON) {
-		return miss_vel_def;
+		return cruising_speed;
 	}
 
 	// Cornering slow down effect
 	if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) {
-		const float cornering_speed = miss_vel_gain / prev_acc_rad;
+		const float cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - prev_waypoint_transition_angle,
-		return math::constrain(cornering_speed, miss_vel_min, miss_vel_def);
+					      0.f, M_PI_F, 0.f, 1.f);
+		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 = miss_vel_gain / acc_rad;
+		const float cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
-		return math::constrain(cornering_speed, miss_vel_min, miss_vel_def);
+					      M_PI_F, 0.f, 1.f);
+		return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
 
 	}
 
 	// Straight line speed
-	if (max_accel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
+	if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
-		float max_velocity{0.f};
+		float straight_line_speed{0.f};
 
 		if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
-			max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
+			straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-					max_accel, distance_to_curr_wp, 0.f);
+					      max_decel, distance_to_curr_wp, 0.f);
 
 		} else {
-			const float cornering_speed = miss_vel_gain / acc_rad;
+			float cornering_speed = max_speed - miss_speed_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, M_PI_F,
-			max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
+						0.f, 1.f);
-					max_accel, distance_to_curr_wp - acc_rad, cornering_speed);
+			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);
 		}
 
-		return math::constrain(max_velocity, miss_vel_min, miss_vel_def);
+		return math::min(straight_line_speed, cruising_speed);
 
 	} else {
-		return miss_vel_def;
+		return cruising_speed;
 	}
 
 }
 
 float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned,
 		const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed,
-		const float vehicle_yaw, const float max_steering)
+		const float vehicle_yaw, const float max_steering, const bool armed)
 {
 	const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned,
 				      desired_speed);
@@ -297,6 +291,10 @@ float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, con
 
 	float desired_steering{0.f};
 
+	if (!armed) {
+		return desired_steering;
+	}
+
 	if (math::abs_t(heading_error) <= M_PI_2_F) {
 		desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance);
 
@@ -309,22 +307,3 @@ float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, con
 	return math::constrain(desired_steering, -max_steering, max_steering);
 
 }
-
-float RoverAckermannGuidance::calcThrottleSetpoint(PID_t &pid_throttle, const float desired_speed,
-		const float actual_speed, const float max_speed, const float dt)
-{
-	float throttle = 0.f;
-
-	if (desired_speed < FLT_EPSILON) {
-		pid_reset_integral(&pid_throttle);
-
-	} else {
-		throttle = pid_calculate(&pid_throttle, desired_speed, actual_speed, 0, dt);
-	}
-
-	if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term
-		throttle += math::interpolate<float>(desired_speed, 0.f, max_speed, 0.f, 1.f);
-	}
-
-	return math::constrain(throttle, 0.f, 1.f);
-}

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -41,14 +41,15 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/rover_ackermann_guidance_status.h>
+#include <uORB/topics/rover_ackermann_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_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>
+#include <uORB/topics/navigator_mission_item.h>
 
 // Standard library includes
 #include <matrix/matrix/math.hpp>
@@ -74,21 +75,13 @@ public:
 	~RoverAckermannGuidance() = default;
 
 	/**
-	 * @brief Struct for steering and throttle setpoints.
+	 * @brief Compute and publish speed and steering setpoints based on the mission plan.
-	 * @param steering Steering setpoint.
+	 * @param vehicle_forward_speed Forward speed of the vehicle [m/s]
-	 * @param throttle Throttle setpoint.
+	 * @param vehicle_yaw Yaw of the vehicle [rad]
-	 */
-	struct motor_setpoint {
-		float steering{0.f};
-		float throttle{0.f};
-	};
-
-	/**
-	 * @brief Calculate motor setpoints based on the mission plan.
 	 * @param nav_state Vehicle navigation state.
-	 * @return Motor setpoints for throttle and steering.
+	 * @param armed Vehicle arm state.
 	 */
-	motor_setpoint computeGuidance(int nav_state);
+	void computeGuidance(float vehicle_forward_speed, float vehicle_yaw, int nav_state, bool armed);
 
 protected:
 	/**
@@ -110,9 +103,7 @@ private:
 	/**
 	 * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment
 	 * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle.
-	 * @param curr_wp_ned Current waypoint in NED frame [m].
+	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
-	 * @param prev_wp_ned Previous waypoint in NED frame [m].
-	 * @param next_wp_ned Next waypoint in NED frame [m].
 	 * @param default_acceptance_radius Default acceptance radius for waypoints [m].
 	 * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-].
 	 * @param acceptance_radius_max Maximum value for the acceptance radius [m].
@@ -120,30 +111,34 @@ private:
 	 * @param max_steer_angle Rover maximum steer angle [rad].
 	 * @return Updated acceptance radius [m].
 	 */
-	float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
+	float updateAcceptanceRadius(const float waypoint_transition_angle, float default_acceptance_radius,
-				     const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain,
+				     float acceptance_radius_gain,
 				     float acceptance_radius_max, float wheel_base, float max_steer_angle);
 
 
 	/**
 	 * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse
-	 * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a velocity trajectory
+	 * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a speed trajectory
 	 * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the
 	 * maximum acceleration and jerk.
-	 * @param miss_vel_def Default desired velocity for the rover during mission [m/s].
+	 * @param cruising_speed Cruising speed [m/s].
-	 * @param miss_vel_min Minimum desired velocity for the rover during mission [m/s].
+	 * @param miss_speed_min Minimum speed setpoint [m/s].
-	 * @param miss_vel_gain Tuning parameter for the slow down effect during cornering [-].
+	 * @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].
 	 * @param prev_acc_rad Acceptance radius of the previous waypoint [m].
-	 * @param max_accel Maximum allowed acceleration for the rover [m/s^2].
+	 * @param max_accel Maximum allowed acceleration [m/s^2].
-	 * @param max_jerk Maximum allowed jerk for the rover [m/s^3].
+	 * @param max_jerk Maximum allowed jerk [m/s^3].
 	 * @param nav_state Current nav_state of the rover.
-	 * @return Speed setpoint for the rover [m/s].
+	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	 * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	 * @param max_speed Maximum speed setpoint [m/s]
+	 * @return Speed setpoint [m/s].
 	 */
-	float calcDesiredSpeed(float miss_vel_def, float miss_vel_min, float miss_vel_gain, float distance_to_prev_wp,
+	float calcDesiredSpeed(float cruising_speed, float miss_speed_min, float miss_speed_gain, 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 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
@@ -156,32 +151,23 @@ private:
 	 * @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);
+				  const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering, bool armed);
-
-	/**
-	 * @brief Calculate the throttle setpoint. Calculated with a PID controller using the difference between
-	 * the desired/actual speed and a feedforward term based on the full throttle speed.
-	 * @param pid_throttle Reference to PID instance.
-	 * @param desired_speed Reference speed for the rover [m/s].
-	 * @param actual_speed Actual speed of the rover [m/s].
-	 * @param max_speed Rover speed at full throttle [m/s].
-	 * @param dt Time interval since last update [s].
-	 * @return Normalized throttle setpoint [0, 1].
-	 */
-	float calcThrottleSetpoint(PID_t &pid_throttle, float desired_speed, float actual_speed, float max_speed, float dt);
 
 	// uORB subscriptions
 	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 	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::Subscription _navigator_mission_item_sub{ORB_ID(navigator_mission_item)};
 
 	// uORB publications
+	uORB::Publication<rover_ackermann_setpoint_s> _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)};
 	uORB::Publication<rover_ackermann_guidance_status_s> _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)};
 	uORB::Publication<position_controller_status_s>	_position_controller_status_pub{ORB_ID(position_controller_status)};
 	rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{};
@@ -192,13 +178,10 @@ private:
 
 	// Rover variables
 	float _desired_steering{0.f};
-	float _vehicle_yaw{0.f};
 	float _desired_speed{0.f};
-	float _actual_speed{0.f};
 	Vector2d _curr_pos{};
 	Vector2f _curr_pos_ned{};
-	PID_t _pid_throttle;
+	bool _mission_finished{false};
-	hrt_abstime _timestamp{0};
 
 	// Waypoint variables
 	Vector2d _home_position{};
@@ -208,13 +191,12 @@ private:
 	Vector2d _curr_wp{};
 	Vector2d _prev_wp{};
 	Vector2d _next_wp{};
-	float _distance_to_prev_wp{0.f};
-	float _distance_to_curr_wp{0.f};
-	float _distance_to_next_wp{0.f};
 	float _acceptance_radius{0.5f};
 	float _prev_acceptance_radius{0.5f};
-	float _wp_max_desired_vel{0.f};
+	float _cruising_speed{0.f};
-	bool _mission_finished{false};
+	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};
 
 	// Parameters
 	DEFINE_PARAMETERS(
@@ -222,14 +204,14 @@ private:
 		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_steer_angle,
 		(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_VEL_DEF>) _param_ra_miss_vel_def,
+		(ParamFloat<px4::params::RA_MISS_SPD_DEF>) _param_ra_miss_spd_def,
-		(ParamFloat<px4::params::RA_MISS_VEL_MIN>) _param_ra_miss_vel_min,
+		(ParamFloat<px4::params::RA_MISS_SPD_MIN>) _param_ra_miss_spd_min,
-		(ParamFloat<px4::params::RA_MISS_VEL_GAIN>) _param_ra_miss_vel_gain,
+		(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_ACCEL>) _param_ra_max_accel,
+		(ParamFloat<px4::params::RA_MAX_DECEL>) _param_ra_max_decel,
 		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
 	)
 };

src/modules/rover_ackermann/module.yaml

@@ -57,9 +57,9 @@ parameters:
         decimal: 2
         default: 2
 
-      RA_MISS_VEL_DEF:
+      RA_MISS_SPD_DEF:
         description:
-          short: Default rover velocity during a mission
+          short: Default rover speed during a mission
         type: float
         unit: m/s
         min: 0
@@ -68,11 +68,11 @@ parameters:
         decimal: 2
         default: 2
 
-      RA_MISS_VEL_MIN:
+      RA_MISS_SPD_MIN:
         description:
-          short: Minimum rover velocity during a mission
+          short: Minimum rover speed during a mission
           long: |
-            The velocity off the rover is reduced based on the corner it has to take
+            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
@@ -82,13 +82,13 @@ parameters:
         decimal: 2
         default: 1
 
-      RA_MISS_VEL_GAIN:
+      RA_MISS_SPD_GAIN:
         description:
-          short: Tuning parameter for the velocity reduction during cornering
+          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 velocity reduction during cornering.
+            Lower value -> More speed reduction during cornering.
         type: float
         min: 0.05
         max: 100
@@ -112,17 +112,32 @@ parameters:
         type: float
         min: 0
         max: 100
-        increment: 0.01
+        increment: 0.001
-        decimal: 2
+        decimal: 3
         default: 1
 
       RA_MAX_SPEED:
+        description:
+          short: Maximum allowed speed
+          long: |
+            This is the maximum allowed speed setpoint when driving in a mode that uses
+            closed loop speed control.
+        type: float
+        unit: m/s
+        min: -1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: -1
+
+      RA_MAX_THR_SPEED:
         description:
           short: Speed the rover drives at maximum throttle
           long: |
-            This is used for the feed-forward term of the speed controller.
+            This parameter is used to calculate the feedforward term of the closed loop speed control which linearly
-            A value of -1 disables the feed-forward term in which case the
+            maps desired speeds to normalized motor commands [-1. 1].
-            Integrator (RA_SPEED_I) becomes necessary to track speed setpoints.
+            A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode.
+            Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower.
         type: float
         unit: m/s
         min: -1
@@ -147,6 +162,22 @@ parameters:
         decimal: 2
         default: -1
 
+      RA_MAX_DECEL:
+        description:
+          short: Maximum deceleration for the rover
+          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.
+        type: float
+        unit: m/s^2
+        min: -1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: -1
+
       RA_MAX_JERK:
         description:
           short: Maximum jerk