differential: centralize mode management, resets and checks

Author: chfriedrich98

src/modules/rover_differential/CMakeLists.txt

@@ -36,6 +36,7 @@ add_subdirectory(DifferentialRateControl)
 add_subdirectory(DifferentialAttControl)
 add_subdirectory(DifferentialVelControl)
 add_subdirectory(DifferentialPosControl)
+add_subdirectory(DifferentialDriveModes)
 
 px4_add_module(
 	MODULE modules__rover_differential
@@ -49,6 +50,9 @@ px4_add_module(
 		DifferentialAttControl
 		DifferentialVelControl
 		DifferentialPosControl
+		DifferentialAutoMode
+		DifferentialManualMode
+		DifferentialOffboardMode
 		px4_work_queue
 		rover_control
 		pure_pursuit

src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp

@@ -99,21 +99,6 @@ Vector2f DifferentialActControl::computeInverseKinematics(float throttle, const
 			throttle + speed_diff_normalized);
 }
 
-void DifferentialActControl::manualManualMode()
-{
-	manual_control_setpoint_s manual_control_setpoint{};
-	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
-	rover_steering_setpoint_s rover_steering_setpoint{};
-	rover_steering_setpoint.timestamp = hrt_absolute_time();
-	rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll;
-	_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
-	rover_throttle_setpoint_s rover_throttle_setpoint{};
-	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 DifferentialActControl::stopVehicle()
 {
 	actuator_motors_s actuator_motors{};

src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp

@@ -67,11 +67,6 @@ class DifferentialActControl : public ModuleParams
 	 */
 	void updateActControl();
 
-	/**
-	 * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
-	 */
-	void manualManualMode();
-
 	/**
 	 * @brief Stop the vehicle by sending 0 commands to motors and servos.
 	 */
@@ -96,12 +91,9 @@ class DifferentialActControl : public ModuleParams
 	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::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 
 	// uORB publications
-	uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
-	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
+	uORB::Publication<actuator_motors_s> 	     _actuator_motors_pub{ORB_ID(actuator_motors)};
 
 	// Variables
 	hrt_abstime _timestamp{0};

src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp

@@ -38,8 +38,6 @@ using namespace time_literals;
 DifferentialAttControl::DifferentialAttControl(ModuleParams *parent) : ModuleParams(parent)
 {
 	_rover_rate_setpoint_pub.advertise();
-	_rover_throttle_setpoint_pub.advertise();
-	_rover_attitude_setpoint_pub.advertise();
 	_rover_attitude_status_pub.advertise();
 	updateParams();
 }
@@ -52,21 +50,20 @@ void DifferentialAttControl::updateParams()
 		_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
 	}
 
+	// Set up PID controller
 	_pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f);
 	_pid_yaw.setIntegralLimit(_max_yaw_rate);
 	_pid_yaw.setOutputLimit(_max_yaw_rate);
+
+	// Set up slew rate
 	_adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate);
 }
 
 void DifferentialAttControl::updateAttControl()
 {
-	const hrt_abstime timestamp_prev = _timestamp;
+	hrt_abstime timestamp_prev = _timestamp;
 	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
@@ -75,17 +72,20 @@ void DifferentialAttControl::updateAttControl()
 		_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
 	}
 
-	if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-
-		if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
-			generateAttitudeAndThrottleSetpoint();
-		}
+	if (_rover_attitude_setpoint_sub.updated()) {
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		_rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint);
+		_yaw_setpoint = rover_attitude_setpoint.yaw_setpoint;
+	}
 
-		generateRateSetpoint();
+	if (PX4_ISFINITE(_yaw_setpoint)) {
+		const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
+						_vehicle_yaw, _yaw_setpoint, dt);
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = _timestamp;
+		rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
 
-	} else { // Reset pid and slew rate when attitude control is not active
-		_pid_yaw.resetIntegral();
-		_adjusted_yaw_setpoint.setForcedValue(0.f);
 	}
 
 	// Publish attitude controller status (logging only)
@@ -97,96 +97,6 @@ void DifferentialAttControl::updateAttControl()
 
 }
 
-void DifferentialAttControl::generateAttitudeAndThrottleSetpoint()
-{
-	const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-				       && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
-
-	if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode
-		manual_control_setpoint_s manual_control_setpoint{};
-
-		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-
-			rover_throttle_setpoint_s rover_throttle_setpoint{};
-			rover_throttle_setpoint.timestamp = _timestamp;
-			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);
-
-			const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
-						_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
-						_max_yaw_rate / _param_ro_yaw_p.get());
-
-			if (fabsf(yaw_delta) > FLT_EPSILON
-			    || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control
-				_stab_yaw_ctl = false;
-				const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
-				rover_attitude_setpoint_s rover_attitude_setpoint{};
-				rover_attitude_setpoint.timestamp = _timestamp;
-				rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
-				_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-
-			} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
-				if (!_stab_yaw_ctl) {
-					_stab_yaw_setpoint = _vehicle_yaw;
-					_stab_yaw_ctl = true;
-				}
-
-				rover_attitude_setpoint_s rover_attitude_setpoint{};
-				rover_attitude_setpoint.timestamp = _timestamp;
-				rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
-				_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-			}
-
-
-		}
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control
-		trajectory_setpoint_s trajectory_setpoint{};
-		_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		const bool offboard_att_control = _offboard_control_mode.attitude && !_offboard_control_mode.position
-						  && !_offboard_control_mode.velocity;
-
-		if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) {
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = _timestamp;
-			rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-		}
-	}
-}
-
-void DifferentialAttControl::generateRateSetpoint()
-{
-	if (_rover_attitude_setpoint_sub.updated()) {
-		_rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint);
-	}
-
-	if (_rover_rate_setpoint_sub.updated()) {
-		_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
-	}
-
-	// Check if a new rate setpoint was already published from somewhere else
-	if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update
-	    && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) {
-		return;
-	}
-
-	const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
-					_vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt);
-
-	_last_rate_setpoint_update = _timestamp;
-	rover_rate_setpoint_s rover_rate_setpoint{};
-	rover_rate_setpoint.timestamp = _timestamp;
-	rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
-	_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-}
-
 bool DifferentialAttControl::runSanityChecks()
 {
 	bool ret = true;
@@ -197,13 +107,9 @@ bool DifferentialAttControl::runSanityChecks()
 
 	if (_param_ro_yaw_p.get() < FLT_EPSILON) {
 		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("differential_att_control_conf_invalid_yaw_p"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get());
-		}
+		events::send<float>(events::ID("differential_att_control_conf_invalid_yaw_p"), events::Log::Error,
+				    "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get());
 	}
 
-	_prev_param_check_passed = ret;
 	return ret;
 }

src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp

@@ -48,15 +48,9 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/rover_rate_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/rover_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/rover_attitude_status.h>
-#include <uORB/topics/rover_attitude_setpoint.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/trajectory_setpoint.h>
 
 /**
  * @brief Class for differential attitude control.
@@ -72,63 +66,41 @@ class DifferentialAttControl : public ModuleParams
 	~DifferentialAttControl() = default;
 
 	/**
-	 * @brief Update attitude controller.
+	 * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
 	 */
 	void updateAttControl();
 
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
 	/**
-	 * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode)
-	 * 	  or trajectorySetpoint (Offboard attitude control).
+	 * @brief Reset attitude controller.
 	 */
-	void generateAttitudeAndThrottleSetpoint();
-
-	/**
-	 * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
-	 */
-	void generateRateSetpoint();
+	void reset() {_pid_yaw.resetIntegral(); _yaw_setpoint = NAN;};
 
 	/**
 	 * @brief Check if the necessary parameters are set.
 	 * @return True if all checks pass.
 	 */
 	bool runSanityChecks();
 
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
 	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 	uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
-	vehicle_control_mode_s _vehicle_control_mode{};
-	rover_attitude_setpoint_s _rover_attitude_setpoint{};
-	rover_rate_setpoint_s _rover_rate_setpoint{};
-	offboard_control_mode_s _offboard_control_mode{};
 
 	// uORB publications
-	uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Publication<rover_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
+	uORB::Publication<rover_rate_setpoint_s>     _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
+	uORB::Publication<rover_attitude_status_s>   _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
 
 	// Variables
-	hrt_abstime _timestamp{0};
-	hrt_abstime _last_rate_setpoint_update{0};
 	float _vehicle_yaw{0.f};
-	float _dt{0.f};
+	hrt_abstime _timestamp{0};
 	float _max_yaw_rate{0.f};
-	float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode
-	bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode
-	bool _prev_param_check_passed{true};
+	float _yaw_setpoint{NAN};
 
 	// Controllers
 	PID _pid_yaw;

src/modules/rover_differential/DifferentialDriveModes/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(DifferentialAutoMode)
+add_subdirectory(DifferentialManualMode)
+add_subdirectory(DifferentialOffboardMode)