rover_mecanum: enable yaw control via MAVLink SET_POSITION_TARGET commands (#26218)

* rover_mecanum: enable yaw control via MAVLink SET_POSITION_TARGET commands

* Maintain the original judgment conditions

---------

Co-authored-by: V <null>

Author: fakerror

src/modules/mavlink/mavlink_receiver.cpp

@@ -1115,6 +1115,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 		ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
 		ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
 		ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
+		ocm.attitude = PX4_ISFINITE(setpoint.yaw);
+		ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed);
 
 		if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_LOCAL_NED force not supported\t");
@@ -1237,6 +1239,8 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 		ocm.position = !matrix::Vector3f(setpoint.position).isAllNan();
 		ocm.velocity = !matrix::Vector3f(setpoint.velocity).isAllNan();
 		ocm.acceleration = !matrix::Vector3f(setpoint.acceleration).isAllNan();
+		ocm.attitude = PX4_ISFINITE(setpoint.yaw);
+		ocm.body_rate = PX4_ISFINITE(setpoint.yawspeed);
 
 		if (ocm.acceleration && (type_mask & POSITION_TARGET_TYPEMASK_FORCE_SET)) {
 			mavlink_log_critical(&_mavlink_log_pub, "SET_POSITION_TARGET_GLOBAL_INT force not supported\t");

src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp

@@ -76,6 +76,11 @@ void MecanumAttControl::updateAttControl()
 		rover_attitude_setpoint_s rover_attitude_setpoint{};
 		_rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint);
 		_yaw_setpoint = rover_attitude_setpoint.yaw_setpoint;
+		_last_yaw_setpoint_timestamp = hrt_absolute_time();
+	}
+
+	if (hrt_elapsed_time(&_last_yaw_setpoint_timestamp) > YAW_SETPOINT_TIMEOUT_US) {
+		_yaw_setpoint = NAN;
 	}
 
 	if (PX4_ISFINITE(_yaw_setpoint)) {

src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp

@@ -34,6 +34,7 @@
 #pragma once
 
 // PX4 includes
+#include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/events.h>
 
@@ -52,6 +53,8 @@
 #include <uORB/topics/rover_attitude_status.h>
 #include <uORB/topics/rover_attitude_setpoint.h>
 
+using namespace time_literals;
+
 /**
  * @brief Class for mecanum attitude control.
  */
@@ -103,6 +106,10 @@ class MecanumAttControl : public ModuleParams
 	float _max_yaw_rate{0.f};
 	float _yaw_setpoint{NAN};
 
+	hrt_abstime _last_yaw_setpoint_timestamp{0};
+	/** Timeout in us for yaw setpoint to get considered invalid */
+	static constexpr uint64_t YAW_SETPOINT_TIMEOUT_US = 500_ms;
+
 	// Controllers
 	PID _pid_yaw;
 	SlewRateYaw<float> _adjusted_yaw_setpoint;

src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp

@@ -88,7 +88,10 @@ void MecanumOffboardMode::offboardControl()
 		rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0));
 		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 
-	} else if (offboard_control_mode.attitude) {
+	}
+
+	// For Mecanum wheel systems, attitude and position control can be decoupled
+	if (offboard_control_mode.attitude) {
 		rover_attitude_setpoint_s rover_attitude_setpoint{};
 		rover_attitude_setpoint.timestamp = hrt_absolute_time();
 		rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;

src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp

@@ -47,8 +47,6 @@ MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent
 void MecanumPosControl::updateParams()
 {
 	ModuleParams::updateParams();
-	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
-
 }
 
 void MecanumPosControl::updatePosControl()
@@ -89,21 +87,13 @@ void MecanumPosControl::updatePosControl()
 			rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0);
 			rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1);
 			_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
-			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 {
 			rover_speed_setpoint_s rover_speed_setpoint{};
 			rover_speed_setpoint.timestamp = timestamp;
 			rover_speed_setpoint.speed_body_x = 0.f;
 			rover_speed_setpoint.speed_body_y = 0.f;
 			_rover_speed_setpoint_pub.publish(rover_speed_setpoint);
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = timestamp;
-			rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 
 			if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) {
 				_stopped = true;
@@ -124,7 +114,6 @@ void MecanumPosControl::updateSubscriptions()
 		vehicle_attitude_s vehicle_attitude{};
 		_vehicle_attitude_sub.copy(&vehicle_attitude);
 		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
 	}
 
 	if (_vehicle_local_position_sub.updated()) {

src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp

@@ -114,9 +114,6 @@ class MecanumPosControl : public ModuleParams
 	Vector2f _start_ned{};
 	Vector2f _target_waypoint_ned{};
 	float _arrival_speed{0.f};
-	float _vehicle_yaw{0.f};
-	float _max_yaw_rate{0.f};
-	float _yaw_setpoint{NAN};
 	float _vehicle_speed{0.f};
 	float _cruising_speed{NAN};
 	bool _stopped{false};