From 1e3ca6f63e782a1ebf59ea9d9d54d272e2cd993a Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Fri, 21 Mar 2025 14:42:43 +0100 Subject: [PATCH 1/2] mecanum: separate velocity control --- msg/CMakeLists.txt | 1 + msg/MecanumVelocitySetpoint.msg | 5 + src/modules/logger/logged_topics.cpp | 1 + src/modules/rover_mecanum/CMakeLists.txt | 6 +- .../MecanumAttControl/MecanumAttControl.cpp | 2 +- .../MecanumAttControl/MecanumAttControl.hpp | 2 +- .../CMakeLists.txt | 10 +- .../MecanumPosControl/MecanumPosControl.cpp | 351 ++++++++++++++ .../MecanumPosControl.hpp} | 64 ++- .../MecanumPosVelControl.cpp | 432 ------------------ .../MecanumVelControl/CMakeLists.txt | 40 ++ .../MecanumVelControl/MecanumVelControl.cpp | 249 ++++++++++ .../MecanumVelControl/MecanumVelControl.hpp | 157 +++++++ src/modules/rover_mecanum/RoverMecanum.cpp | 7 +- src/modules/rover_mecanum/RoverMecanum.hpp | 14 +- 15 files changed, 855 insertions(+), 486 deletions(-) create mode 100644 msg/MecanumVelocitySetpoint.msg rename src/modules/rover_mecanum/{MecanumPosVelControl => MecanumPosControl}/CMakeLists.txt (86%) create mode 100644 src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp rename src/modules/rover_mecanum/{MecanumPosVelControl/MecanumPosVelControl.hpp => MecanumPosControl/MecanumPosControl.hpp} (78%) delete mode 100644 src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp create mode 100644 src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt create mode 100644 src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp create mode 100644 src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 4d5a19cfa0e4..be9e55f0f921 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -131,6 +131,7 @@ set(msg_files ManualControlSwitches.msg MavlinkLog.msg MavlinkTunnel.msg + MecanumVelocitySetpoint.msg MessageFormatRequest.msg MessageFormatResponse.msg Mission.msg diff --git a/msg/MecanumVelocitySetpoint.msg b/msg/MecanumVelocitySetpoint.msg new file mode 100644 index 000000000000..0af422f31356 --- /dev/null +++ b/msg/MecanumVelocitySetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 speed # [m/s] [-inf, inf] Speed setpoint +float32 bearing # [rad] [-pi, pi] from North. +float32 yaw # [rad] [-pi, pi] (Optional, defaults to current vehicle yaw) Vehicle yaw setpoint in NED frame diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index ac146f597504..89bb5f0d7591 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -93,6 +93,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("magnetometer_bias_estimate", 200); add_topic("manual_control_setpoint", 200); add_topic("manual_control_switches"); + add_optional_topic("mecanum_velocity_setpoint", 100); add_topic("mission_result"); add_topic("navigator_mission_item"); add_topic("navigator_status"); diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index 0dfcb5fd480a..998a2c38088d 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -33,7 +33,8 @@ add_subdirectory(MecanumRateControl) add_subdirectory(MecanumAttControl) -add_subdirectory(MecanumPosVelControl) +add_subdirectory(MecanumVelControl) +add_subdirectory(MecanumPosControl) px4_add_module( MODULE modules__rover_mecanum @@ -44,7 +45,8 @@ px4_add_module( DEPENDS MecanumRateControl MecanumAttControl - MecanumPosVelControl + MecanumVelControl + MecanumPosControl px4_work_queue rover_control pure_pursuit diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp index 0582c5f2995a..1c4a93d4d601 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -92,7 +92,7 @@ void MecanumAttControl::updateAttControl() rover_attitude_status_s rover_attitude_status; rover_attitude_status.timestamp = _timestamp; rover_attitude_status.measured_yaw = _vehicle_yaw; - rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + rover_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); _rover_attitude_status_pub.publish(rover_attitude_status); } diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp index c6e9a3353e71..b382e789a5b0 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -126,7 +126,7 @@ class MecanumAttControl : public ModuleParams float _vehicle_yaw{0.f}; float _dt{0.f}; float _max_yaw_rate{0.f}; - float _stab_yaw_setpoint{0.f}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad] + float _stab_yaw_setpoint{NAN}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad] bool _prev_param_check_passed{true}; // Controllers diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt b/src/modules/rover_mecanum/MecanumPosControl/CMakeLists.txt similarity index 86% rename from src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt rename to src/modules/rover_mecanum/MecanumPosControl/CMakeLists.txt index 692e4e661b6d..2cb91acd9339 100644 --- a/src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt +++ b/src/modules/rover_mecanum/MecanumPosControl/CMakeLists.txt @@ -31,10 +31,10 @@ # ############################################################################ -px4_add_library(MecanumPosVelControl - MecanumPosVelControl.cpp +px4_add_library(MecanumPosControl + MecanumPosControl.cpp ) -target_link_libraries(MecanumPosVelControl PUBLIC PID) -target_link_libraries(MecanumPosVelControl PUBLIC pure_pursuit) -target_include_directories(MecanumPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(MecanumPosControl PUBLIC PID) +target_link_libraries(MecanumPosControl PUBLIC pure_pursuit) +target_include_directories(MecanumPosControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp new file mode 100644 index 000000000000..6e2146be7b30 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -0,0 +1,351 @@ +/**************************************************************************** + * + * 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 "MecanumPosControl.hpp" + +using namespace time_literals; + +MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent) +{ + _mecanum_velocity_setpoint_pub.advertise(); + _rover_position_setpoint_pub.advertise(); + _pure_pursuit_status_pub.advertise(); + + // Initially set to NaN to indicate that the rover has no position setpoint + _rover_position_setpoint.position_ned[0] = NAN; + _rover_position_setpoint.position_ned[1] = NAN; + + updateParams(); +} + +void MecanumPosControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + +} + +void MecanumPosControl::updatePosControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_offboard_enabled) { + generatePositionSetpoint(); + } + + generateVelocitySetpoint(); + + } +} + +void MecanumPosControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + 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()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + } + +} + +void MecanumPosControl::generatePositionSetpoint() +{ + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (!_offboard_control_mode.position) { + return; + } + + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover position setpoint + rover_position_setpoint_s rover_position_setpoint{}; + rover_position_setpoint.timestamp = _timestamp; + rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; + rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; + rover_position_setpoint.cruising_speed = _param_ro_speed_limit.get(); + rover_position_setpoint.yaw = _vehicle_yaw; + _rover_position_setpoint_pub.publish(rover_position_setpoint); + +} + +void MecanumPosControl::generateVelocitySetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) { + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { + autoPositionMode(); + + } else if (_rover_position_setpoint_sub.copy(&_rover_position_setpoint) + && PX4_ISFINITE(_rover_position_setpoint.position_ned[0]) && PX4_ISFINITE(_rover_position_setpoint.position_ned[1])) { + goToPositionMode(); + } + +} + +void MecanumPosControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + Vector3f velocity_setpoint_body{}; + velocity_setpoint_body(0) = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + velocity_setpoint_body(1) = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_delta = math::interpolate(math::deadzone(manual_control_setpoint.yaw, + _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 || velocity_setpoint_body.norm() < FLT_EPSILON) { // Closed loop yaw rate control + _pos_ctl_yaw_setpoint = NAN; + const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta); + const Vector3f velocity_setpoint_local = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body); + mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; + mecanum_velocity_setpoint.timestamp = _timestamp; + mecanum_velocity_setpoint.speed = velocity_setpoint_body.norm(); + mecanum_velocity_setpoint.bearing = atan2f(velocity_setpoint_local(1), velocity_setpoint_local(0)); + mecanum_velocity_setpoint.yaw = yaw_setpoint; + _mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + const Vector3f velocity = Vector3f(velocity_setpoint_body(0), velocity_setpoint_body(1), 0.f); + const float velocity_magnitude_setpoint = velocity.norm(); + const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); + const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0), + pos_ctl_course_direction_local(1)); + + // Reset course control if course direction change is above threshold + if (fabsf(acosf(pos_ctl_course_direction_temp(0) * _pos_ctl_course_direction(0) + pos_ctl_course_direction_temp( + 1) * _pos_ctl_course_direction(1))) > _param_rm_course_ctl_th.get()) { + _pos_ctl_yaw_setpoint = NAN; + } + + if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) { + _pos_ctl_start_position_ned = _curr_pos_ned; + _pos_ctl_yaw_setpoint = _vehicle_yaw; + _pos_ctl_course_direction = pos_ctl_course_direction_temp; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned; + const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; + pure_pursuit_status_s pure_pursuit_status{}; + pure_pursuit_status.timestamp = _timestamp; + const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, velocity_magnitude_setpoint); + _pure_pursuit_status_pub.publish(pure_pursuit_status); + mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; + mecanum_velocity_setpoint.timestamp = _timestamp; + mecanum_velocity_setpoint.speed = velocity_magnitude_setpoint; + mecanum_velocity_setpoint.bearing = bearing_setpoint; + mecanum_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint; + _mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); + } +} + +void MecanumPosControl::autoPositionMode() +{ + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + _curr_wp_type = position_setpoint_triplet.current.type; + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _global_ned_proj_ref); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Waypoint cruising speed + _auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + + // Waypoint yaw setpoint + if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { + _auto_yaw = position_setpoint_triplet.current.yaw; + + } else { + _auto_yaw = _vehicle_yaw; + } + } + + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + // Check stopping conditions + bool auto_stop{false}; + + if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND + || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE + || !_next_wp_ned.isAllFinite()) { // Check stopping conditions + auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + if (auto_stop) { + mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; + mecanum_velocity_setpoint.timestamp = _timestamp; + mecanum_velocity_setpoint.speed = 0.f; + mecanum_velocity_setpoint.bearing = 0.f; + mecanum_velocity_setpoint.yaw = _vehicle_yaw; + _mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); + + } else { // Regular guidance algorithm + const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), + _curr_wp_type); + pure_pursuit_status_s pure_pursuit_status{}; + pure_pursuit_status.timestamp = _timestamp; + const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + velocity_magnitude); + _pure_pursuit_status_pub.publish(pure_pursuit_status); + mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; + mecanum_velocity_setpoint.timestamp = _timestamp; + mecanum_velocity_setpoint.speed = velocity_magnitude; + mecanum_velocity_setpoint.bearing = bearing_setpoint; + mecanum_velocity_setpoint.yaw = _auto_yaw; + _mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); + } +} + + +float MecanumPosControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp, + const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, + const float miss_spd_gain, const int curr_wp_type) +{ + // Upcoming stop + if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle) + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); + } + + // Straight line speed + if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, + M_PI_F, 0.f, 1.f), 0.f, 1.f); + const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, + distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + + return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); + } + + return auto_speed; // Fallthrough +} + +void MecanumPosControl::goToPositionMode() +{ + const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (distance_to_target > _param_nav_acc_rad.get()) { + float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ? + _rover_position_setpoint.cruising_speed : + _param_ro_speed_limit.get(); + speed_setpoint = math::min(speed_setpoint, max_speed); + pure_pursuit_status_s pure_pursuit_status{}; + pure_pursuit_status.timestamp = _timestamp; + const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(speed_setpoint)); + _pure_pursuit_status_pub.publish(pure_pursuit_status); + mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; + mecanum_velocity_setpoint.timestamp = _timestamp; + mecanum_velocity_setpoint.speed = speed_setpoint; + mecanum_velocity_setpoint.bearing = bearing_setpoint; + mecanum_velocity_setpoint.yaw = _pos_ctl_yaw_setpoint; + _mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); + + } else { + mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; + mecanum_velocity_setpoint.timestamp = _timestamp; + mecanum_velocity_setpoint.speed = 0.f; + mecanum_velocity_setpoint.bearing = 0.f; + mecanum_velocity_setpoint.yaw = _vehicle_yaw; + _mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); + } +} + +bool MecanumPosControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp similarity index 78% rename from src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp rename to src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index c4339798dc46..3d98b39e987d 100644 --- a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -49,11 +49,8 @@ // uORB includes #include #include -#include -#include -#include -#include -#include +#include +#include #include #include #include @@ -67,17 +64,17 @@ using namespace matrix; /** - * @brief Class for mecanum position/velocity control. + * @brief Class for mecanum position control. */ -class MecanumPosVelControl : public ModuleParams +class MecanumPosControl : public ModuleParams { public: /** - * @brief Constructor for MecanumPosVelControl. + * @brief Constructor for MecanumPosControl. * @param parent The parent ModuleParams object. */ - MecanumPosVelControl(ModuleParams *parent); - ~MecanumPosVelControl() = default; + MecanumPosControl(ModuleParams *parent); + ~MecanumPosControl() = default; /** * @brief Update position controller. @@ -97,21 +94,20 @@ class MecanumPosVelControl : public ModuleParams void updateSubscriptions(); /** - * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or - * trajcetorySetpoint (Offboard position or velocity control) or - * positionSetpointTriplet (Auto Mode). + * @brief Generate and publish roverPositionSetpoint from position of trajectorySetpoint. */ - void generateAttitudeSetpoint(); + void generatePositionSetpoint(); /** - * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint (Position Mode) or + * positionSetpointTriplet (Auto Mode) or roverPositionSetpoint. */ - void manualPositionMode(); + void generateVelocitySetpoint(); /** - * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. */ - void offboardPositionMode(); + void manualPositionMode(); /** * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. @@ -119,10 +115,15 @@ class MecanumPosVelControl : public ModuleParams void offboardVelocityMode(); /** - * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet. */ void autoPositionMode(); + /** + * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. + */ + void goToPositionMode(); + /** * @brief Calculate the velocity magnitude setpoint. During waypoint transition the speed is restricted to * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). @@ -155,17 +156,16 @@ class MecanumPosVelControl : public ModuleParams uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; vehicle_control_mode_s _vehicle_control_mode{}; offboard_control_mode_s _offboard_control_mode{}; - rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; - uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; + uORB::Publication _mecanum_velocity_setpoint_pub{ORB_ID(mecanum_velocity_setpoint)}; + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; + uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + // Variables hrt_abstime _timestamp{0}; @@ -173,12 +173,8 @@ class MecanumPosVelControl : public ModuleParams Vector2f _curr_pos_ned{}; Vector2f _pos_ctl_course_direction{}; Vector2f _pos_ctl_start_position_ned{}; - float _vehicle_speed_body_x{0.f}; - float _vehicle_speed_body_y{0.f}; float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; - float _speed_body_x_setpoint{0.f}; - float _speed_body_y_setpoint{0.f}; float _pos_ctl_yaw_setpoint{0.f}; // Yaw setpoint for manual position mode, NAN if yaw rate is manually controlled [rad] float _dt{0.f}; float _auto_speed{0.f}; @@ -190,14 +186,9 @@ class MecanumPosVelControl : public ModuleParams Vector2f _curr_wp_ned{}; Vector2f _prev_wp_ned{}; Vector2f _next_wp_ned{}; + float _cruising_speed{0.f}; float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - // Controllers - PID _pid_speed_x; - PID _pid_speed_y; - SlewRate _speed_x_setpoint; - SlewRate _speed_y_setpoint; - // Class Instances MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates @@ -217,6 +208,7 @@ class MecanumPosVelControl : public ModuleParams (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, (ParamFloat) _param_nav_acc_rad ) diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp deleted file mode 100644 index 5139ef8e2071..000000000000 --- a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp +++ /dev/null @@ -1,432 +0,0 @@ -/**************************************************************************** - * - * 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 "MecanumPosVelControl.hpp" - -using namespace time_literals; - -MecanumPosVelControl::MecanumPosVelControl(ModuleParams *parent) : ModuleParams(parent) -{ - _rover_rate_setpoint_pub.advertise(); - _rover_throttle_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_status_pub.advertise(); - _pure_pursuit_status_pub.advertise(); - updateParams(); -} - -void MecanumPosVelControl::updateParams() -{ - ModuleParams::updateParams(); - _pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); - _pid_speed_x.setIntegralLimit(1.f); - _pid_speed_x.setOutputLimit(1.f); - _pid_speed_y.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); - _pid_speed_y.setIntegralLimit(1.f); - _pid_speed_y.setOutputLimit(1.f); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; - - if (_param_ro_accel_limit.get() > FLT_EPSILON) { - _speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get()); - _speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get()); - } -} - -void MecanumPosVelControl::updatePosControl() -{ - const hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - updateSubscriptions(); - - if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) - && _vehicle_control_mode.flag_armed && runSanityChecks()) { - generateAttitudeSetpoint(); - - if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible - if (_rover_steering_setpoint_sub.updated()) { - _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); - } - - float speed_body_x_setpoint_normalized = math::interpolate(_speed_body_x_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); - - float speed_body_y_setpoint_normalized = math::interpolate(_speed_body_y_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); - - const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf( - _rover_steering_setpoint.normalized_speed_diff); - - if (total_speed > 1.f) { - const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized)); - const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta)); - const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized, - 2.f) + powf(speed_body_y_setpoint_normalized, 2.f))); - speed_body_x_setpoint_normalized *= magnitude * normalization; - speed_body_y_setpoint_normalized *= magnitude * normalization; - _speed_body_x_setpoint = math::interpolate(speed_body_x_setpoint_normalized, -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); - _speed_body_y_setpoint = math::interpolate(speed_body_y_setpoint_normalized, -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); - } - } - - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - _speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f; - _speed_body_y_setpoint = fabsf(_speed_body_y_setpoint) > _param_ro_speed_th.get() ? _speed_body_y_setpoint : 0.f; - rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x, - _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), _dt); - rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y, - _speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), _dt); - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - - } else { // Reset controller and slew rate when position control is not active - _pid_speed_x.resetIntegral(); - _speed_x_setpoint.setForcedValue(0.f); - _pid_speed_y.resetIntegral(); - _speed_y_setpoint.setForcedValue(0.f); - } - - // Publish position controller status (logging only) - rover_velocity_status_s rover_velocity_status; - rover_velocity_status.timestamp = _timestamp; - rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; - rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; - rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_x_setpoint.getState(); - rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; - rover_velocity_status.speed_body_y_setpoint = _speed_body_y_setpoint; - rover_velocity_status.adjusted_speed_body_y_setpoint = _speed_y_setpoint.getState(); - rover_velocity_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral(); - rover_velocity_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral(); - _rover_velocity_status_pub.publish(rover_velocity_status); -} - -void MecanumPosVelControl::updateSubscriptions() -{ - if (_vehicle_control_mode_sub.updated()) { - _vehicle_control_mode_sub.copy(&_vehicle_control_mode); - } - - if (_vehicle_attitude_sub.updated()) { - 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()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, - vehicle_local_position.ref_timestamp); - } - - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; - _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; - } - -} - -void MecanumPosVelControl::generateAttitudeSetpoint() -{ - if (_vehicle_control_mode.flag_control_manual_enabled - && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode - manualPositionMode(); - - } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control - if (_offboard_control_mode_sub.updated()) { - _offboard_control_mode_sub.copy(&_offboard_control_mode); - } - - if (_offboard_control_mode.position) { - offboardPositionMode(); - - } else if (_offboard_control_mode.velocity) { - offboardVelocityMode(); - } - - } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode - autoPositionMode(); - } -} - -void MecanumPosVelControl::manualPositionMode() -{ - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); - _speed_body_y_setpoint = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); - const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, - _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - - if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control - _pos_ctl_yaw_setpoint = NAN; - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); - - } else if ((fabsf(_speed_body_x_setpoint) > FLT_EPSILON - || fabsf(_speed_body_y_setpoint) > - FLT_EPSILON)) { // Course control if the steering input is zero (keep driving on a straight line) - const Vector3f velocity = Vector3f(_speed_body_x_setpoint, _speed_body_y_setpoint, 0.f); - const float velocity_magnitude_setpoint = velocity.norm(); - const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); - const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0), - pos_ctl_course_direction_local(1)); - - // Reset course control if course direction change is above threshold - if (fabsf(asinf(pos_ctl_course_direction_temp % _pos_ctl_course_direction)) > _param_rm_course_ctl_th.get()) { - _pos_ctl_yaw_setpoint = NAN; - } - - if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) { - _pos_ctl_start_position_ned = _curr_pos_ned; - _pos_ctl_yaw_setpoint = _vehicle_yaw; - _pos_ctl_course_direction = pos_ctl_course_direction_temp; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned; - const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; - pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = _timestamp; - const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _pos_ctl_start_position_ned, - _curr_pos_ned, velocity_magnitude_setpoint); - _pure_pursuit_status_pub.publish(pure_pursuit_status); - const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); - _speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame); - _speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _pos_ctl_yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - - } else { // Reset course control and yaw rate setpoint - _pos_ctl_yaw_setpoint = NAN; - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = 0.f; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); - } - } -} - -void MecanumPosVelControl::offboardPositionMode() -{ - trajectory_setpoint_s trajectory_setpoint{}; - _trajectory_setpoint_sub.copy(&trajectory_setpoint); - - // Translate trajectory setpoint to rover setpoints - const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); - const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); - - if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { - const float velocity_magnitude_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance( - _param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, 0.f), _param_ro_speed_limit.get()); - pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = _timestamp; - const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _curr_pos_ned, - _curr_pos_ned, velocity_magnitude_setpoint); - _pure_pursuit_status_pub.publish(pure_pursuit_status); - const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); - _speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame); - _speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame); - - } else { - _speed_body_x_setpoint = 0.f; - _speed_body_y_setpoint = 0.f; - } -} - -void MecanumPosVelControl::offboardVelocityMode() -{ - trajectory_setpoint_s trajectory_setpoint{}; - _trajectory_setpoint_sub.copy(&trajectory_setpoint); - - const Vector3f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1], - trajectory_setpoint.velocity[2]); - const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - - if (velocity_in_body_frame.isAllFinite()) { - _speed_body_x_setpoint = velocity_in_body_frame(0); - _speed_body_y_setpoint = velocity_in_body_frame(1); - - } else { - _speed_body_x_setpoint = 0.f; - _speed_body_y_setpoint = 0.f; - } -} - -void MecanumPosVelControl::autoPositionMode() -{ - if (_position_setpoint_triplet_sub.updated()) { - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - _curr_wp_type = position_setpoint_triplet.current.type; - - RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, - _curr_pos_ned, _global_ned_proj_ref); - - _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); - - // Waypoint cruising speed - _auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); - - // Waypoint yaw setpoint - if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { - _auto_yaw = position_setpoint_triplet.current.yaw; - - } else { - _auto_yaw = _vehicle_yaw; - } - } - - const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), - 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); - - // Check stopping conditions - bool auto_stop{false}; - - if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND - || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE - || !_next_wp_ned.isAllFinite()) { // Check stopping conditions - auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - if (auto_stop) { - _speed_body_x_setpoint = 0.f; - _speed_body_y_setpoint = 0.f; - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = 0.f; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); - - } else { // Regular guidance algorithm - const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), - _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), - _curr_wp_type); - pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = _timestamp; - const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), - _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - velocity_magnitude); - _pure_pursuit_status_pub.publish(pure_pursuit_status); - const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); - Vector2f desired_velocity(0.f, 0.f); - _speed_body_x_setpoint = velocity_magnitude * cosf(bearing_setpoint_body_frame); - _speed_body_y_setpoint = velocity_magnitude * sinf(bearing_setpoint_body_frame); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _auto_yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } -} - -float MecanumPosVelControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp, - const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, - const float miss_spd_gain, const int curr_wp_type) -{ - // Upcoming stop - if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle) - || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND - || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { - const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp, 0.f); - return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); - } - - // Straight line speed - if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) { - const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, - distance_to_curr_wp, - max_speed * (1.f - speed_reduction)); - - return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); - } - - return auto_speed; // Fallthrough -} - -bool MecanumPosVelControl::runSanityChecks() -{ - bool ret = true; - - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { - ret = false; - } - - if (_param_ro_speed_limit.get() < FLT_EPSILON) { - ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("mecanum_posVel_control_conf_invalid_speed_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); - } - - } - - if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { - ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("mecanum_posVel_control_conf_invalid_speed_control"), events::Log::Error, - "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPD) nor feedback (RO_SPEED_P) is setup", - _param_ro_max_thr_speed.get(), - _param_ro_speed_p.get()); - } - } - - _prev_param_check_passed = ret; - return ret; -} diff --git a/src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt b/src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt new file mode 100644 index 000000000000..890ec41fa68c --- /dev/null +++ b/src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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(MecanumVelControl + MecanumVelControl.cpp +) + +target_link_libraries(MecanumVelControl PUBLIC PID) +target_link_libraries(MecanumVelControl PUBLIC pure_pursuit) +target_include_directories(MecanumVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp new file mode 100644 index 000000000000..894a5cfe12e5 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * 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 "MecanumVelControl.hpp" + +using namespace time_literals; + +MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _mecanum_velocity_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void MecanumVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed_x.setIntegralLimit(1.f); + _pid_speed_x.setOutputLimit(1.f); + _pid_speed_y.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed_y.setIntegralLimit(1.f); + _pid_speed_y.setOutputLimit(1.f); + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get()); + _speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } +} + +void MecanumVelControl::updateVelControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_velocity_enabled) && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Velocity Control + generateVelocitySetpoint(); + } + + generateAttitudeAndThrottleSetpoint(); + + } else { // Reset controller and slew rate when velocity control is not active + _pid_speed_x.resetIntegral(); + _speed_x_setpoint.setForcedValue(0.f); + _pid_speed_y.resetIntegral(); + _speed_y_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_x_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.adjusted_speed_body_y_setpoint = _speed_y_setpoint.getState(); + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral(); + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void MecanumVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + 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()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + +} +void MecanumVelControl::generateVelocitySetpoint() +{ + 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_vel_control = _offboard_control_mode.velocity && !_offboard_control_mode.position; + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) { + mecanum_velocity_setpoint_s mecanum_velocity_setpoint{}; + mecanum_velocity_setpoint.timestamp = _timestamp; + mecanum_velocity_setpoint.speed = velocity_in_local_frame.norm(); + mecanum_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); + mecanum_velocity_setpoint.yaw = _vehicle_yaw; + _mecanum_velocity_setpoint_pub.publish(mecanum_velocity_setpoint); + + } +} + +void MecanumVelControl::generateAttitudeAndThrottleSetpoint() +{ + if (_mecanum_velocity_setpoint_sub.updated()) { + _mecanum_velocity_setpoint_sub.copy(&_mecanum_velocity_setpoint); + } + + // Attitude Setpoint + if (PX4_ISFINITE(_mecanum_velocity_setpoint.yaw)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _mecanum_velocity_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + _last_attitude_setpoint_update = _timestamp; + + } else { + 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); + _last_attitude_setpoint_update = _timestamp; + } + + // Throttle Setpoint + float speed_body_x_setpoint{0.f}; + float speed_body_y_setpoint{0.f}; + + if (fabsf(_mecanum_velocity_setpoint.speed) > FLT_EPSILON) { + const Vector3f velocity_in_local_frame(_mecanum_velocity_setpoint.speed * cosf( + _mecanum_velocity_setpoint.bearing), + _mecanum_velocity_setpoint.speed * sinf(_mecanum_velocity_setpoint.bearing), 0.f); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + speed_body_x_setpoint = velocity_in_body_frame(0); + speed_body_y_setpoint = velocity_in_body_frame(1); + + } + + if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + float speed_body_x_setpoint_normalized = math::interpolate(speed_body_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + float speed_body_y_setpoint_normalized = math::interpolate(speed_body_y_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf( + _rover_steering_setpoint.normalized_speed_diff); + + if (total_speed > 1.f) { + const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized)); + const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized, + 2.f) + powf(speed_body_y_setpoint_normalized, 2.f))); + speed_body_x_setpoint_normalized *= magnitude * normalization; + speed_body_y_setpoint_normalized *= magnitude * normalization; + speed_body_x_setpoint = math::interpolate(speed_body_x_setpoint_normalized, -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + speed_body_y_setpoint = math::interpolate(speed_body_y_setpoint_normalized, -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + } + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + speed_body_x_setpoint = fabsf(speed_body_x_setpoint) > _param_ro_speed_th.get() ? speed_body_x_setpoint : 0.f; + speed_body_y_setpoint = fabsf(speed_body_y_setpoint) > _param_ro_speed_th.get() ? speed_body_y_setpoint : 0.f; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x, + speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y, + speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + +} + +bool MecanumVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_vel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_vel_control_conf_invalid_speed_control"), events::Log::Error, + "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", + _param_ro_max_thr_speed.get(), + _param_ro_speed_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp new file mode 100644 index 000000000000..a124af06a0d3 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * 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 +#include + +// Libraries +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for mecanum velocity control. + */ +class MecanumVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumVelControl. + * @param parent The parent ModuleParams object. + */ + MecanumVelControl(ModuleParams *parent); + ~MecanumVelControl() = default; + + /** + * @brief Update velocity controller. + */ + void updateVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in velocity controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint. + */ + void generateVelocitySetpoint(); + + /** + * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint + * from roverVelocitySetpoint. + */ + void generateAttitudeAndThrottleSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + 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 _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mecanum_velocity_setpoint_sub{ORB_ID(mecanum_velocity_setpoint)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + + // uORB publications + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _mecanum_velocity_setpoint_pub{ORB_ID(mecanum_velocity_setpoint)}; + mecanum_velocity_setpoint_s _mecanum_velocity_setpoint{}; + + // Variables + hrt_abstime _timestamp{0}; + hrt_abstime _last_attitude_setpoint_update{0}; + Quatf _vehicle_attitude_quaternion{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + bool _prev_param_check_passed{false}; + + // Controllers + PID _pid_speed_x; + PID _pid_speed_y; + SlewRate _speed_x_setpoint; + SlewRate _speed_y_setpoint; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th + + ) +}; diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index f881fcdc5d7d..724246770dae 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -70,7 +70,8 @@ void RoverMecanum::Run() _timestamp = hrt_absolute_time(); _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - _mecanum_pos_vel_control.updatePosControl(); + _mecanum_pos_control.updatePosControl(); + _mecanum_vel_control.updateVelControl(); _mecanum_att_control.updateAttControl(); _mecanum_rate_control.updateRateControl(); @@ -83,7 +84,7 @@ void RoverMecanum::Run() && !_vehicle_control_mode.flag_control_rates_enabled; if (full_manual_mode_enabled) { // Manual mode - generateSteeringSetpoint(); + generateSteeringAndThrottleSetpoint(); } if (_vehicle_control_mode.flag_armed) { @@ -93,7 +94,7 @@ void RoverMecanum::Run() } -void RoverMecanum::generateSteeringSetpoint() +void RoverMecanum::generateSteeringAndThrottleSetpoint() { manual_control_setpoint_s manual_control_setpoint{}; diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 1812e1c7c4eb..95ffe311bb84 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -58,7 +58,8 @@ // Local includes #include "MecanumRateControl/MecanumRateControl.hpp" #include "MecanumAttControl/MecanumAttControl.hpp" -#include "MecanumPosVelControl/MecanumPosVelControl.hpp" +#include "MecanumVelControl/MecanumVelControl.hpp" +#include "MecanumPosControl/MecanumPosControl.hpp" class RoverMecanum : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -91,9 +92,9 @@ class RoverMecanum : public ModuleBase, public ModuleParams, void Run() override; /** - * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). + * @brief Generate and publish roverSteeringSetpoint and roverThrottleSetpoint from manualControlSetpoint (Manual Mode). */ - void generateSteeringSetpoint(); + void generateSteeringAndThrottleSetpoint(); /** * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. @@ -126,9 +127,10 @@ class RoverMecanum : public ModuleBase, public ModuleParams, uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; // Class instances - MecanumRateControl _mecanum_rate_control{this}; - MecanumAttControl _mecanum_att_control{this}; - MecanumPosVelControl _mecanum_pos_vel_control{this}; + MecanumRateControl _mecanum_rate_control{this}; + MecanumAttControl _mecanum_att_control{this}; + MecanumVelControl _mecanum_vel_control{this}; + MecanumPosControl _mecanum_pos_control{this}; // Variables hrt_abstime _timestamp{0}; From d9d6848204ec4132a4464ed56a7533cb37bd04f7 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 29 Apr 2025 14:38:44 +0200 Subject: [PATCH 2/2] mecanum: streamline flow of information --- .../MecanumAttControl/MecanumAttControl.cpp | 35 +++++++------------ .../MecanumAttControl/MecanumAttControl.hpp | 4 +-- .../MecanumRateControl/MecanumRateControl.cpp | 4 +-- .../MecanumRateControl/MecanumRateControl.hpp | 5 ++- 4 files changed, 19 insertions(+), 29 deletions(-) diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp index 1c4a93d4d601..edddef8ce151 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -78,7 +78,7 @@ void MecanumAttControl::updateAttControl() 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) { - generateAttitudeSetpoint(); + generateAttitudeAndThrottleSetpoint(); } generateRateSetpoint(); @@ -97,7 +97,7 @@ void MecanumAttControl::updateAttControl() } -void MecanumAttControl::generateAttitudeSetpoint() +void MecanumAttControl::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; @@ -113,20 +113,19 @@ void MecanumAttControl::generateAttitudeSetpoint() rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, - _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + const float yaw_delta = math::interpolate(math::deadzone(manual_control_setpoint.yaw, + _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_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control + if (fabsf(yaw_delta) > FLT_EPSILON) { // Closed loop yaw rate control _stab_yaw_setpoint = NAN; - _adjusted_yaw_setpoint.setForcedValue(0.f); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); - - } else if (fabsf(rover_throttle_setpoint.throttle_body_x) > FLT_EPSILON - || fabsf(rover_throttle_setpoint.throttle_body_y) > - FLT_EPSILON) { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + 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 (!PX4_ISFINITE(_stab_yaw_setpoint)) { _stab_yaw_setpoint = _vehicle_yaw; } @@ -136,16 +135,8 @@ void MecanumAttControl::generateAttitudeSetpoint() rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } else { // Reset yaw control and yaw rate setpoint - _stab_yaw_setpoint = NAN; - _adjusted_yaw_setpoint.setForcedValue(0.f); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = 0.f; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); } - } } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp index b382e789a5b0..9f037f8fc952 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -84,10 +84,10 @@ class MecanumAttControl : public ModuleParams private: /** - * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode) + * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode) * or trajectorySetpoint (Offboard attitude control). */ - void generateAttitudeSetpoint(); + void generateAttitudeAndThrottleSetpoint(); /** * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp index 49f1ae5a8c67..ee25b1595019 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp @@ -75,7 +75,7 @@ void MecanumRateControl::updateRateControl() if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { - generateRateSetpoint(); + generateRateAndThrottleSetpoint(); } generateSteeringSetpoint(); @@ -95,7 +95,7 @@ void MecanumRateControl::updateRateControl() } -void MecanumRateControl::generateRateSetpoint() +void MecanumRateControl::generateRateAndThrottleSetpoint() { const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp index 3e61c4cb1bc5..912a97ff63be 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp @@ -84,10 +84,9 @@ class MecanumRateControl : public ModuleParams private: /** - * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode) - * or trajectorySetpoint (Offboard rate control). + * @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode). */ - void generateRateSetpoint(); + void generateRateAndThrottleSetpoint(); /** * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.