diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 7697e23db769..26429add58e5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -14,15 +14,17 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge # Rover parameters param set-default RD_WHEEL_TRACK 0.3 param set-default RD_MAN_YAW_SCALE 0.1 -param set-default RD_YAW_RATE_I 0.1 -param set-default RD_YAW_RATE_P 5 param set-default RD_MAX_ACCEL 6 param set-default RD_MAX_JERK 30 param set-default RD_MAX_SPEED 7 -param set-default RD_HEADING_P 5 -param set-default RD_HEADING_I 0.1 +param set-default RD_YAW_RATE_P 5 +param set-default RD_YAW_RATE_I 0 +param set-default RD_YAW_P 5 +param set-default RD_YAW_I 0 +param set-default RD_SPEED_P 1 +param set-default RD_SPEED_I 0 param set-default RD_MAX_YAW_RATE 180 -param set-default RD_MISS_SPD_DEF 7 +param set-default RD_MISS_SPD_DEF 5 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index c0d91e222365..8a05e2a15e52 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -27,8 +27,8 @@ param set-default RD_YAW_RATE_P 5 param set-default RD_MAX_ACCEL 1 param set-default RD_MAX_JERK 3 param set-default RD_MAX_SPEED 8 -param set-default RD_HEADING_P 5 -param set-default RD_HEADING_I 0.1 +param set-default RD_YAW_P 5 +param set-default RD_YAW_I 0.1 param set-default RD_MAX_YAW_RATE 30 param set-default RD_MISS_SPD_DEF 8 param set-default RD_TRANS_DRV_TRN 0.349066 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index c06b158cf857..1ea26ad6bff0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -20,3 +20,25 @@ param set-default RBCLW_ADDRESS 128 param set-default RBCLW_FUNC1 101 param set-default RBCLW_FUNC2 102 param set-default RBCLW_REV 1 # reverse right wheels + +# Rover parameters +param set-default RD_WHEEL_TRACK 0.3 +param set-default RD_MAN_YAW_SCALE 1 +param set-default RD_MAX_ACCEL 5 +param set-default RD_MAX_JERK 50 +param set-default RD_MAX_SPEED 2 +param set-default RD_YAW_RATE_P 0.1 +param set-default RD_YAW_RATE_I 0 +param set-default RD_YAW_P 5 +param set-default RD_YAW_I 0 +param set-default RD_SPEED_P 0.5 +param set-default RD_SPEED_I 0.1 +param set-default RD_MAX_YAW_RATE 300 +param set-default RD_MISS_SPD_DEF 1.8 +param set-default RD_TRANS_DRV_TRN 0.349066 +param set-default RD_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 +param set-default PP_LOOKAHD_GAIN 1 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3b31b7de554d..dce7e851dca9 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -182,6 +182,7 @@ set(msg_files RoverAckermannGuidanceStatus.msg RoverAckermannStatus.msg RoverDifferentialGuidanceStatus.msg + RoverDifferentialSetpoint.msg RoverDifferentialStatus.msg Rpm.msg RtlStatus.msg diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg index 836546e7ebb7..ce3d37511163 100644 --- a/msg/RoverDifferentialGuidanceStatus.msg +++ b/msg/RoverDifferentialGuidanceStatus.msg @@ -1,10 +1,7 @@ uint64 timestamp # time since system start (microseconds) -float32 desired_speed # [m/s] Desired forward speed for the rover float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller float32 heading_error_deg # [deg] Heading error of the pure pursuit controller -float32 pid_heading_integral # Integral of the PID for the desired yaw rate during missions -float32 pid_throttle_integral # Integral of the PID for the throttle during missions uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] # TOPICS rover_differential_guidance_status diff --git a/msg/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg new file mode 100644 index 000000000000..b16f712828d8 --- /dev/null +++ b/msg/RoverDifferentialSetpoint.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +float32 forward_speed_setpoint # [m/s] Desired forward speed for the rover +float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover +float32 yaw_rate_setpoint # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used) +float32 yaw_rate_setpoint_normalized # [-1, 1] Normalized yaw rate for the rover +float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover + +# TOPICS rover_differential_setpoint diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg index 31907ffa6477..4d3d54eb9909 100644 --- a/msg/RoverDifferentialStatus.msg +++ b/msg/RoverDifferentialStatus.msg @@ -1,8 +1,11 @@ uint64 timestamp # time since system start (microseconds) float32 actual_speed # [m/s] Actual forward speed of the rover -float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate +float32 actual_yaw_deg # [deg] Actual yaw of the rover float32 actual_yaw_rate_deg_s # [deg/s] Actual yaw rate of the rover -float32 pid_yaw_rate_integral # Integral of the PID for the desired yaw rate controller +float32 desired_yaw_rate_deg_s # [deg/s] Yaw rate setpoint for the closed loop yaw rate controller +float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller +float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller +float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller # TOPICS rover_differential_status diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 5be39234e34f..f5ce636c23c9 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -105,6 +105,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("rover_ackermann_guidance_status", 100); add_optional_topic("rover_ackermann_status", 100); add_optional_topic("rover_differential_guidance_status", 100); + add_optional_topic("rover_differential_setpoint", 100); add_optional_topic("rover_differential_status", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index beaec32a8776..880589316074 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2024 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 @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(RoverDifferentialGuidance) +add_subdirectory(RoverDifferentialControl) px4_add_module( MODULE modules__rover_differential @@ -41,8 +42,8 @@ px4_add_module( RoverDifferential.hpp DEPENDS RoverDifferentialGuidance + RoverDifferentialControl px4_work_queue - modules__control_allocator # for parameter CA_R_REV pure_pursuit MODULE_CONFIG module.yaml diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index ab89392a0ec8..20cbf7d9e3e2 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -32,16 +32,13 @@ ****************************************************************************/ #include "RoverDifferential.hpp" -using namespace matrix; -using namespace time_literals; RoverDifferential::RoverDifferential() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { updateParams(); - _rover_differential_status_pub.advertise(); - pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); + _rover_differential_setpoint_pub.advertise(); } bool RoverDifferential::init() @@ -53,16 +50,7 @@ bool RoverDifferential::init() void RoverDifferential::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; - - pid_set_parameters(&_pid_yaw_rate, - _param_rd_p_gain_yaw_rate.get(), // Proportional gain - _param_rd_i_gain_yaw_rate.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit - } void RoverDifferential::Run() @@ -73,135 +61,99 @@ void RoverDifferential::Run() return; } - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // uORB subscriber updates - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); - updateParams(); - } + updateSubscriptions(); - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - _nav_state = vehicle_status.nav_state; - } + // Generate and publish attitude and velocity setpoints + hrt_abstime timestamp = hrt_absolute_time(); - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; - } - - 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); - 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_forward_speed = velocity_in_body_frame(0); - } - - // Navigation modes switch (_nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: { manual_control_setpoint_s manual_control_setpoint{}; if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _differential_setpoint.throttle = manual_control_setpoint.throttle; - _differential_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); - + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; + rover_differential_setpoint.yaw_setpoint = NAN; + rover_differential_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); + rover_differential_setpoint.yaw_rate_setpoint = NAN; + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); } - - _differential_setpoint.closed_loop_yaw_rate = false; } break; case vehicle_status_s::NAVIGATION_STATE_ACRO: { manual_control_setpoint_s manual_control_setpoint{}; if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - _differential_setpoint.throttle = manual_control_setpoint.throttle; - _differential_setpoint.yaw_rate = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, - -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; + rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = NAN; + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); } - _differential_setpoint.closed_loop_yaw_rate = true; } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _differential_setpoint = _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, - _nav_state); + _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); break; default: // Unimplemented nav states will stop the rover - _differential_setpoint.throttle = 0.f; - _differential_setpoint.yaw_rate = 0.f; - _differential_setpoint.closed_loop_yaw_rate = false; + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; + rover_differential_setpoint.yaw_rate_setpoint = NAN; + rover_differential_setpoint.yaw_rate_setpoint_normalized = 0.f; + rover_differential_setpoint.yaw_setpoint = NAN; + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); break; } - float speed_diff_normalized = _differential_setpoint.yaw_rate; - - // Closed loop yaw rate control - if (_differential_setpoint.closed_loop_yaw_rate) { - if (fabsf(_differential_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD) { - speed_diff_normalized = 0.f; - pid_reset_integral(&_pid_yaw_rate); - - } else { - const float speed_diff = _differential_setpoint.yaw_rate * _param_rd_wheel_track.get(); // Feedforward - speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_speed.get(), - _param_rd_max_speed.get(), -1.f, 1.f); - speed_diff_normalized = math::constrain(speed_diff_normalized + - pid_calculate(&_pid_yaw_rate, _differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt), - -1.f, 1.f); // Feedback - } - - } else { - pid_reset_integral(&_pid_yaw_rate); - } - - // Publish rover differential status (logging) - rover_differential_status_s rover_differential_status{}; - rover_differential_status.timestamp = _timestamp; - rover_differential_status.actual_speed = _vehicle_forward_speed; - rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _differential_setpoint.yaw_rate; - rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate; - rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; - _rover_differential_status_pub.publish(rover_differential_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeMotorCommands(_differential_setpoint.throttle, speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); + _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); } -matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff) +void RoverDifferential::updateSubscriptions() { - float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff); - if (combined_velocity > 1.0f) { // Prioritize yaw rate - float excess_velocity = fabsf(combined_velocity - 1.0f); - forward_speed -= sign(forward_speed) * excess_velocity; + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; } - // Calculate the left and right wheel speeds - return Vector2f(forward_speed - speed_diff, - forward_speed + speed_diff); + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + + 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); + 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_forward_speed = velocity_in_body_frame(0); + } } int RoverDifferential::task_spawn(int argc, char *argv[]) diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 3dafe99b0566..897a7199ef4d 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -42,23 +42,21 @@ // uORB includes #include -#include #include #include #include #include -#include #include #include #include -#include +#include // Standard libraries -#include #include // Local includes #include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" +#include "RoverDifferentialControl/RoverDifferentialControl.hpp" using namespace time_literals; @@ -80,21 +78,17 @@ class RoverDifferential : public ModuleBase, public ModulePar bool init(); - /** - * @brief Computes motor commands for differential drive. - * - * @param forward_speed Linear velocity along the x-axis. - * @param speed_diff Speed difference between left and right wheels. - * @return matrix::Vector2f Motor velocities for the right and left motors. - */ - matrix::Vector2f computeMotorCommands(float forward_speed, const float speed_diff); - protected: void updateParams() override; private: void Run() override; + /** + * @brief Update uORB subscriptions. + */ + void updateSubscriptions(); + // uORB Subscriptions uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; @@ -104,33 +98,22 @@ class RoverDifferential : public ModuleBase, public ModulePar uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // uORB Publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; + uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; // Instances RoverDifferentialGuidance _rover_differential_guidance{this}; + RoverDifferentialControl _rover_differential_control{this}; // Variables - float _vehicle_body_yaw_rate{0.f}; + matrix::Quatf _vehicle_attitude_quaternion{}; + float _vehicle_yaw_rate{0.f}; float _vehicle_forward_speed{0.f}; float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; int _nav_state{0}; - matrix::Quatf _vehicle_attitude_quaternion{}; - hrt_abstime _timestamp{0}; - PID_t _pid_yaw_rate; // The PID controller for yaw rate - RoverDifferentialGuidance::differential_setpoint _differential_setpoint; - - // Constants - static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control DEFINE_PARAMETERS( (ParamFloat) _param_rd_man_yaw_scale, - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_p_gain_yaw_rate, - (ParamFloat) _param_rd_i_gain_yaw_rate, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamInt) _param_r_rev + (ParamFloat) _param_rd_max_yaw_rate ) }; diff --git a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt new file mode 100644 index 000000000000..58a5239f8cc7 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 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(RoverDifferentialControl + RoverDifferentialControl.cpp +) + +target_link_libraries(RoverDifferentialControl PUBLIC pid) +target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp new file mode 100644 index 000000000000..c1f96c0ce066 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "RoverDifferentialControl.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_differential_status_pub.advertise(); + pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_yaw, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void RoverDifferentialControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + pid_set_parameters(&_pid_yaw_rate, + _param_rd_yaw_rate_p.get(), // Proportional gain + _param_rd_yaw_rate_i.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + pid_set_parameters(&_pid_throttle, + _param_rd_p_gain_speed.get(), // Proportional gain + _param_rd_i_gain_speed.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + pid_set_parameters(&_pid_yaw, + _param_rd_p_gain_yaw.get(), // Proportional gain + _param_rd_i_gain_yaw.get(), // Integral gain + 0.f, // Derivative gain + _max_yaw_rate, // Integral limit + _max_yaw_rate); // Output limit +} + +void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, + const float vehicle_forward_speed) +{ + // Timestamps + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // Update differential setpoint + _rover_differential_setpoint_sub.update(&_rover_differential_setpoint); + + // Closed loop yaw control (Overrides yaw rate setpoint) + if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { + if (fabsf(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw) < FLT_EPSILON) { + _rover_differential_setpoint.yaw_rate_setpoint = 0.f; + pid_reset_integral(&_pid_yaw); + + } else { + const float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw); + _rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt); + } + } + + // Yaw rate control + float speed_diff_normalized{0.f}; + + if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control + if (fabsf(_rover_differential_setpoint.yaw_rate_setpoint) < FLT_EPSILON) { + speed_diff_normalized = 0.f; + pid_reset_integral(&_pid_yaw_rate); + + } else { + const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get(); // Feedforward + speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_speed.get(), + _param_rd_max_speed.get(), -1.f, 1.f); + speed_diff_normalized = math::constrain(speed_diff_normalized + + pid_calculate(&_pid_yaw_rate, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt), + -1.f, 1.f); // Feedback + } + + } else { // Use normalized setpoint + speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ? + math::constrain(_rover_differential_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f; + } + + // Speed control + float throttle{0.f}; + + if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control + if (fabsf(_rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { + pid_reset_integral(&_pid_throttle); + + } else { + throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0, + dt); + + if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term + throttle += math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, + 0.f, _param_rd_max_speed.get(), + 0.f, 1.f); + } + } + + } else { // Use normalized setpoint + throttle = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ? + math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f; + } + + // Publish rover differential status (logging) + rover_differential_status_s rover_differential_status{}; + rover_differential_status.timestamp = _timestamp; + rover_differential_status.actual_speed = vehicle_forward_speed; + rover_differential_status.actual_yaw_deg = M_RAD_TO_DEG_F * vehicle_yaw; + rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _rover_differential_setpoint.yaw_rate_setpoint; + rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * vehicle_yaw_rate; + rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; + rover_differential_status.pid_throttle_integral = _pid_throttle.integral; + rover_differential_status.pid_yaw_integral = _pid_yaw.integral; + _rover_differential_status_pub.publish(rover_differential_status); + + // Publish to motors + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(throttle, speed_diff_normalized).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + +} + +matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized, + const float speed_diff_normalized) +{ + float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized); + + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + forward_speed_normalized -= sign(forward_speed_normalized) * excess; + } + + // Calculate the left and right wheel speeds + return Vector2f(forward_speed_normalized - speed_diff_normalized, + forward_speed_normalized + speed_diff_normalized); +} diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp new file mode 100644 index 000000000000..d3ef7a076fd6 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + +// uORB includes +#include +#include +#include +#include +#include +#include + + +// Standard libraries +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for differential rover guidance. + */ +class RoverDifferentialControl : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverDifferentialControl. + * @param parent The parent ModuleParams object. + */ + RoverDifferentialControl(ModuleParams *parent); + ~RoverDifferentialControl() = default; + + /** + * @brief Compute motor commands based on setpoints + */ + void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * + * @param forward_speed_normalized Normalized forward speed [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1]. + */ + matrix::Vector2f computeInverseKinematics(float forward_speed_normalized, const float speed_diff_normalized); + + // uORB subscriptions + uORB::Subscription _rover_differential_setpoint_sub{ORB_ID(rover_differential_setpoint)}; + + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; + + // Variables + rover_differential_setpoint_s _rover_differential_setpoint{}; + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + + // Controllers + PID_t _pid_throttle; // The PID controller for the closed loop speed control + PID_t _pid_yaw; // The PID controller for the closed loop yaw control + PID_t _pid_yaw_rate; // The PID controller for the closed loop yaw rate control + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_yaw_rate_p, + (ParamFloat) _param_rd_yaw_rate_i, + (ParamFloat) _param_rd_p_gain_speed, + (ParamFloat) _param_rd_i_gain_speed, + (ParamFloat) _param_rd_p_gain_yaw, + (ParamFloat) _param_rd_i_gain_yaw, + (ParamInt) _param_r_rev + ) +}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt index 0fd7b68c394e..81f350d48403 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt @@ -35,5 +35,4 @@ px4_add_library(RoverDifferentialGuidance RoverDifferentialGuidance.cpp ) -target_link_libraries(RoverDifferentialGuidance PUBLIC pid) target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index efad52e7b874..848a7f513acf 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -36,108 +36,45 @@ #include using namespace matrix; -using namespace time_literals; RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) { updateParams(); + _max_forward_speed = _param_rd_miss_spd_def.get(); _rover_differential_guidance_status_pub.advertise(); - pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); - } void RoverDifferentialGuidance::updateParams() { ModuleParams::updateParams(); - - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; - pid_set_parameters(&_pid_heading, - _param_rd_p_gain_heading.get(), // Proportional gain - _param_rd_i_gain_heading.get(), // Integral gain - 0.f, // Derivative gain - _max_yaw_rate, // Integral limit - _max_yaw_rate); // Output limit - pid_set_parameters(&_pid_throttle, - _param_rd_p_gain_speed.get(), // Proportional gain - _param_rd_i_gain_speed.get(), // Integral gain - 0.f, // Derivative gain - 1.f, // Integral limit - 1.f); // Output limit } -RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::computeGuidance(const float yaw, - const float actual_speed, const int nav_state) +void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const float forward_speed, const int nav_state) { - // Initializations - bool mission_finished{false}; - float desired_speed{0.f}; - float desired_yaw_rate{0.f}; - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // uORB subscriber updates - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position{}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } + updateSubscriptions(); - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); + // Catch return to launch + const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + _curr_wp(0), _curr_wp(1)); - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); - } - - _curr_pos_ned = Vector2f(local_position.x, local_position.y); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - mission_finished = mission_result.finished; - } - - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } - - const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - math::max(actual_speed, 0.f)); - - const float heading_error = matrix::wrap_pi(desired_heading - yaw); - - const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), - _curr_wp(1)); - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL - && distance_to_next_wp < _param_nav_acc_rad.get()) { // Return to launch - mission_finished = true; + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); } // State machine - if (!mission_finished && distance_to_next_wp > _param_nav_acc_rad.get()) { + float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + math::max(forward_speed, 0.f)); + const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw); + + if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { if (_currentState == GuidanceState::STOPPED) { _currentState = GuidanceState::DRIVING; } if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { - pid_reset_integral(&_pid_heading); _currentState = GuidanceState::SPOT_TURNING; } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { - pid_reset_integral(&_pid_heading); _currentState = GuidanceState::DRIVING; } @@ -146,68 +83,93 @@ RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::comp } // Guidance logic + float desired_forward_speed{0.f}; + switch (_currentState) { case GuidanceState::DRIVING: { - desired_speed = _param_rd_miss_spd_def.get(); - - if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) { - desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_accel.get(), distance_to_next_wp, 0.0f); - desired_speed = math::constrain(desired_speed, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); + // Calculate desired forward speed + desired_forward_speed = _max_forward_speed; + + if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) { + if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) { + desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), + _param_rd_max_accel.get(), distance_to_curr_wp, 0.0f); + desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed); + } } - desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); } break; case GuidanceState::SPOT_TURNING: - if (actual_speed < TURN_MAX_VELOCITY) { // Wait for the rover to stop - desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); // Turn on the spot + if (forward_speed > TURN_MAX_VELOCITY) { + desired_yaw = vehicle_yaw; // Wait for the rover to stop + } break; case GuidanceState::STOPPED: default: - desired_speed = 0.f; - desired_yaw_rate = 0.f; + desired_yaw = vehicle_yaw; break; } - // Closed loop speed control - float throttle{0.f}; + // Publish differential guidance status (logging) + hrt_abstime timestamp = hrt_absolute_time(); + rover_differential_guidance_status_s rover_differential_guidance_status{}; + rover_differential_guidance_status.timestamp = timestamp; + rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); + rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; + rover_differential_guidance_status.state_machine = (uint8_t) _currentState; + _rover_differential_guidance_status_pub.publish(rover_differential_guidance_status); + + // Publish setpoints + rover_differential_setpoint_s rover_differential_setpoint{}; + rover_differential_setpoint.timestamp = timestamp; + rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed; + rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_rate_setpoint = NAN; + rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_differential_setpoint.yaw_setpoint = desired_yaw; + _rover_differential_setpoint_pub.publish(rover_differential_setpoint); +} - if (fabsf(desired_speed) < FLT_EPSILON) { - pid_reset_integral(&_pid_throttle); +void RoverDifferentialGuidance::updateSubscriptions() +{ + if (_vehicle_global_position_sub.updated()) { + vehicle_global_position_s vehicle_global_position{}; + _vehicle_global_position_sub.copy(&vehicle_global_position); + _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); + } - } else { - throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, - dt); + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); - if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term - throttle += math::interpolate(desired_speed, - 0.f, _param_rd_max_speed.get(), - 0.f, 1.f); + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); } + + _curr_pos_ned = Vector2f(local_position.x, local_position.y); } - // Publish differential controller status (logging) - _rover_differential_guidance_status.timestamp = _timestamp; - _rover_differential_guidance_status.desired_speed = desired_speed; - _rover_differential_guidance_status.pid_throttle_integral = _pid_throttle.integral; - _rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - _rover_differential_guidance_status.pid_heading_integral = _pid_heading.integral; - _rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; - _rover_differential_guidance_status.state_machine = (uint8_t) _currentState; - _rover_differential_guidance_status_pub.publish(_rover_differential_guidance_status); - - // Return setpoints - differential_setpoint differential_setpoint_temp; - differential_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); - differential_setpoint_temp.yaw_rate = math::constrain(desired_yaw_rate, -_max_yaw_rate, - _max_yaw_rate); - differential_setpoint_temp.closed_loop_yaw_rate = true; - return differential_setpoint_temp; + if (_position_setpoint_triplet_sub.updated()) { + updateWaypoints(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } + + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } } void RoverDifferentialGuidance::updateWaypoints() @@ -243,5 +205,20 @@ void RoverDifferentialGuidance::updateWaypoints() // NED waypoint coordinates _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); + _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); + // Distances + const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; + const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; + float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); + cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem + _waypoint_transition_angle = acosf(cosin); + + // Waypoint cruising speed + if (position_setpoint_triplet.current.cruising_speed > 0.f) { + _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); + + } else { + _max_forward_speed = _param_rd_miss_spd_def.get(); + } } diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp index 29a131f8d8e2..fd3a9485a04e 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -47,9 +47,9 @@ #include #include #include +#include // Standard libraries -#include #include #include #include @@ -80,26 +80,13 @@ class RoverDifferentialGuidance : public ModuleParams RoverDifferentialGuidance(ModuleParams *parent); ~RoverDifferentialGuidance() = default; - struct differential_setpoint { - float throttle{0.f}; - float yaw_rate{0.f}; - bool closed_loop_yaw_rate{false}; - }; - /** - * @brief Compute guidance for the vehicle. - * @param yaw The yaw orientation of the vehicle in radians. - * @param actual_speed The velocity of the vehicle in m/s. - * @param dt The time step in seconds. + * @brief Compute and publish attitude and velocity setpoints based on the mission plan. + * @param vehicle_yaw The yaw of the vehicle [rad]. + * @param forward_speed The forward speed of the vehicle [m/s]. * @param nav_state Navigation state of the rover. */ - RoverDifferentialGuidance::differential_setpoint computeGuidance(float yaw, float actual_speed, - int nav_state); - - /** - * @brief Update global/ned waypoint coordinates - */ - void updateWaypoints(); + void computeGuidance(float vehicle_yaw, float forward_speed, int nav_state); protected: /** @@ -108,6 +95,16 @@ class RoverDifferentialGuidance : public ModuleParams void updateParams() override; private: + /** + * @brief Update uORB subscriptions. + */ + void updateSubscriptions(); + + /** + * @brief Update global/ned waypoint coordinates + */ + void updateWaypoints(); + // uORB subscriptions uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; @@ -116,16 +113,14 @@ class RoverDifferentialGuidance : public ModuleParams uORB::Subscription _home_position_sub{ORB_ID(home_position)}; // uORB publications + uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; - rover_differential_guidance_status_s _rover_differential_guidance_status{}; // Variables MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance. + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. PurePursuit _pure_pursuit{this}; // Pure pursuit library - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - + bool _mission_finished{false}; // Waypoints Vector2d _curr_pos{}; @@ -135,27 +130,21 @@ class RoverDifferentialGuidance : public ModuleParams Vector2d _curr_wp{}; Vector2f _curr_wp_ned{}; Vector2d _next_wp{}; + Vector2f _next_wp_ned{}; Vector2d _home_position{}; - - // Controllers - PID_t _pid_heading; // The PID controller for the heading - PID_t _pid_throttle; // The PID controller for velocity + float _max_forward_speed{0.f}; // Maximum forward speed for the rover [m/s] + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] // Constants static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rd_p_gain_heading, - (ParamFloat) _param_rd_i_gain_heading, - (ParamFloat) _param_rd_p_gain_speed, - (ParamFloat) _param_rd_i_gain_speed, (ParamFloat) _param_rd_max_speed, (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_rd_max_jerk, (ParamFloat) _param_rd_max_accel, (ParamFloat) _param_rd_miss_spd_def, - (ParamFloat) _param_rd_max_yaw_rate, (ParamFloat) _param_rd_trans_trn_drv, (ParamFloat) _param_rd_trans_drv_trn diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index be0a76c60e9b..edc3f8e5ba7f 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -23,70 +23,70 @@ parameters: In manual mode the setpoint for the yaw rate received from the rc remote is scaled by this value. type: float - min: 0.01 + min: 0.001 max: 1 increment: 0.01 - decimal: 2 + decimal: 3 default: 1 - RD_HEADING_P: + RD_YAW_P: description: - short: Proportional gain for heading controller + short: Proportional gain for closed loop yaw controller type: float min: 0 max: 100 increment: 0.01 - decimal: 2 + decimal: 3 default: 1 - RD_HEADING_I: + RD_YAW_I: description: - short: Integral gain for heading controller + short: Integral gain for closed loop yaw controller type: float min: 0 max: 100 increment: 0.01 - decimal: 2 - default: 0.1 + decimal: 3 + default: 0 RD_SPEED_P: description: - short: Proportional gain for speed controller + short: Proportional gain for closed loop forward speed controller type: float min: 0 max: 100 increment: 0.01 - decimal: 2 + decimal: 3 default: 1 RD_SPEED_I: description: - short: Integral gain for ground speed controller + short: Integral gain for closed loop forward speed controller type: float min: 0 max: 100 increment: 0.01 - decimal: 2 + decimal: 3 default: 0 RD_YAW_RATE_P: description: - short: Proportional gain for angular velocity controller + short: Proportional gain for closed loop yaw rate controller type: float min: 0 max: 100 increment: 0.01 - decimal: 2 + decimal: 3 default: 1 RD_YAW_RATE_I: description: - short: Integral gain for angular velocity controller + short: Integral gain for closed loop yaw rate controller type: float min: 0 max: 100 increment: 0.01 - decimal: 2 + decimal: 3 default: 0 RD_MAX_JERK: @@ -123,11 +123,11 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 7 + default: 1 RD_MAX_YAW_RATE: description: - short: Maximum allowed yaw rate for the rover. + short: Maximum allowed yaw rate for the rover long: | This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode. type: float @@ -140,7 +140,7 @@ parameters: RD_MISS_SPD_DEF: description: - short: Default rover speed during a mission + short: Default forward speed for the rover during auto modes type: float unit: m/s min: 0 @@ -151,7 +151,7 @@ parameters: RD_TRANS_TRN_DRV: description: - short: Heading error threshhold to switch from spot turning to driving + short: Yaw error threshhold to switch from spot turning to driving type: float unit: rad min: 0.001 @@ -162,7 +162,12 @@ parameters: RD_TRANS_DRV_TRN: description: - short: Heading error threshhold to switch from driving to spot turning + short: Yaw error threshhold to switch from driving to spot turning + long: | + This threshold is used for the state machine to switch from driving to turning based on the + error between the desired and actual yaw. It is also used as the threshold whether the rover should come + to a smooth stop at the next waypoint. This slow down effect is active if the angle between the + line segments from prevWP-currWP and currWP-nextWP is smaller then 180 - RD_TRANS_DRV_TRN. type: float unit: rad min: 0.001