added ackermann module

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/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

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,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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();
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		_vehicle_status_sub.copy(&_vehicle_status);
+	}
+
+	// Navigation modes
+	switch (_vehicle_status.nav_state) {
+	case VEHICLE_STATUS_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_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);
+}

src/modules/ackermann_drive/AckermannDrive.hpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/ackermann_drive_setpoint.h>
+
+// Standard library includes
+#include <math.h>
+
+// Local includes
+#include "AckermannDriveControl/AckermannDriveControl.hpp"
+#include "AckermannDriveGuidance/AckermannDriveGuidance.hpp"
+
+// Defines for nav state
+#define VEHICLE_STATUS_NAVIGATION_STATE_MANUAL 0
+#define VEHICLE_STATUS_NAVIGATION_STATE_AUTO_MISSION 3
+
+using namespace time_literals;
+
+class AckermannDrive : public ModuleBase<AckermannDrive>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	/**
+	 * @brief Constructor for AckermannDrive
+	 */
+	AckermannDrive();
+	~AckermannDrive() 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 _ackermann_drive_setpoint_sub{ORB_ID(ackermann_drive_setpoint)};
+	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)};
+	vehicle_status_s _vehicle_status{};
+
+	// uORB sublications
+	uORB::Publication<ackermann_drive_setpoint_s> _ackermann_drive_setpoint_pub{ORB_ID(ackermann_drive_setpoint)};
+
+	// Instances
+	AckermannDriveControl _ackermann_drive_control{this};
+	AckermannDriveGuidance _ackermann_drive_guidance{this};
+};