Rover ackermann module

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

@@ -0,0 +1,45 @@
+#!/bin/sh
+# @name Rover Gazebo
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_ackermann_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Waypoint acceptance radius
+param set-default NAV_ACC_RAD 0.5
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
+param set-default SIM_GZ_WH_MIN2 0
+param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_DIS2 100
+
+param set-default SIM_GZ_WH_FUNC3 103 # left wheel
+param set-default SIM_GZ_WH_MIN3 0
+param set-default SIM_GZ_WH_MAX3 200
+param set-default SIM_GZ_WH_DIS3 100
+
+param set-default SIM_GZ_WH_FUNC4 104 # left wheel
+param set-default SIM_GZ_WH_MIN4 0
+param set-default SIM_GZ_WH_MAX4 200
+param set-default SIM_GZ_WH_DIS4 100
+
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_FUNC2 202

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

@@ -83,6 +83,7 @@ px4_add_romfs_files(
 	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
 	4011_gz_lawnmower
+	4012_gz_rover
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -84,6 +84,13 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()
 
+if(CONFIG_MODULES_ACKERMANN_DRIVE)
+	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,38 @@
+#!/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
+
+param set-default BAT1_N_CELLS 3
+
+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
+
+# Failsaves
+param set-default COM_FAIL_ACT_T 1 # Delay before failsafe after rc loss
+param set-default NAV_RCL_ACT 6 # Disarm on manual control loss
+
+# Motors
+param set-default PWM_AUX_TIM0 50
+param set-default PWM_AUX_FUNC1 201
+param set-default CA_R_REV 1 # Make motor bidirectional
+param set-default PWM_AUX_REV 2 # Reverse throttle
+param set-default PWM_AUX_FUNC2 101
+param set-default PWM_AUX_DIS1 1500
+param set-default PWM_AUX_DIS2 1500
+param set-default PWM_AUX_MAX2 1900
+param set-default PWM_AUX_MIN2 1100
+
+# Geometry
+param set-default AD_WHEEL_BASE 0.321

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

@@ -149,6 +149,12 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()
 
+if(CONFIG_MODULES_ACKERMANN_DRIVE)
+	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.
+ackermann_drive start
+
+# Start Land Detector.
+land_detector start rover

ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults

@@ -0,0 +1,11 @@
+#!/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 3 # Right and left motors reversible
+
+param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying

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-v5x/default.px4board

@@ -47,6 +47,7 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
+CONFIG_MODULES_ACKERMANN_DRIVE=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y

boards/px4/sitl/default.px4board

@@ -5,6 +5,7 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_TONE_ALARM=y
+CONFIG_MODULES_ACKERMANN_DRIVE=y
 CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y

msg/AckermannDriveSetpoint.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 speed # [m/s] requested rover ground speed
+float32 steering # [rad] requested rover steering angle
+bool manual # manual steering flag
+float32 speed_actual # [m/s] actual rover ground speed (for loging)
+float32 lookahead_distance # [m] pure pursuit controller lookahead distance (for loging)
+float32 heading_error # [deg] Heading error towards lookahead point (for loging)
+
+# TOPICS ackermann_drive_setpoint

msg/CMakeLists.txt

@@ -37,6 +37,7 @@ cmake_policy(SET CMP0057 NEW)
 include(px4_list_make_absolute)
 
 set(msg_files
+	AckermannDriveSetpoint.msg
 	ActionRequest.msg
 	ActuatorArmed.msg
 	ActuatorControlsStatus.msg

src/modules/ackermann_drive/AckermannDrive.cpp

@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ *   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 "AckermannDrive.hpp"
+
+using namespace time_literals;
+using namespace matrix;
+
+AckermannDrive::AckermannDrive() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+}
+
+bool AckermannDrive::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void AckermannDrive::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void AckermannDrive::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	// uORB subscriber updates
+	if (_parameter_update_sub.updated()) {
+		updateParams();
+	}
+
+	_vehicle_status_sub.update(&_vehicle_status);
+
+	// Navigation modes
+	switch (_vehicle_status.nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL: // Manual mode
+		if (_manual_control_setpoint_sub.updated()) {
+			ackermann_drive_setpoint_s _ackermann_drive_setpoint{};
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+				_ackermann_drive_setpoint.timestamp = hrt_absolute_time();
+				_ackermann_drive_setpoint.speed = manual_control_setpoint.throttle;
+				_ackermann_drive_setpoint.steering = manual_control_setpoint.roll;
+				_ackermann_drive_setpoint.manual = true;
+				_ackermann_drive_setpoint_pub.publish(_ackermann_drive_setpoint);
+			}
+		}
+
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: // Mission mode
+		_ackermann_drive_guidance.purePursuit();
+		break;
+
+	default: // Unimplemented nav states will stop the rover
+		ackermann_drive_setpoint_s _ackermann_drive_setpoint{};
+		_ackermann_drive_setpoint.timestamp = hrt_absolute_time();
+		_ackermann_drive_setpoint.speed = 0.f;
+		_ackermann_drive_setpoint.steering = 0.f;
+		_ackermann_drive_setpoint.manual = false;
+		_ackermann_drive_setpoint_pub.publish(_ackermann_drive_setpoint);
+		break;
+	}
+
+	_ackermann_drive_control.actuatorControl();
+}
+
+int AckermannDrive::task_spawn(int argc, char *argv[])
+{
+	AckermannDrive *instance = new AckermannDrive();
+
+	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 AckermannDrive::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int AckermannDrive::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("ackermann_drive", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int ackermann_drive_main(int argc, char *argv[])
+{
+	return AckermannDrive::main(argc, argv);
+}