ackermann: separate actuator control

src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 "AckermannActControl.hpp"
+
+using namespace time_literals;
+
+AckermannActControl::AckermannActControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+}
+
+void AckermannActControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) {
+		_servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get());
+	}
+
+	if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
+		_motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
+	}
+}
+
+void AckermannActControl::updateActControl()
+{
+	const hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	generateActuatorSetpoint();
+
+}
+
+void AckermannActControl::generateActuatorSetpoint()
+{
+	// Motor control
+	rover_throttle_setpoint_s rover_throttle_setpoint{};
+	_rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint);
+	actuator_motors_s actuator_motors_sub{};
+	_actuator_motors_sub.copy(&actuator_motors_sub);
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get();
+	actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint,
+				     rover_throttle_setpoint.throttle_body_x, actuator_motors_sub.control[0], _param_ro_accel_limit.get(),
+				     _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt);
+	actuator_motors.timestamp = _timestamp;
+	_actuator_motors_pub.publish(actuator_motors);
+
+	// Servo control
+	rover_steering_setpoint_s rover_steering_setpoint{};
+	_rover_steering_setpoint_sub.copy(&rover_steering_setpoint);
+	actuator_servos_s actuator_servos_sub{};
+	_actuator_servos_sub.copy(&actuator_servos_sub);
+
+	if (_param_ra_str_rate_limit.get() > FLT_EPSILON
+	    && _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured
+		if (fabsf(_servo_setpoint.getState() - actuator_servos_sub.control[0]) > fabsf(
+			    rover_steering_setpoint.normalized_steering_angle -
+			    actuator_servos_sub.control[0])) {
+			_servo_setpoint.setForcedValue(actuator_servos_sub.control[0]);
+		}
+
+		_servo_setpoint.update(rover_steering_setpoint.normalized_steering_angle, _dt);
+
+	} else {
+		_servo_setpoint.setForcedValue(rover_steering_setpoint.normalized_steering_angle);
+	}
+
+	actuator_servos_s actuator_servos{};
+	actuator_servos.control[0] = _servo_setpoint.getState();
+	actuator_servos.timestamp = _timestamp;
+	_actuator_servos_pub.publish(actuator_servos);
+}

src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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>
+
+// Libraries
+#include <lib/rover_control/RoverControl.hpp>
+#include <lib/slew_rate/SlewRate.hpp>
+#include <math.h>
+
+// uORB includes
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/actuator_servos.h>
+#include <uORB/topics/rover_steering_setpoint.h>
+#include <uORB/topics/rover_throttle_setpoint.h>
+
+/**
+ * @brief Class for ackermann actuator control.
+ */
+class AckermannActControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for AckermannActControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	AckermannActControl(ModuleParams *parent);
+	~AckermannActControl() = default;
+
+	/**
+	 * @brief Update actuator controller.
+	 */
+	void updateActControl();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+
+	/**
+	 * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
+	 */
+	void generateActuatorSetpoint();
+
+	// uORB subscriptions
+	uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
+	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
+	uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_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)};
+
+	// Variables
+	hrt_abstime _timestamp{0};
+	float _dt{0.f};
+
+	// Controllers
+	SlewRate<float> _servo_setpoint{0.f};
+	SlewRate<float> _motor_setpoint{0.f};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
+		(ParamFloat<px4::params::RA_STR_RATE_LIM>) _param_ra_str_rate_limit,
+		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
+		(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
+		(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
+		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
+	)
+};

src/modules/rover_ackermann/AckermannActControl/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2025 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(AckermannActControl
+	AckermannActControl.cpp
+)
+
+target_include_directories(AckermannActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/rover_ackermann/CMakeLists.txt

@@ -31,6 +31,7 @@
 #
 ############################################################################
 
+add_subdirectory(AckermannActControl)
 add_subdirectory(AckermannRateControl)
 add_subdirectory(AckermannAttControl)
 add_subdirectory(AckermannVelControl)
@@ -43,6 +44,7 @@ px4_add_module(
 		RoverAckermann.cpp
 		RoverAckermann.hpp
 	DEPENDS
+		AckermannActControl
 		AckermannRateControl
 		AckermannAttControl
 		AckermannVelControl

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -53,14 +53,6 @@ bool RoverAckermann::init()
 void RoverAckermann::updateParams()
 {
 	ModuleParams::updateParams();
-
-	if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) {
-		_servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get());
-	}
-
-	if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
-		_motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
-	}
 }
 
 void RoverAckermann::Run()
@@ -69,15 +61,6 @@ void RoverAckermann::Run()
 		updateParams();
 	}
 
-	const hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	_ackermann_pos_control.updatePosControl();
-	_ackermann_vel_control.updateVelControl();
-	_ackermann_att_control.updateAttControl();
-	_ackermann_rate_control.updateRateControl();
-
 	if (_vehicle_control_mode_sub.updated()) {
 		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
 	}
@@ -90,8 +73,12 @@ void RoverAckermann::Run()
 		generateSteeringAndThrottleSetpoint();
 	}
 
-	generateActuatorSetpoint();
 
+	_ackermann_pos_control.updatePosControl();
+	_ackermann_vel_control.updateVelControl();
+	_ackermann_att_control.updateAttControl();
+	_ackermann_rate_control.updateRateControl();
+	_ackermann_act_control.updateActControl();
 }
 
 void RoverAckermann::generateSteeringAndThrottleSetpoint()
@@ -100,70 +87,17 @@ void RoverAckermann::generateSteeringAndThrottleSetpoint()
 
 	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
 		rover_steering_setpoint_s rover_steering_setpoint{};
-		rover_steering_setpoint.timestamp = _timestamp;
+		rover_steering_setpoint.timestamp = hrt_absolute_time();
 		rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll;
 		_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
 		rover_throttle_setpoint_s rover_throttle_setpoint{};
-		rover_throttle_setpoint.timestamp = _timestamp;
+		rover_throttle_setpoint.timestamp = hrt_absolute_time();
 		rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
 		rover_throttle_setpoint.throttle_body_y = 0.f;
 		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
 	}
 }
 
-void RoverAckermann::generateActuatorSetpoint()
-{
-	if (_rover_throttle_setpoint_sub.updated()) {
-		_rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint);
-	}
-
-	if (_actuator_motors_sub.updated()) {
-		actuator_motors_s actuator_motors{};
-		_actuator_motors_sub.copy(&actuator_motors);
-		_current_motor_setpoint = actuator_motors.control[0];
-	}
-
-	if (_vehicle_control_mode.flag_armed) {
-		actuator_motors_s actuator_motors{};
-		actuator_motors.reversible_flags = _param_r_rev.get();
-		actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint,
-					     _rover_throttle_setpoint.throttle_body_x, _current_motor_setpoint, _param_ro_accel_limit.get(),
-					     _param_ro_decel_limit.get(),
-					     _param_ro_max_thr_speed.get(), _dt);
-		actuator_motors.timestamp = _timestamp;
-		_actuator_motors_pub.publish(actuator_motors);
-	}
-
-	if (_rover_steering_setpoint_sub.updated()) {
-		_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
-	}
-
-	if (_actuator_servos_sub.updated()) {
-		actuator_servos_s actuator_servos{};
-		_actuator_servos_sub.copy(&actuator_servos);
-		_current_servo_setpoint = actuator_servos.control[0];
-	}
-
-	if (_param_ra_str_rate_limit.get() > FLT_EPSILON
-	    && _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured
-		if (fabsf(_servo_setpoint.getState() - _current_servo_setpoint) > fabsf(
-			    _rover_steering_setpoint.normalized_steering_angle -
-			    _current_servo_setpoint)) {
-			_servo_setpoint.setForcedValue(_current_servo_setpoint);
-		}
-
-		_servo_setpoint.update(_rover_steering_setpoint.normalized_steering_angle, _dt);
-
-	} else {
-		_servo_setpoint.setForcedValue(_rover_steering_setpoint.normalized_steering_angle);
-	}
-
-	actuator_servos_s actuator_servos{};
-	actuator_servos.control[0] = _servo_setpoint.getState();
-	actuator_servos.timestamp = _timestamp;
-	_actuator_servos_pub.publish(actuator_servos);
-}
-
 int RoverAckermann::task_spawn(int argc, char *argv[])
 {
 	RoverAckermann *instance = new RoverAckermann();

src/modules/rover_ackermann/RoverAckermann.hpp

@@ -40,24 +40,17 @@
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
-// Libraries
-#include <lib/pure_pursuit/PurePursuit.hpp>
-#include <lib/rover_control/RoverControl.hpp>
-#include <lib/slew_rate/SlewRate.hpp>
-
 // uORB includes
 #include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/actuator_servos.h>
 #include <uORB/topics/rover_steering_setpoint.h>
 #include <uORB/topics/rover_throttle_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/manual_control_setpoint.h>
 
 // Local includes
+#include "AckermannActControl/AckermannActControl.hpp"
 #include "AckermannRateControl/AckermannRateControl.hpp"
 #include "AckermannAttControl/AckermannAttControl.hpp"
 #include "AckermannVelControl/AckermannVelControl.hpp"
@@ -98,52 +91,21 @@ private:
 	 */
 	void generateSteeringAndThrottleSetpoint();
 
-	/**
-	 * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
-	 */
-	void generateActuatorSetpoint();
-
 	// uORB subscriptions
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
-	uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)};
-	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 	vehicle_control_mode_s _vehicle_control_mode{};
-	rover_steering_setpoint_s _rover_steering_setpoint{};
-	rover_throttle_setpoint_s _rover_throttle_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_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
 	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
 
 	// Class instances
+	AckermannActControl  _ackermann_act_control{this};
 	AckermannRateControl _ackermann_rate_control{this};
 	AckermannAttControl  _ackermann_att_control{this};
 	AckermannVelControl  _ackermann_vel_control{this};
 	AckermannPosControl  _ackermann_pos_control{this};
 
-	// Variables
-	hrt_abstime _timestamp{0};
-	float _dt{0.f};
-	float _current_servo_setpoint{0.f};
-	float _current_motor_setpoint{0.f};
-
-	// Controllers
-	SlewRate<float> _servo_setpoint{0.f};
-	SlewRate<float> _motor_setpoint{0.f};
-
-	// Parameters
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
-		(ParamFloat<px4::params::RA_STR_RATE_LIM>) _param_ra_str_rate_limit,
-		(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
-		(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
-		(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
-		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
-	)
 };