Rover Ackermann module (#23024)

New module handling Ackermann rover guidance and control.
Only enabled in SITL and in the rover builds for now.

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -84,6 +84,13 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()
 
+if(CONFIG_MODULES_ROVER_ACKERMANN)
+	px4_add_romfs_files(
+		rc.rover_ackermann_apps
+		rc.rover_ackermann_defaults
+	)
+endif()
+
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		rc.uuv_apps

ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic

@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Generic ackermann rover
+#
+# @type Rover
+# @class Rover
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.rover_ackermann_defaults

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -149,6 +149,12 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()
 
+if(CONFIG_MODULES_ROVER_ACKERMANN)
+	px4_add_romfs_files(
+		50010_ackermann_rover_generic
+	)
+endif()
+
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		# [60000, 61000] (Unmanned) Underwater Robots

ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps

@@ -0,0 +1,11 @@
+#!/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
+
+# Start Land Detector.
+land_detector start rover

ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults

@@ -0,0 +1,13 @@
+#!/bin/sh
+# Ackermann rover parameters.
+
+set VEHICLE_TYPE rover_ackermann
+param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
+param set-default CA_AIRFRAME 5 # Rover (Ackermann)
+param set-default CA_R_REV 1 # Motor is assumed to be reversible
+param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01
+param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius
+param set-default NAV_RCL_ACT 6 # Disarm on manual control loss
+param set-default COM_FAIL_ACT_T 1 # Delay before failsafe after rc loss

ROMFS/px4fmu_common/init.d/rc.vehicle_setup

@@ -41,6 +41,15 @@ then
 	. ${R}etc/init.d/rc.rover_differential_apps
 fi
 
+#
+# Ackermann Rover setup.
+#
+if [ $VEHICLE_TYPE = rover_ackermann ]
+then
+	# Start ackermann drive rover apps.
+	. ${R}etc/init.d/rc.rover_ackermann_apps
+fi
+
 #
 # VTOL setup.
 #

boards/px4/fmu-v5/rover.px4board

@@ -19,3 +19,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y

boards/px4/fmu-v5x/rover.px4board

@@ -14,3 +14,5 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y

boards/px4/fmu-v6c/rover.px4board

@@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y

boards/px4/fmu-v6u/rover.px4board

@@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y

boards/px4/fmu-v6x/rover.px4board

@@ -14,3 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y

boards/px4/fmu-v6xrt/rover.px4board

@@ -15,3 +15,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
+CONFIG_MODULES_ROVER_ACKERMANN=y

boards/px4/sitl/default.px4board

@@ -45,6 +45,7 @@ CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
 CONFIG_MODULES_PAYLOAD_DELIVERER=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_REPLAY=y
+CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_COMMON_SIMULATION=y

msg/CMakeLists.txt

@@ -178,6 +178,7 @@ set(msg_files
 	RcParameterMap.msg
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
+	RoverAckermannGuidanceStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg

msg/RoverAckermannGuidanceStatus.msg

@@ -0,0 +1,10 @@
+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
+
+# TOPICS rover_ackermann_guidance_status

src/modules/logger/logged_topics.cpp

@@ -102,6 +102,7 @@ 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_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);
 	add_optional_topic("sensor_airflow", 100);

src/modules/rover_ackermann/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(RoverAckermannGuidance)
+
+px4_add_module(
+	MODULE modules__rover_ackermann
+	MAIN rover_ackermann
+	SRCS
+		RoverAckermann.cpp
+		RoverAckermann.hpp
+	DEPENDS
+		RoverAckermannGuidance
+		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
+)

src/modules/rover_ackermann/Kconfig

@@ -0,0 +1,6 @@
+menuconfig MODULES_ROVER_ACKERMANN
+	bool "rover_ackermann"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Enable support for control of ackermann drive rovers

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   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 "RoverAckermann.hpp"
+
+using namespace time_literals;
+using namespace matrix;
+
+RoverAckermann::RoverAckermann() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+}
+
+bool RoverAckermann::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void RoverAckermann::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void RoverAckermann::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	// uORB subscriber updates
+	if (_parameter_update_sub.updated()) {
+		updateParams();
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+		_vehicle_status_sub.copy(&vehicle_status);
+		_nav_state = vehicle_status.nav_state;
+	}
+
+	// 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:
+		_motor_setpoint = _ackermann_guidance.purePursuit();
+		break;
+
+	default: // Unimplemented nav states will stop the rover
+		_motor_setpoint.steering = 0.f;
+		_motor_setpoint.throttle =  0.f;
+		break;
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+
+	// 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);
+
+	// 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);
+}
+
+int RoverAckermann::task_spawn(int argc, char *argv[])
+{
+	RoverAckermann *instance = new RoverAckermann();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int RoverAckermann::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int RoverAckermann::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Rover state machine.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("rover_ackermann", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int rover_ackermann_main(int argc, char *argv[])
+{
+	return RoverAckermann::main(argc, argv);
+}

src/modules/rover_ackermann/RoverAckermann.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.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/actuator_servos.h>
+
+// Standard library includes
+#include <math.h>
+
+// Local includes
+#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
+
+using namespace time_literals;
+
+class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	/**
+	 * @brief Constructor for RoverAckermann
+	 */
+	RoverAckermann();
+	~RoverAckermann() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+protected:
+	void updateParams() override;
+
+private:
+	void Run() override;
+
+	// 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 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)};
+
+	// Instances
+	RoverAckermannGuidance _ackermann_guidance{this};
+
+	// Variables
+	int _nav_state{0};
+	RoverAckermannGuidance::motor_setpoint _motor_setpoint;
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
+};

src/modules/rover_ackermann/RoverAckermannGuidance/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(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

@@ -0,0 +1,321 @@
+/****************************************************************************
+ *
+ *   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 "RoverAckermannGuidance.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	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::purePursuit()
+{
+	// Initializations
+	float desired_speed{0.f};
+	float desired_steering{0.f};
+	float vehicle_yaw{0.f};
+	float actual_speed{0.f};
+
+	// uORB subscriber updates
+	if (_vehicle_global_position_sub.updated()) {
+		vehicle_global_position_s vehicle_global_position{};
+		_vehicle_global_position_sub.copy(&vehicle_global_position);
+		_curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
+	}
+
+	if (_local_position_sub.updated()) {
+		vehicle_local_position_s local_position{};
+		_local_position_sub.copy(&local_position);
+
+		if (!_global_local_proj_ref.isInitialized()
+		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
+			_global_local_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
+		}
+
+		_curr_pos_local = 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 (_position_setpoint_triplet_sub.updated()) {
+		updateWaypoints();
+	}
+
+	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;
+	}
+
+	// Guidance logic
+	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));
+
+		if (distance_to_prev_wp <= _prev_acc_rad) { // Cornering speed
+			const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acc_rad;
+			desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
+
+		} else { // Default mission speed
+			desired_speed = _param_ra_miss_vel_def.get();
+		}
+
+		// Calculate desired steering
+		desired_steering = calcDesiredSteering(_curr_wp_local, _prev_wp_local, _curr_pos_local, _param_ra_lookahd_gain.get(),
+						       _param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw);
+		desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(),
+						   _param_ra_max_steer_angle.get());
+	}
+
+	// Throttle PID
+	hrt_abstime now = hrt_absolute_time();
+	const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
+	_time_stamp_last = now;
+	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, _param_ra_max_speed.get(),
+						     0.f, 1.f);
+	}
+
+	// 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);
+
+	// Return motor setpoints
+	motor_setpoint motor_setpoint_temp;
+	motor_setpoint_temp.steering = math::interpolate<float>(desired_steering, -_param_ra_max_steer_angle.get(),
+				       _param_ra_max_steer_angle.get(),
+				       -1.f, 1.f);
+	motor_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f);
+	return motor_setpoint_temp;
+}
+
+void RoverAckermannGuidance::updateWaypoints()
+{
+	position_setpoint_triplet_s position_setpoint_triplet{};
+	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+
+	// Global waypoint coordinates
+	_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
+
+	if (position_setpoint_triplet.previous.valid) {
+		_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
+
+	} else {
+		_prev_wp = _curr_pos;
+	}
+
+	if (position_setpoint_triplet.next.valid) {
+		_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
+
+	} else {
+		_next_wp = _curr_wp;
+	}
+
+	// Local waypoint coordinates
+	_curr_wp_local = _global_local_proj_ref.project(_curr_wp(0), _curr_wp(1));
+	_prev_wp_local = _global_local_proj_ref.project(_prev_wp(0), _prev_wp(1));
+	_next_wp_local = _global_local_proj_ref.project(_next_wp(0), _next_wp(1));
+
+	// Update acceptance radius
+	_prev_acc_rad = _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());
+}
+
+float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+		const Vector2f &next_wp_local, 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_local = prev_wp_local - curr_wp_local;
+	const Vector2f curr_to_next_wp_local = next_wp_local - curr_wp_local;
+	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_local.norm() > FLT_EPSILON && curr_to_prev_wp_local.norm() > FLT_EPSILON) {
+		const float theta = acosf((curr_to_prev_wp_local * curr_to_next_wp_local) / (curr_to_prev_wp_local.norm() *
+					  curr_to_next_wp_local.norm())) / 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{};
+	pos_ctrl_status.acceptance_radius = acceptance_radius;
+	pos_ctrl_status.timestamp = hrt_absolute_time();
+	_position_controller_status_pub.publish(pos_ctrl_status);
+	return acceptance_radius;
+}
+
+float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+		const Vector2f &curr_pos_local, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max,
+		const float &wheel_base, const float &desired_speed, const float &vehicle_yaw)
+{
+	// Calculate crosstrack error
+	const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local;
+
+	if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen)
+		return 0.f;
+	}
+
+	const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local;
+	const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) /
+			prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized();
+	const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local;
+
+	// Calculate desired heading towards lookahead point
+	float desired_heading{0.f};
+	float lookahead_distance = math::constrain(lookahead_gain * desired_speed,
+				   lookahead_min, lookahead_max);
+
+	if (crosstrack_error.longerThan(lookahead_distance)) {
+		if (crosstrack_error.norm() < lookahead_max) {
+			lookahead_distance = crosstrack_error.norm(); // Scale lookahead radius
+			desired_heading = calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, lookahead_distance);
+
+		} else { // Excessively large crosstrack error
+			desired_heading = calcDesiredHeading(curr_wp_local, curr_pos_local, curr_pos_local, lookahead_distance);
+		}
+
+	} else { // Crosstrack error smaller than lookahead
+		desired_heading = calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, lookahead_distance);
+	}
+
+	// Calculate desired steering to reach lookahead point
+	const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw);
+
+	// For logging
+	_rover_ackermann_guidance_status.lookahead_distance = lookahead_distance;
+	_rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F);
+	_rover_ackermann_guidance_status.crosstrack_error = crosstrack_error.norm();
+
+	// Calculate desired steering
+	if (math::abs_t(heading_error) <= M_PI_2_F) {
+		return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance);
+
+	} else if (heading_error > FLT_EPSILON) {
+		return atanf(2 * wheel_base * (1.0f + sinf(heading_error - M_PI_2_F)) /
+			     lookahead_distance);
+
+	} else {
+		return atanf(2 * wheel_base * (-1.0f + sinf(heading_error + M_PI_2_F)) /
+			     lookahead_distance);
+	}
+}
+
+float RoverAckermannGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+		const Vector2f &curr_pos_local,
+		const float &lookahead_distance)
+{
+	// Setup variables
+	const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0));
+	const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local(
+			0) - prev_wp_local(0));
+	const float a = -line_segment_slope;
+	const float c = -line_segment_rover_offset;
+	const float r = lookahead_distance;
+	const float x0 = -a * c / (a * a + 1.0f);
+	const float y0 = -c / (a * a + 1.0f);
+
+	// Calculate intersection points
+	if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist
+		return 0.f;
+
+	} else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists
+		return atan2f(y0, x0);
+
+	} else { // Two intersetion points exist
+		const float d = r * r - c * c / (a * a + 1.0f);
+		const float mult = sqrt(d / (a * a + 1.0f));
+		const float ax = x0 + mult;
+		const float bx = x0 - mult;
+		const float ay = y0 - a * mult;
+		const float by = y0 + a * mult;
+		const Vector2f point1(ax, ay);
+		const Vector2f point2(bx, by);
+		const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1;
+		const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2;
+
+		// Return intersection point closer to current waypoint
+		if (distance1.norm_squared() < distance2.norm_squared()) {
+			return atan2f(ay, ax);
+
+		} else {
+			return atan2f(by, bx);
+		}
+	}
+}

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -0,0 +1,190 @@
+/****************************************************************************
+ *
+ *   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/Subscription.hpp>
+#include <uORB/topics/rover_ackermann_guidance_status.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/home_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/mission_result.h>
+
+// Standard library includes
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+#include <lib/pid/pid.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for ackermann drive guidance.
+ */
+class RoverAckermannGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for RoverAckermannGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	RoverAckermannGuidance(ModuleParams *parent);
+	~RoverAckermannGuidance() = default;
+
+	struct motor_setpoint {
+		float steering{0.f};
+		float throttle{0.f};
+	};
+
+	/**
+	 * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering.
+	 */
+	motor_setpoint purePursuit();
+
+	/**
+	 * @brief Update global/local waypoint coordinates and acceptance radius
+	 */
+	void updateWaypoints();
+
+	/**
+	 * @brief Returns and publishes 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_local Current waypoint in local frame.
+	 * @param prev_wp_local Previous waypoint in local frame.
+	 * @param next_wp_local Next waypoint in local frame.
+	 * @param default_acceptance_radius Default acceptance radius for waypoints.
+	 * @param acceptance_radius_gain Scaling of the geometric optimal acceptance radius for the rover to cut corners.
+	 * @param acceptance_radius_max Maximum value for the acceptance radius.
+	 * @param wheel_base Rover wheelbase.
+	 * @param max_steer_angle Rover maximum steer angle.
+	 */
+	float updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
+				     const Vector2f &next_wp_local, const float &default_acceptance_radius, const float &acceptance_radius_gain,
+				     const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle);
+
+	/**
+	 * @brief Calculate and return desired steering input
+	 * @param curr_wp_local Current waypoint in local frame.
+	 * @param prev_wp_local Previous waypoint in local frame.
+	 * @param curr_pos_local Current position of the vehicle in local frame.
+	 * @param lookahead_gain Tuning parameter for the lookahead distance pure pursuit controller.
+	 * @param lookahead_min Minimum lookahead distance.
+	 * @param lookahead_max Maximum lookahead distance.
+	 * @param wheel_base Rover wheelbase.
+	 * @param desired_speed Desired speed for the rover.
+	 * @param vehicle_yaw Current yaw of the rover.
+	 */
+	float calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local,
+				  const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base,
+				  const float &desired_speed, const float &vehicle_yaw);
+
+	/**
+	 * @brief Return desired heading to the intersection point between a circle with a radius of
+	 * lookahead_distance around the vehicle and a line segment from the previous to the current waypoint.
+	 * @param curr_wp_local Current waypoint in local frame.
+	 * @param prev_wp_local Previous waypoint in local frame.
+	 * @param curr_pos_local Current position of the vehicle in local frame.
+	 * @param lookahead_distance Radius of circle around vehicle.
+	 */
+	float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local,
+				 const float &lookahead_distance);
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	// 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 publications
+	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{};
+
+
+	MapProjection _global_local_proj_ref{}; // Transform global to local coordinates.
+
+	// Rover variables
+	Vector2d _curr_pos{};
+	Vector2f _curr_pos_local{};
+	PID_t _pid_throttle;
+	hrt_abstime _time_stamp_last{0};
+
+	// Waypoint variables
+	Vector2d _curr_wp{};
+	Vector2d _next_wp{};
+	Vector2d _prev_wp{};
+	Vector2f _curr_wp_local{};
+	Vector2f _prev_wp_local{};
+	Vector2f _next_wp_local{};
+	float _acceptance_radius{0.5f};
+	float _prev_acc_rad{0.f};
+	bool _mission_finished{false};
+
+	// 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_LOOKAHD_GAIN>) _param_ra_lookahd_gain,
+		(ParamFloat<px4::params::RA_LOOKAHD_MAX>) _param_ra_lookahd_max,
+		(ParamFloat<px4::params::RA_LOOKAHD_MIN>) _param_ra_lookahd_min,
+		(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
+		(ParamFloat<px4::params::RA_ACC_RAD_DEF>) _param_ra_acc_rad_def,
+		(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_VEL_MIN>) _param_ra_miss_vel_min,
+		(ParamFloat<px4::params::RA_MISS_VEL_GAIN>) _param_ra_miss_vel_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::NAV_ACC_RAD>) _param_nav_acc_rad
+	)
+};

src/modules/rover_ackermann/module.yaml

@@ -0,0 +1,177 @@
+module_name: Rover Ackermann
+
+parameters:
+  - group: Rover Ackermann
+    definitions:
+      RA_WHEEL_BASE:
+        description:
+          short: Wheel base
+          long: Distance from the front to the rear axle
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+
+      RA_MAX_STR_ANG:
+        description:
+          short: Maximum steering angle
+          long: The maximum angle that the rover can steer
+        type: float
+        unit: rad
+        min: 0.1
+        max: 1.5708
+        increment: 0.01
+        decimal: 2
+        default: 0.5236
+
+      RA_LOOKAHD_GAIN:
+        description:
+          short: Tuning parameter for the pure pursuit controller
+          long: Lower value -> More aggressive controller (beware overshoot/oscillations)
+        type: float
+        min: 0.1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RA_LOOKAHD_MAX:
+        description:
+          short: Maximum lookahead distance for the pure pursuit controller
+          long: |
+            This is the maximum crosstrack error before the controller starts
+            targeting the current waypoint rather then the path between the previous
+            and next waypoint.
+        type: float
+        unit: m
+        min: 0.1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 10
+
+      RA_LOOKAHD_MIN:
+        description:
+          short: Minimum lookahead distance for the pure pursuit controller
+        type: float
+        unit: m
+        min: 0.1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RA_ACC_RAD_MAX:
+        description:
+          short: Maximum acceptance radius
+          long: |
+            The controller scales the acceptance radius based on the angle between
+            the previous, current and next waypoint. Used as tuning parameter.
+            Higher value -> smoother trajectory at the cost of how close the rover gets
+            to the waypoint (Set equal to RA_ACC_RAD_DEF to disable corner cutting).
+        type: float
+        unit: m
+        min: 0.1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 3
+
+      RA_ACC_RAD_DEF:
+        description:
+          short: Default acceptance radius
+        type: float
+        unit: m
+        min: 0.1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5
+
+      RA_ACC_RAD_GAIN:
+        description:
+          short: Tuning parameter for corner cutting
+          long: |
+            The geometric ideal acceptance radius is multiplied by this factor
+            to account for kinematic and dynamic effects.
+            Higher value -> The rover starts to cut the corner earlier.
+        type: float
+        min: 1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 2
+
+      RA_MISS_VEL_DEF:
+        description:
+          short: Default rover velocity during a mission
+        type: float
+        unit: m/s
+        min: 0.1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 3
+
+      RA_MISS_VEL_MIN:
+        description:
+          short: Minimum rover velocity during a mission
+          long: |
+            The velocity off the rover is reduced based on the corner it has to take
+            to smooth the trajectory (To disable this feature set it equal to RA_MISSION_VEL_DEF)
+        type: float
+        unit: m/s
+        min: 0.1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RA_MISS_VEL_GAIN:
+        description:
+          short: Tuning parameter for the velocity reduction during cornering
+          long: Lower value -> More velocity reduction during cornering
+        type: float
+        min: 0.1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 5
+
+      RA_SPEED_P:
+        description:
+          short: Proportional gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RA_SPEED_I:
+        description:
+          short: Integral gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RA_MAX_SPEED:
+        description:
+          short: Speed the rover drives at maximum throttle
+          long: |
+            This is used for the feed-forward term of the speed controller.
+            A value of -1 disables the feed-forward term in which case the
+            Integrator (RA_SPEED_I) becomes necessary to track speed setpoints.
+        type: float
+        unit: m/s
+        min: -1
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: -1