From e457a5baedc10e707fe25fd655df2b94f1c5677d Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 12 Dec 2023 14:57:39 +0100 Subject: [PATCH] Differential Drive Guidance: Add guidance also add dependency on control allocation parameter CA_R_REV Differential Drive Guidance: Added mission logic Differential Drive Guidance Differential Drive Guidance Differential Guidance: Inlcude library Differential Guidance: Compiles, does not work though Differential Guidance: Works somewhat Differential Guidance: Temp Differential Guidance: Tuning Differeital Drive Guidance: Remove waypoint mover Differential Guidance: Fixed accuracy issue by converting from float to double Differential Guidance: rebased on differentialdrive and improved waypoint accuracy Temp Differential Guidance: cleanup temp --- msg/DifferentialDriveSetpoint.msg | 2 + .../differential_drive_control/CMakeLists.txt | 4 + .../DifferentialDriveControl.cpp | 88 +++++++++-- .../DifferentialDriveControl.hpp | 49 +++++- .../DifferentialDriveGuidance/CMakeLists.txt | 42 +++++ .../DifferentialDriveGuidance.cpp | 102 ++++++++++++ .../DifferentialDriveGuidance.hpp | 147 ++++++++++++++++++ .../differential_drive_control/module.yaml | 70 ++++++++- src/modules/logger/logged_topics.cpp | 3 +- src/modules/uxrce_dds_client/dds_topics.yaml | 5 +- 10 files changed, 495 insertions(+), 17 deletions(-) create mode 100644 src/modules/differential_drive_control/DifferentialDriveGuidance/CMakeLists.txt create mode 100644 src/modules/differential_drive_control/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp create mode 100644 src/modules/differential_drive_control/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp diff --git a/msg/DifferentialDriveSetpoint.msg b/msg/DifferentialDriveSetpoint.msg index 386dc3a9d9..d21c8151a0 100644 --- a/msg/DifferentialDriveSetpoint.msg +++ b/msg/DifferentialDriveSetpoint.msg @@ -2,3 +2,5 @@ uint64 timestamp # time since system start (microseconds) float32 speed # [m/s] collective roll-off speed in body x-axis float32 yaw_rate # [rad/s] yaw rate + +# TOPICS feed_forward_differential_drive_setpoint closed_loop_differential_drive_setpoint diff --git a/src/modules/differential_drive_control/CMakeLists.txt b/src/modules/differential_drive_control/CMakeLists.txt index 3da82522fc..877e470d5f 100644 --- a/src/modules/differential_drive_control/CMakeLists.txt +++ b/src/modules/differential_drive_control/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(DifferentialDriveKinematics) +add_subdirectory(DifferentialDriveGuidance) px4_add_module( MODULE modules__differential_drive_control @@ -41,7 +42,10 @@ px4_add_module( DifferentialDriveControl.hpp DEPENDS DifferentialDriveKinematics + DifferentialDriveGuidance + pid px4_work_queue + modules__control_allocator # for parameter CA_R_REV MODULE_CONFIG module.yaml ) diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.cpp b/src/modules/differential_drive_control/DifferentialDriveControl.cpp index 41e5093499..d662882326 100644 --- a/src/modules/differential_drive_control/DifferentialDriveControl.cpp +++ b/src/modules/differential_drive_control/DifferentialDriveControl.cpp @@ -43,6 +43,9 @@ DifferentialDriveControl::DifferentialDriveControl() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { updateParams(); + + pid_init(&_angular_velocity_pid, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_speed_pid, PID_MODE_DERIVATIV_NONE, 0.001f); } bool DifferentialDriveControl::init() @@ -54,12 +57,29 @@ bool DifferentialDriveControl::init() void DifferentialDriveControl::updateParams() { ModuleParams::updateParams(); + + pid_set_parameters(&_angular_velocity_pid, + _param_rdd_p_gain_angular_velocity.get(), // Proportional gain + _param_rdd_i_gain_angular_velocity.get(), // Integral gain + 0, // Derivative gain + 20, // Integral limit + 200); // Output limit + + pid_set_parameters(&_speed_pid, + _param_rdd_p_gain_speed.get(), // Proportional gain + _param_rdd_i_gain_speed.get(), // Integral gain + 0, // Derivative gain + 2, // Integral limit + 200); // Output limit + _max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get(); _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); _differential_drive_kinematics.setMaxSpeed(_max_speed); + _differential_guidance_controller.setMaxSpeed(_max_speed); _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); + _differential_guidance_controller.setMaxAngularVelocity(_max_angular_velocity); } void DifferentialDriveControl::Run() @@ -70,23 +90,45 @@ void DifferentialDriveControl::Run() } hrt_abstime now = hrt_absolute_time(); + const double dt = static_cast(math::min((now - _time_stamp_last), kTimeoutUs)) / 1e6; + _time_stamp_last = now; if (_parameter_update_sub.updated()) { - parameter_update_s pupdate; + parameter_update_s pupdate{}; _parameter_update_sub.copy(&pupdate); updateParams(); } if (_vehicle_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode; + vehicle_control_mode_s vehicle_control_mode{}; if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { _armed = vehicle_control_mode.flag_armed; - _manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported + _manual_driving = vehicle_control_mode.flag_control_manual_enabled; + _mission_driving = vehicle_control_mode.flag_control_auto_enabled; } } + if (_vehicle_attitude_sub.updated()) { + _vehicle_attitude_sub.copy(&_vehicle_attitude); + + _vehicle_yaw = matrix::Eulerf(matrix::Quatf(_vehicle_attitude.q)).psi(); + } + + if (_vehicle_angular_velocity_sub.updated()) { + _vehicle_angular_velocity_sub.copy(&_vehicle_angular_velocity); + } + + if (_vehicle_local_position_sub.updated()) { + _vehicle_local_position_sub.copy(&_vehicle_local_position); + + matrix::Vector3f ground_speed(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); + + // rotate the velocity vector from the local frame to the body frame + _velocity_in_body_frame = Quatf(_vehicle_attitude.q).rotateVectorInverse(ground_speed); + } + if (_manual_driving) { // Manual mode // directly produce setpoints from the manual control setpoint (joystick) @@ -98,18 +140,46 @@ void DifferentialDriveControl::Run() _differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed; _differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity; - _differential_drive_setpoint_pub.publish(_differential_drive_setpoint); + _feed_forward_differential_drive_setpoint_pub.publish(_differential_drive_setpoint); } } + + } else if (_mission_driving) { + // Mission mode + // directly receive setpoints from the guidance library + matrix::Vector2f guidance_output = + _differential_guidance_controller.computeGuidance( + _vehicle_yaw, + _vehicle_angular_velocity.xyz[2], + dt + ); + + _differential_drive_setpoint.timestamp = now; + _differential_drive_setpoint.speed = guidance_output(0); + _differential_drive_setpoint.yaw_rate = guidance_output(1); + _closed_loop_differential_drive_setpoint_pub.publish(_differential_drive_setpoint); } - _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); + // no need for any bools, just check if the topic is updated and update the setpoint + if (_feed_forward_differential_drive_setpoint_sub.updated()) { + _feed_forward_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint); - // publish data to actuator_motors (output module) - // get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics) + _speed_pid_output = 0; + _angular_velocity_pid_output = 0; + } + + if (_closed_loop_differential_drive_setpoint_sub.updated()) { + _closed_loop_differential_drive_setpoint_sub.copy(&_differential_drive_setpoint); + + _speed_pid_output = pid_calculate(&_speed_pid, _differential_drive_setpoint.speed, _velocity_in_body_frame(0), 0, dt); + _angular_velocity_pid_output = pid_calculate(&_angular_velocity_pid, _differential_drive_setpoint.yaw_rate, + _vehicle_angular_velocity.xyz[2], 0, dt); + } + + // get the normalized wheel speeds from the inverse kinematics class (DifferentialDriveKinematics) Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics( - _differential_drive_setpoint.speed, - _differential_drive_setpoint.yaw_rate); + _differential_drive_setpoint.speed + _speed_pid_output, + _differential_drive_setpoint.yaw_rate + _angular_velocity_pid_output); // Check if max_angular_wheel_speed is zero const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now; diff --git a/src/modules/differential_drive_control/DifferentialDriveControl.hpp b/src/modules/differential_drive_control/DifferentialDriveControl.hpp index 47133c8d27..2ae0b5458b 100644 --- a/src/modules/differential_drive_control/DifferentialDriveControl.hpp +++ b/src/modules/differential_drive_control/DifferentialDriveControl.hpp @@ -48,6 +48,13 @@ #include #include #include +#include +#include + +#include +#include +#include + #include #include #include @@ -55,9 +62,15 @@ // Standard library includes #include +#include // Local includes #include +#include + +using namespace time_literals; + +static constexpr uint64_t kTimeoutUs = 5000_ms; // Maximal time in microseconds before a loop or data times out namespace differential_drive_control { @@ -86,24 +99,50 @@ protected: private: void Run() override; - uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; + uORB::Subscription _feed_forward_differential_drive_setpoint_sub{ORB_ID(feed_forward_differential_drive_setpoint)}; + uORB::Subscription _closed_loop_differential_drive_setpoint_sub{ORB_ID(closed_loop_differential_drive_setpoint)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; + uORB::Publication _feed_forward_differential_drive_setpoint_pub{ORB_ID(feed_forward_differential_drive_setpoint)}; + uORB::Publication _closed_loop_differential_drive_setpoint_pub{ORB_ID(closed_loop_differential_drive_setpoint)}; differential_drive_setpoint_s _differential_drive_setpoint{}; - DifferentialDriveKinematics _differential_drive_kinematics{}; - + vehicle_attitude_s _vehicle_attitude{}; + vehicle_angular_velocity_s _vehicle_angular_velocity{}; + vehicle_local_position_s _vehicle_local_position{}; bool _armed = false; bool _manual_driving = false; + bool _mission_driving = false; + + hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + + DifferentialDriveKinematics _differential_drive_kinematics; + DifferentialDriveGuidance _differential_guidance_controller{this}; + float _max_speed{0.f}; float _max_angular_velocity{0.f}; + float _vehicle_yaw{0.f}; + Vector3f _velocity_in_body_frame{0.f, 0.f, 0.f}; + + PID_t _angular_velocity_pid; ///< The PID controller for yaw rate. + PID_t _speed_pid; ///< The PID controller for velocity. + + float _speed_pid_output{0.f}; + float _angular_velocity_pid_output{0.f}; + DEFINE_PARAMETERS( + (ParamFloat) _param_rdd_p_gain_speed, + (ParamFloat) _param_rdd_i_gain_speed, + (ParamFloat) _param_rdd_p_gain_angular_velocity, + (ParamFloat) _param_rdd_i_gain_angular_velocity, (ParamFloat) _param_rdd_speed_scale, (ParamFloat) _param_rdd_ang_velocity_scale, (ParamFloat) _param_rdd_max_wheel_speed, diff --git a/src/modules/differential_drive_control/DifferentialDriveGuidance/CMakeLists.txt b/src/modules/differential_drive_control/DifferentialDriveGuidance/CMakeLists.txt new file mode 100644 index 0000000000..39be618c00 --- /dev/null +++ b/src/modules/differential_drive_control/DifferentialDriveGuidance/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2023 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(DifferentialDriveGuidance + DifferentialDriveGuidance.cpp + DifferentialDriveGuidance.hpp +) + +target_compile_options(DifferentialDriveGuidance PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# px4_add_unit_gtest(SRC DifferentialDriveGuidanceTest.cpp LINKLIBS DifferentialDriveGuidance) diff --git a/src/modules/differential_drive_control/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive_control/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp new file mode 100644 index 0000000000..6352548d91 --- /dev/null +++ b/src/modules/differential_drive_control/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp @@ -0,0 +1,102 @@ +#include "DifferentialDriveGuidance.hpp" + +#include + +using namespace matrix; + +DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + + pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +matrix::Vector2f DifferentialDriveGuidance::computeGuidance(float vehicle_yaw, float angular_velocity, float dt) +{ + if (_position_setpoint_triplet_sub.updated()) { + _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); + } + + if (_vehicle_global_position_sub.updated()) { + _vehicle_global_position_sub.copy(&_vehicle_global_position); + } + + matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); + matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); + matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); + + const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), + current_waypoint(0), + current_waypoint(1)); + + float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), + current_waypoint(1)); + float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); + + const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), + _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); + + _forwards_velocity_smoothing.updateDurations(max_velocity); + _forwards_velocity_smoothing.updateTraj(dt); + + if (_next_waypoint != next_waypoint) { + if (fabsf(heading_error) < 0.1f) { + _currentState = GuidanceState::DRIVING; + + } else { + _currentState = GuidanceState::TURNING; + } + + } + + // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. + if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { + _currentState = GuidanceState::GOAL_REACHED; + } + + matrix::Vector2f output; + float desired_speed = 0.f; + + switch (_currentState) { + case GuidanceState::TURNING: + desired_speed = 0.f; + break; + + case GuidanceState::DRIVING: + desired_speed = math::interpolate(abs(heading_error), 0.1f, 0.4f, + _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); + break; + + case GuidanceState::GOAL_REACHED: + // temporary till I find a better way to stop the vehicle + desired_speed = 0.f; + heading_error = 0.f; + angular_velocity = 0.f; + _desired_angular_velocity = 0.f; + break; + } + + // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. + float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, dt); + + output(0) = math::constrain(desired_speed, -_max_speed, _max_speed); + output(1) = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity); + + return output; +} + +void DifferentialDriveGuidance::updateParams() +{ + ModuleParams::updateParams(); + + pid_set_parameters(&_heading_p_controller, + _param_rdd_p_gain_heading.get(), // Proportional gain + 0, // Integral gain + 0, // Derivative gain + 0, // Integral limit + 200); // Output limit + + _forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get()); + _forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get()); + _forwards_velocity_smoothing.setMaxVel(_max_speed); +} diff --git a/src/modules/differential_drive_control/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive_control/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp new file mode 100644 index 0000000000..07a32ca485 --- /dev/null +++ b/src/modules/differential_drive_control/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 + +#include +#include + +#include +#include +#include +#include +#include +// #include + +#include +#include + +#include +#include +#include +#include + +#include + + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + TURNING, ///< The vehicle is currently turning. + DRIVING, ///< The vehicle is currently driving straight. + GOAL_REACHED ///< The vehicle has reached its goal. +}; + +/** + * @brief Class for differential drive guidance. + */ +class DifferentialDriveGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialDriveGuidance. + * @param parent The parent ModuleParams object. + */ + DifferentialDriveGuidance(ModuleParams *parent); + + /** + * @brief Default destructor. + */ + ~DifferentialDriveGuidance() = default; + + /** + * @brief Compute guidance for the vehicle. + * @param global_pos The global position of the vehicle in degrees. + * @param current_waypoint The current waypoint the vehicle is heading towards in degrees. + * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees. + * @param vehicle_yaw The yaw orientation of the vehicle in radians. + * @param body_velocity The velocity of the vehicle in m/s. + * @param angular_velocity The angular velocity of the vehicle in rad/s. + * @param dt The time step in seconds. + * @return A 2D vector containing the computed guidance (speed in m/s, yaw rate in rad/s). + */ + matrix::Vector2f computeGuidance(float vehicle_yaw, + float angular_velocity, float dt); + + /** + * @brief Set the maximum speed for the vehicle. + * @param max_speed The maximum speed in m/s. + * @return The set maximum speed in m/s. + */ + float setMaxSpeed(float max_speed) { return _max_speed = max_speed; } + + + /** + * @brief Set the maximum angular velocity for the vehicle. + * @param max_angular_velocity The maximum angular velocity in rad/s. + * @return The set maximum angular velocity in rad/s. + */ + float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + + position_setpoint_triplet_s _position_setpoint_triplet{}; + vehicle_global_position_s _vehicle_global_position{}; + + GuidanceState _currentState; ///< The current state of guidance. + + float _desired_angular_velocity; ///< The desired angular velocity. + + float _max_speed; ///< The maximum speed. + float _max_angular_velocity; ///< The maximum angular velocity. + + matrix::Vector2d _next_waypoint; ///< The next waypoint. + + VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. + PositionSmoothing _position_smoothing; ///< The position smoothing. + + PID_t _heading_p_controller; ///< The PID controller for yaw rate. + + DEFINE_PARAMETERS( + (ParamFloat) _param_rdd_p_gain_heading, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rdd_max_jerk, + (ParamFloat) _param_rdd_max_accel + ) + +}; diff --git a/src/modules/differential_drive_control/module.yaml b/src/modules/differential_drive_control/module.yaml index 77bd61dda4..6d8ab3ff29 100644 --- a/src/modules/differential_drive_control/module.yaml +++ b/src/modules/differential_drive_control/module.yaml @@ -52,4 +52,72 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 10 + default: 0.3 + RDD_P_HEADING: + description: + short: Proportional gain for heading controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1.0 + RDD_P_SPEED: + description: + short: Proportional gain for speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1.0 + RDD_I_SPEED: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.0 + RDD_P_ANG_VEL: + description: + short: Proportional gain for angular velocity controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1.0 + RDD_I_ANG_VEL: + description: + short: Integral gain for angular velocity controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.0 + RDD_MAX_JERK: + description: + short: Maximum jerk + long: Limit for forwards acc/deceleration change. + type: float + unit: m/s^3 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 + RDD_MAX_ACCEL: + description: + short: Maximum acceleration + long: Maximum acceleration is used to limit the acceleration of the rover + type: float + unit: m/s^2 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5 + diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index c138da8069..5068299b33 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -57,7 +57,8 @@ void LoggedTopics::add_default_topics() add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); - add_optional_topic("differential_drive_setpoint", 100); + add_optional_topic("feed_forward_differential_drive_setpoint", 100); + add_optional_topic("closed_loop_differential_drive_setpoint", 100); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 255c8e0b54..bce6677d06 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -127,7 +127,10 @@ subscriptions: - topic: /fmu/in/vehicle_rates_setpoint type: px4_msgs::msg::VehicleRatesSetpoint - - topic: /fmu/in/differential_drive_setpoint + - topic: /fmu/in/feed_forward_differential_drive_setpoint + type: px4_msgs::msg::DifferentialDriveSetpoint + + - topic: /fmu/in/closed_loop_differential_drive_setpoint type: px4_msgs::msg::DifferentialDriveSetpoint - topic: /fmu/in/vehicle_visual_odometry