From f4247aee58fa731f33e893ce8f2533c1427cdc96 Mon Sep 17 00:00:00 2001 From: fakerror <7368920+fakerror@users.noreply.github.com> Date: Fri, 9 Jan 2026 21:08:53 +0800 Subject: [PATCH] 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 --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++++ .../MecanumAttControl/MecanumAttControl.cpp | 5 +++++ .../MecanumAttControl/MecanumAttControl.hpp | 7 +++++++ .../MecanumOffboardMode/MecanumOffboardMode.cpp | 5 ++++- .../MecanumPosControl/MecanumPosControl.cpp | 11 ----------- .../MecanumPosControl/MecanumPosControl.hpp | 3 --- 6 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8ac70225a0..4e01ca3683 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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"); diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp index 3d3a1cb6e6..bacf072cf0 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp +++ b/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)) { diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp index 327b4652ad..88879380bb 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -34,6 +34,7 @@ #pragma once // PX4 includes +#include #include #include @@ -52,6 +53,8 @@ #include #include +using namespace time_literals; + /** * @brief Class for mecanum attitude control. */ @@ -103,6 +106,10 @@ private: 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 _adjusted_yaw_setpoint; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp index f217ada200..fec1fab36b 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp +++ b/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; diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index 3ec9be67d3..969bb28b30 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/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,10 +87,6 @@ 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{}; @@ -100,10 +94,6 @@ void MecanumPosControl::updatePosControl() 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()) { diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index 54880657ec..7e89f6c711 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -114,9 +114,6 @@ private: 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};