From 55f51d7e7e7a525ce0a0c990ccabbcd926f1a81e Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Fri, 20 Dec 2024 15:58:58 +0100 Subject: [PATCH] ackermann: refactor code architecture --- .../airframes/4012_gz_rover_ackermann | 36 +- .../51001_axial_scx10_2_trail_honcho | 37 +- boards/px4/sitl/spacecraft.px4board | 4 +- msg/CMakeLists.txt | 10 +- msg/RoverAckermannGuidanceStatus.msg | 6 - msg/RoverAckermannSetpoint.msg | 9 - msg/RoverAckermannStatus.msg | 11 - msg/RoverAttitudeSetpoint.msg | 5 + msg/RoverAttitudeStatus.msg | 6 + msg/RoverRateSetpoint.msg | 5 + msg/RoverRateStatus.msg | 7 + msg/RoverSteeringSetpoint.msg | 6 + msg/RoverThrottleSetpoint.msg | 7 + msg/RoverVelocityStatus.msg | 12 + src/lib/CMakeLists.txt | 1 + .../rover_control}/CMakeLists.txt | 9 +- src/lib/rover_control/RoverControl.cpp | 214 +++++++++ src/lib/rover_control/RoverControl.hpp | 118 +++++ src/lib/rover_control/RoverControlTest.cpp | 115 +++++ src/lib/rover_control/module.yaml | 191 ++++++++ src/modules/logger/logged_topics.cpp | 10 +- .../AckermannAttControl.cpp | 205 +++++++++ .../AckermannAttControl.hpp | 140 ++++++ .../CMakeLists.txt | 10 +- .../AckermannPosVelControl.cpp | 418 ++++++++++++++++++ .../AckermannPosVelControl.hpp | 257 +++++++++++ .../AckermannPosVelControl/CMakeLists.txt | 39 ++ .../AckermannRateControl.cpp | 223 ++++++++++ .../AckermannRateControl.hpp | 139 ++++++ .../AckermannRateControl/CMakeLists.txt | 39 ++ src/modules/rover_ackermann/CMakeLists.txt | 17 +- src/modules/rover_ackermann/Kconfig | 3 +- .../rover_ackermann/RoverAckermann.cpp | 250 ++++------- .../rover_ackermann/RoverAckermann.hpp | 110 ++--- .../RoverAckermannControl.cpp | 228 ---------- .../RoverAckermannControl.hpp | 137 ------ .../RoverAckermannGuidance.cpp | 309 ------------- .../RoverAckermannGuidance.hpp | 215 --------- src/modules/rover_ackermann/module.yaml | 163 +------ .../rover_pos_control_params.c | 34 +- 40 files changed, 2418 insertions(+), 1337 deletions(-) delete mode 100644 msg/RoverAckermannGuidanceStatus.msg delete mode 100644 msg/RoverAckermannSetpoint.msg delete mode 100644 msg/RoverAckermannStatus.msg create mode 100644 msg/RoverAttitudeSetpoint.msg create mode 100644 msg/RoverAttitudeStatus.msg create mode 100644 msg/RoverRateSetpoint.msg create mode 100644 msg/RoverRateStatus.msg create mode 100644 msg/RoverSteeringSetpoint.msg create mode 100644 msg/RoverThrottleSetpoint.msg create mode 100644 msg/RoverVelocityStatus.msg rename src/{modules/rover_ackermann/RoverAckermannGuidance => lib/rover_control}/CMakeLists.txt (88%) create mode 100644 src/lib/rover_control/RoverControl.cpp create mode 100644 src/lib/rover_control/RoverControl.hpp create mode 100644 src/lib/rover_control/RoverControlTest.cpp create mode 100644 src/lib/rover_control/module.yaml create mode 100644 src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp create mode 100644 src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp rename src/modules/rover_ackermann/{RoverAckermannControl => AckermannAttControl}/CMakeLists.txt (85%) create mode 100644 src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp create mode 100644 src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp create mode 100644 src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt create mode 100644 src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp create mode 100644 src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp create mode 100644 src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt delete mode 100644 src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp delete mode 100644 src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp delete mode 100644 src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp delete mode 100644 src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index e058b87dc4..cd870caabb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -11,23 +11,33 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters param set-default NAV_ACC_RAD 0.5 + +# Ackermann Parameters +param set-default RA_WHEEL_BASE 0.321 param set-default RA_ACC_RAD_GAIN 2 param set-default RA_ACC_RAD_MAX 3 -param set-default RA_LAT_ACCEL_I 0.01 -param set-default RA_LAT_ACCEL_P 0.1 -param set-default RA_MAX_ACCEL 3 -param set-default RA_MAX_DECEL 6 -param set-default RA_MAX_JERK 15 -param set-default RA_MAX_LAT_ACCEL 4 -param set-default RA_MAX_SPEED 3 param set-default RA_MAX_STR_ANG 0.5236 -param set-default RA_MAX_STR_RATE 360 -param set-default RA_MAX_THR_SPEED 3.1 -param set-default RA_SPEED_I 0.01 -param set-default RA_SPEED_P 0.1 -param set-default RA_WHEEL_BASE 0.321 +param set-default RA_STR_RATE_LIM 360 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 6 +param set-default RO_JERK_LIM 15 +param set-default RO_MAX_THR_SPEED 3.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.1 +param set-default RO_YAW_RATE_P 1 +param set-default RO_YAW_RATE_LIM 180 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 3 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 3 +param set-default RO_SPEED_I 0.1 +param set-default RO_SPEED_P 1 # Pure Pursuit parameters param set-default PP_LOOKAHD_GAIN 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho index 26363414ed..6bd7bc3e2b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho +++ b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho @@ -14,24 +14,33 @@ . ${R}etc/init.d/rc.rover_ackermann_defaults param set-default BAT1_N_CELLS 3 - -# Rover parameters param set-default NAV_ACC_RAD 0.5 + +# Ackermann Parameters +param set-default RA_WHEEL_BASE 0.321 param set-default RA_ACC_RAD_GAIN 2 param set-default RA_ACC_RAD_MAX 3 -param set-default RA_LAT_ACCEL_I 0.01 -param set-default RA_LAT_ACCEL_P 0.1 -param set-default RA_MAX_ACCEL 1.5 -param set-default RA_MAX_DECEL 10 -param set-default RA_MAX_JERK 20 -param set-default RA_MAX_LAT_ACCEL 3 -param set-default RA_MAX_SPEED 2.5 param set-default RA_MAX_STR_ANG 0.5236 -param set-default RA_MAX_STR_RATE 270 -param set-default RA_MAX_THR_SPEED 2.8 -param set-default RA_SPEED_I 0.01 -param set-default RA_SPEED_P 0.1 -param set-default RA_WHEEL_BASE 0.321 +param set-default RA_STR_RATE_LIM 270 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 1.5 +param set-default RO_DECEL_LIM 10 +param set-default RO_JERK_LIM 20 +param set-default RO_MAX_THR_SPEED 2.8 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0 +param set-default RO_YAW_RATE_P 0 +param set-default RO_YAW_RATE_LIM 0 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 0 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2.5 +param set-default RO_SPEED_I 0.01 +param set-default RO_SPEED_P 0.1 # Pure pursuit parameters param set-default PP_LOOKAHD_GAIN 1 diff --git a/boards/px4/sitl/spacecraft.px4board b/boards/px4/sitl/spacecraft.px4board index a5c735f328..56c966598b 100644 --- a/boards/px4/sitl/spacecraft.px4board +++ b/boards/px4/sitl/spacecraft.px4board @@ -11,7 +11,9 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n -CONFIG_MODULES_DIFFERENTIAL_DRIVE=n +CONFIG_MODULES_ROVER_ACKERMANN=n +CONFIG_MODULES_ROVER_DIFFERENTIAL=n +CONFIG_MODULES_ROVER_MECANUM=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_CONTROL_ALLOCATOR=n CONFIG_MODULES_SPACECRAFT=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7abae77293..51c9325413 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -171,9 +171,13 @@ set(msg_files RateCtrlStatus.msg RcChannels.msg RcParameterMap.msg - RoverAckermannGuidanceStatus.msg - RoverAckermannSetpoint.msg - RoverAckermannStatus.msg + RoverAttitudeSetpoint.msg + RoverAttitudeStatus.msg + RoverVelocityStatus.msg + RoverRateSetpoint.msg + RoverRateStatus.msg + RoverSteeringSetpoint.msg + RoverThrottleSetpoint.msg RoverDifferentialGuidanceStatus.msg RoverDifferentialSetpoint.msg RoverDifferentialStatus.msg diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg deleted file mode 100644 index bc096ba4d6..0000000000 --- a/msg/RoverAckermannGuidanceStatus.msg +++ /dev/null @@ -1,6 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error # [deg] Heading error of the pure pursuit controller - -# TOPICS rover_ackermann_guidance_status diff --git a/msg/RoverAckermannSetpoint.msg b/msg/RoverAckermannSetpoint.msg deleted file mode 100644 index f504572774..0000000000 --- a/msg/RoverAckermannSetpoint.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired speed in body x direction. Positiv: forwards, Negativ: backwards -float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized speed in body x direction. Positiv: forwards, Negativ: backwards -float32 steering_setpoint # [rad] Desired steering angle -float32 steering_setpoint_normalized # [-1, 1] Desired normalized steering angle -float32 lateral_acceleration_setpoint # [m/s^2] Desired acceleration in body y direction. Positiv: right, Negativ: left. - -# TOPICS rover_ackermann_setpoint diff --git a/msg/RoverAckermannStatus.msg b/msg/RoverAckermannStatus.msg deleted file mode 100644 index b97f40e438..0000000000 --- a/msg/RoverAckermannStatus.msg +++ /dev/null @@ -1,11 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ -float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 steering_setpoint_normalized # [-1, 1] Normalized steering setpoint -float32 adjusted_steering_setpoint_normalized # [-1, 1] Normalized steering setpoint after applying slew rate -float32 measured_lateral_acceleration # [m/s^2] Measured acceleration in body y direction. Positiv: right, Negativ: left. -float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller -float32 pid_lat_accel_integral # Integral of the PID for the closed loop lateral acceleration controller - -# TOPICS rover_ackermann_status diff --git a/msg/RoverAttitudeSetpoint.msg b/msg/RoverAttitudeSetpoint.msg new file mode 100644 index 0000000000..0ea9a7ea17 --- /dev/null +++ b/msg/RoverAttitudeSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 yaw_setpoint # [rad] Expressed in NED frame + +# TOPICS rover_attitude_setpoint diff --git a/msg/RoverAttitudeStatus.msg b/msg/RoverAttitudeStatus.msg new file mode 100644 index 0000000000..197d31c2a7 --- /dev/null +++ b/msg/RoverAttitudeStatus.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_yaw # [rad/s] Measured yaw rate +float32 adjusted_yaw_setpoint # [rad/s] Yaw setpoint that is being tracked (Applied slew rates) + +# TOPICS rover_attitude_status diff --git a/msg/RoverRateSetpoint.msg b/msg/RoverRateSetpoint.msg new file mode 100644 index 0000000000..88054f9dd6 --- /dev/null +++ b/msg/RoverRateSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 yaw_rate_setpoint # [rad/s] Expressed in NED frame + +# TOPICS rover_rate_setpoint diff --git a/msg/RoverRateStatus.msg b/msg/RoverRateStatus.msg new file mode 100644 index 0000000000..4e82eb30fa --- /dev/null +++ b/msg/RoverRateStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_yaw_rate # [rad/s] Measured yaw rate +float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint that is being tracked (Applied slew rates) +float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller + +# TOPICS rover_rate_status diff --git a/msg/RoverSteeringSetpoint.msg b/msg/RoverSteeringSetpoint.msg new file mode 100644 index 0000000000..252215ee8a --- /dev/null +++ b/msg/RoverSteeringSetpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Only for Ackermann-steered rovers) +float32 normalized_speed_diff # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Only for Differential/Mecanum rovers) + +# TOPICS rover_steering_setpoint diff --git a/msg/RoverThrottleSetpoint.msg b/msg/RoverThrottleSetpoint.msg new file mode 100644 index 0000000000..3b6d7047b3 --- /dev/null +++ b/msg/RoverThrottleSetpoint.msg @@ -0,0 +1,7 @@ + +uint64 timestamp # time since system start (microseconds) + +float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] +float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1] + +# TOPICS rover_throttle_setpoint diff --git a/msg/RoverVelocityStatus.msg b/msg/RoverVelocityStatus.msg new file mode 100644 index 0000000000..f16de0f6b7 --- /dev/null +++ b/msg/RoverVelocityStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_speed_body_x # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards +float32 speed_body_x_setpoint # [m/s] Speed setpoint in body x direction. Positiv: forwards, Negativ: backwards +float32 adjusted_speed_body_x_setpoint # [m/s] Post slew rate speed setpoint in body x direction. Positiv: forwards, Negativ: backwards +float32 measured_speed_body_y # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left +float32 speed_body_y_setpoint # [m/s] Speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers) +float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers) +float32 pid_throttle_body_x_integral # Integral of the PID for the closed loop controller of the speed in body x direction +float32 pid_throttle_body_y_integral # Integral of the PID for the closed loop controller of the speed in body y direction + +# TOPICS rover_velocity_status diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 8f58109b89..1075ad4cd6 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -70,6 +70,7 @@ add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) +add_subdirectory(rover_control EXCLUDE_FROM_ALL) add_subdirectory(rtl EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt b/src/lib/rover_control/CMakeLists.txt similarity index 88% rename from src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt rename to src/lib/rover_control/CMakeLists.txt index 373fc9b1be..f3a9277d29 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt +++ b/src/lib/rover_control/CMakeLists.txt @@ -31,8 +31,11 @@ # ############################################################################ -px4_add_library(RoverAckermannGuidance - RoverAckermannGuidance.cpp +px4_add_library(rover_control + RoverControl.cpp + RoverControl.hpp ) -target_include_directories(RoverAckermannGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(rover_control PUBLIC PID) +target_link_libraries(rover_control PUBLIC geo) +px4_add_unit_gtest(SRC RoverControlTest.cpp LINKLIBS rover_control) diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp new file mode 100644 index 0000000000..7b0864a428 --- /dev/null +++ b/src/lib/rover_control/RoverControl.cpp @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * 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 "RoverControl.hpp" +using namespace matrix; +namespace RoverControl +{ +float throttleControl(SlewRate &motor_setpoint, const float throttle_setpoint, + const float current_motor_setpoint, const float max_accel, const float max_decel, const float max_thr_spd, + const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(throttle_setpoint) || !PX4_ISFINITE(current_motor_setpoint) || !PX4_ISFINITE(dt)) { + return NAN; + } + + const bool accelerating = fabsf(throttle_setpoint) > fabsf(current_motor_setpoint); + + if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate + motor_setpoint.setSlewRate(max_accel / max_thr_spd); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - + current_motor_setpoint)) { + motor_setpoint.setForcedValue(current_motor_setpoint); + } + + motor_setpoint.update(throttle_setpoint, dt); + + } else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate + motor_setpoint.setSlewRate(max_decel / max_thr_spd); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - + current_motor_setpoint)) { + motor_setpoint.setForcedValue(current_motor_setpoint); + } + + motor_setpoint.update(throttle_setpoint, dt); + + } else { // Fallthrough if slew rate parameters are not configured + motor_setpoint.setForcedValue(throttle_setpoint); + } + + return motor_setpoint.getState(); +} + +float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, + const float yaw_slew_rate, float vehicle_yaw, float yaw_setpoint, const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(yaw_setpoint) || !PX4_ISFINITE(vehicle_yaw) || !PX4_ISFINITE(dt)) { + return NAN; + } + + yaw_setpoint = wrap_pi(yaw_setpoint); + vehicle_yaw = wrap_pi(vehicle_yaw); + + if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured + adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) { + adjusted_yaw_setpoint.setForcedValue(vehicle_yaw); + } + + adjusted_yaw_setpoint.update(yaw_setpoint, dt); + + } else { + adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); + } + + // Feedback control + pid_yaw.setSetpoint( + wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)); // Error as setpoint to take care of wrapping + return pid_yaw.update(0.f, dt); + +} + +float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const float speed_setpoint, + const float vehicle_speed, const float max_accel, const float max_decel, const float max_thr_speed, const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(speed_setpoint) || !PX4_ISFINITE(vehicle_speed) || !PX4_ISFINITE(dt)) { + return NAN; + } + + // Apply acceleration and deceleration limit + if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) { + speed_with_rate_limit.setSlewRate(max_accel); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( + speed_setpoint - vehicle_speed)) { + speed_with_rate_limit.setForcedValue(vehicle_speed); + } + + speed_with_rate_limit.update(speed_setpoint, dt); + + } else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) { + speed_with_rate_limit.setSlewRate(max_decel); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( + speed_setpoint - vehicle_speed)) { + speed_with_rate_limit.setForcedValue(vehicle_speed); + } + + speed_with_rate_limit.update(speed_setpoint, dt); + + } else { // Fallthrough if slew rate is not configured + speed_with_rate_limit.setForcedValue(speed_setpoint); + } + + // Calculate normalized forward speed setpoint + float forward_speed_normalized{0.f}; + + // Feedforward + if (max_thr_speed > FLT_EPSILON) { + forward_speed_normalized = math::interpolate(speed_with_rate_limit.getState(), + -max_thr_speed, max_thr_speed, + -1.f, 1.f); + } + + // Feedback control + pid_speed.setSetpoint(speed_with_rate_limit.getState()); + forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + + return math::constrain(forward_speed_normalized, -1.f, 1.f); +} + +void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, + position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, + MapProjection &global_ned_proj_ref) +{ + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) + && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { + curr_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + } else { + curr_wp_ned = curr_pos_ned.isAllFinite() ? curr_pos_ned : Vector2f(NAN, NAN); // Fallback if current waypoint is invalid + } + + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) + && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { + prev_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.previous.lat, + position_setpoint_triplet.previous.lon); + + } else { + prev_wp_ned = curr_pos_ned.isAllFinite() ? curr_pos_ned : Vector2f(NAN, + NAN); // Fallback if previous waypoint is invalid + } + + if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) + && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { + next_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + + } else { + next_wp_ned = home_pos.isAllFinite() ? global_ned_proj_ref.project(home_pos(0), home_pos(1)) : Vector2f(NAN, + NAN); // Enables corner slow down with RTL + } +} + +float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, Vector2f &next_wp_ned) +{ + // Sanitize inputs + if (!prev_wp_ned.isAllFinite() || !curr_wp_ned.isAllFinite() || !next_wp_ned.isAllFinite()) { + return NAN; + } + + 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; + + // Waypoint overlap + if (curr_to_next_wp_ned.norm() < FLT_EPSILON || curr_to_prev_wp_ned.norm() < FLT_EPSILON) { + return NAN; + } + + 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 + return acosf(cosin); +} + +} // RoverControl diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp new file mode 100644 index 0000000000..7f76710225 --- /dev/null +++ b/src/lib/rover_control/RoverControl.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file RoverControl.hpp + * + * Functions that are shared among the different rover modules. + * Also includes the parameters that are shared among the rover modules. + */ + +#pragma once +#include +#include +#include +#include +#include +#include + +using namespace matrix; +namespace RoverControl +{ +/** + * Applies acceleration/deceleration slew rate to a throttle setpoint. + * @param motor_setpoint Throttle setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param throttle_setpoint Throttle setpoint pre slew rate [-1, 1] + * @param current_motor_setpoint Currently applied motor input [-1, 1] + * @param max_accel Maximum allowed acceleration [m/s^2] + * @param max_decel Maximum allowed deceleration [m/s^2] + * @param max_thr_spd Speed the rover drives at maximum throttle [m/s] + * @param dt Time since last update [s] + * @return Motor Setpoint [-1, 1] + */ +float throttleControl(SlewRate &motor_setpoint, float throttle_setpoint, float current_motor_setpoint, + float max_accel, float max_decel, float max_thr_spd, float dt); + +/** + * Applies yaw rate slew rate to a yaw setpoint and calculates the necessary yaw rate setpoint + * using a PID controller. + * @param adjusted_yaw_setpoint Yaw setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param pid_yaw Yaw PID (Updated by this function) + * @param yaw_slew_rate Yaw slew rate [rad/s] + * @param vehicle_yaw Measured vehicle yaw [rad] + * @param yaw_setpoint Yaw setpoint [rad] + * @param dt Time since last update [s] + * @return Yaw rate setpoint [rad/s] + */ +float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, float yaw_slew_rate, + float vehicle_yaw, float yaw_setpoint, float dt); +/** + * Applies acceleration/deceleration slew rate to a speed setpoint and calculates the necessary throttle setpoint + * using a feed forward term and PID controller. + * @param speed_with_rate_limit Speed setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param pid_speed Speed PID (Updated by this function) + * @param speed_setpoint Speed setpoint [m/s] + * @param vehicle_speed Measured vehicle speed [m/s] + * @param max_accel Maximum allowed acceleration [m/s^2] + * @param max_decel Maximum allowed deceleration [m/s^2] + * @param max_thr_speed Speed at maximum throttle [m/s] + * @param dt Time since last update [s] + * @return Throttle setpoint [-1, 1] + */ +float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float speed_setpoint, + float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt); + +/** + * Projects positionSetpointTriplet waypoints from global to ned frame. + * @param curr_wp_ned Current waypoint in NED frame (Updated by this function) + * @param prev_wp_ned Previous waypoint in NED frame (Updated by this function) + * @param next_wp_ned Next waypoint in NED frame (Updated by this function) + * @param position_setpoint_triplet Position Setpoint Triplet + * @param curr_pos Current position of the rover in global frame + * @param home_pos Home position in global frame + * @param global_ned_proj_ref Global to ned projection + */ +void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, + position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, + MapProjection &global_ned_proj_ref); + +/** + * Calculate and return the angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param prev_wp_ned Previous waypoint in NED frame + * @param curr_wp_ned Current waypoint in NED frame + * @param next_wp_ned Next waypoint in NED frame + * @return Waypoint transition angle [rad] + */ +float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, Vector2f &next_wp_ned); + +} diff --git a/src/lib/rover_control/RoverControlTest.cpp b/src/lib/rover_control/RoverControlTest.cpp new file mode 100644 index 0000000000..508cdd4486 --- /dev/null +++ b/src/lib/rover_control/RoverControlTest.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/****************************************************************** + * Test code for the Pure Pursuit algorithm + * Run this test only using "make tests TESTFILTER=RoverControl" +******************************************************************/ + +#include +#include "RoverControl.hpp" + +TEST(calcWaypointTransitionAngle, invalidInputs) +{ + Vector2f prev_wp_ned(NAN, NAN); + Vector2f curr_wp_ned(10.f, 10.f); + Vector2f next_wp_ned(10.f, 10.f); + float prevInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + prev_wp_ned = Vector2f(10.f, 10.f); + curr_wp_ned = Vector2f(NAN, NAN); + float currInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + curr_wp_ned = Vector2f(10.f, 10.f); + next_wp_ned = Vector2f(NAN, NAN); + float nextInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(prevInvalid)); + EXPECT_FALSE(PX4_ISFINITE(currInvalid)); + EXPECT_FALSE(PX4_ISFINITE(nextInvalid)); + +} + +TEST(calcWaypointTransitionAngle, validInputs) +{ + // P -- C -- N + Vector2f prev_wp_ned(0.f, 0.f); + Vector2f curr_wp_ned(10.f, 0.f); + Vector2f next_wp_ned(20.f, 0.f); + const float angle1 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle1, M_PI_F); + + /** + * N + * / + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(20.f, 10.f); + const float angle2 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle2, M_PI_F - M_PI_4_F); + + /** + * N + * | + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(10.f, 10.f); + const float angle3 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle3, M_PI_2_F); + + /** + * N + * \ + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(0.f, 10.f); + const float angle4 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle4, M_PI_4_F); + + // P/C -- N + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(0.f, 0.f); + next_wp_ned = Vector2f(10.f, 0.f); + const float angle5 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(angle5)); + + // P -- C/N + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(10.f, 0.f); + const float angle6 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(angle6)); +} diff --git a/src/lib/rover_control/module.yaml b/src/lib/rover_control/module.yaml new file mode 100644 index 0000000000..8450a03b06 --- /dev/null +++ b/src/lib/rover_control/module.yaml @@ -0,0 +1,191 @@ +module_name: Rover Control + +parameters: + - group: Rover Control + definitions: + RO_MAX_THR_SPEED: + description: + short: Speed the rover drives at maximum throttle + long: Used to linearly map speeds [m/s] to throttle values [-1. 1]. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + + RO_ACCEL_LIM: + description: + short: Acceleration limit + long: | + Set to -1 to disable. + For mecanum rovers this limit is used for longitudinal and lateral acceleration. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_DECEL_LIM: + description: + short: Deceleration limit + long: | + Set to -1 to disable. + Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + For mecanum rovers this limit is used for longitudinal and lateral deceleration. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_JERK_LIM: + description: + short: Jerk limit + long: | + Set to -1 to disable. + Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + For mecanum rovers this limit is used for longitudinal and lateral jerk. + type: float + unit: m/s^3 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_YAW_RATE_TH: + description: + short: Yaw rate measurement threshold + long: The minimum threshold for the yaw rate measurement not to be interpreted as zero. + type: float + unit: deg/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 3 + + RO_SPEED_TH: + description: + short: Speed measurement threshold + long: The minimum threshold for the speed measurement not to be interpreted as zero. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.1 + + RO_YAW_STICK_DZ: + description: + short: Yaw stick deadzone + long: Percentage of stick input range that will be interpreted as zero around the stick centered value. + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 0.1 + + - group: Rover Rate Control + definitions: + RO_YAW_RATE_P: + description: + short: Proportional gain for closed loop yaw rate controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + RO_YAW_RATE_I: + description: + short: Integral gain for closed loop yaw rate controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + RO_YAW_RATE_LIM: + description: + short: Yaw rate limit + long: | + Used to cap yaw rate setpoints and map controller inputs to yaw rate setpoints + in Acro, Stabilized and Position mode. + type: float + unit: deg/s + min: 0 + max: 10000 + increment: 0.01 + decimal: 2 + default: 0 + + RO_YAW_ACCEL_LIM: + description: + short: Yaw acceleration limit + long: Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 10000 + increment: 0.01 + decimal: 2 + default: -1 + + - group: Rover Attitude Control + definitions: + RO_YAW_P: + description: + short: Proportional gain for closed loop yaw controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + - group: Rover Velocity Control + definitions: + RO_SPEED_P: + description: + short: Proportional gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + + RO_SPEED_I: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 + + RO_SPEED_LIM: + description: + short: Speed limit + long: | + Used to cap speed setpoints and map controller inputs to speed setpoints + in Position mode. + type: float + unit: m/s + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 4b1da7d3a1..57b2a429de 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -102,9 +102,13 @@ void LoggedTopics::add_default_topics() add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); - add_optional_topic("rover_ackermann_guidance_status", 100); - add_optional_topic("rover_ackermann_setpoint", 100); - add_optional_topic("rover_ackermann_status", 100); + add_optional_topic("rover_attitude_setpoint", 100); + add_optional_topic("rover_attitude_status", 100); + add_optional_topic("rover_velocity_status", 100); + add_optional_topic("rover_rate_setpoint", 100); + add_optional_topic("rover_rate_status", 100); + add_optional_topic("rover_steering_setpoint", 100); + add_optional_topic("rover_throttle_setpoint", 100); add_optional_topic("rover_differential_guidance_status", 100); add_optional_topic("rover_differential_setpoint", 100); add_optional_topic("rover_differential_status", 100); diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp new file mode 100644 index 0000000000..1af95cbc1a --- /dev/null +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * 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 "AckermannAttControl.hpp" + +using namespace time_literals; + +AckermannAttControl::AckermannAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void AckermannAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(0.f); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void AckermannAttControl::updateAttControl() +{ + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + 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); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + // Estimate forward speed based on throttle + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors; + _actuator_motors_sub.copy(&actuator_motors); + _estimated_speed_body_x = math::interpolate (actuator_motors.control[0], -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + + if (_vehicle_control_mode.flag_control_manual_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + 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_pub.publish(rover_attitude_status); + +} + +void AckermannAttControl::generateAttitudeSetpoint() +{ + 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; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_ctl = false; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_stab_yaw_ctl) { + _stab_yaw_setpoint = _vehicle_yaw; + _stab_yaw_ctl = true; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + + + } + + } +} + +void AckermannAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + // Calculate yaw rate limit for slew rate + float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) / + _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity + float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate); + + float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool AckermannAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp new file mode 100644 index 0000000000..2acf8de524 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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 + +// Library includes +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for ackermann attitude control. + */ +class AckermannAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannAttControl. + * @param parent The parent ModuleParams object. + */ + AckermannAttControl(ModuleParams *parent); + ~AckermannAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @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 _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_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_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + float _vehicle_yaw{0.f}; + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x] + between [0, 0] and [1, _param_ro_max_thr_speed].*/ + float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode + bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt similarity index 85% rename from src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt rename to src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt index e0574682f1..127927f09e 100644 --- a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt +++ b/src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# 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 @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(RoverAckermannControl - RoverAckermannControl.cpp +px4_add_library(AckermannAttControl + AckermannAttControl.cpp ) -target_link_libraries(RoverAckermannControl PUBLIC PID) -target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(AckermannAttControl PUBLIC PID) +target_include_directories(AckermannAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp new file mode 100644 index 0000000000..e9938b5bc8 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp @@ -0,0 +1,418 @@ +/**************************************************************************** + * + * 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 "AckermannPosVelControl.hpp" + +using namespace time_literals; + +AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void AckermannPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.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_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } + + if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON + && _param_ra_max_str_ang.get() > FLT_EPSILON) { + _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); + } +} + +void AckermannPosVelControl::updatePosVelControl() +{ + 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(); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, + _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 = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed.resetIntegral(); + _speed_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_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = NAN; + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void AckermannPosVelControl::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; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void AckermannPosVelControl::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 Position 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 AckermannPosVelControl::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()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _course_control = false; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_vehicle_speed_body_x) * yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_course_control) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _course_control = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) * + vector_scaling * _pos_ctl_course_direction; + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F); + 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); + } + } +} + +void AckermannPosVelControl::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 speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + _speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { + _speed_body_x_setpoint = 0.f; + } +} + +void AckermannPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (velocity_in_local_frame.isAllFinite()) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + _speed_body_x_setpoint = velocity_in_local_frame.norm(); + } +} + +void AckermannPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + // Distances to waypoints + const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2)); + 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)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + if (_mission_finished) { + _speed_body_x_setpoint = 0.f; + + } else { // Regular guidance algorithm + + _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp, + _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _nav_state, + _waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } +} + +void AckermannPosVelControl::updateAutoSubscriptions() +{ + 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); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypointsAndAcceptanceRadius(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _prev_waypoint_transition_angle = _waypoint_transition_angle; + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Update acceptance radius + _prev_acceptance_radius = _acceptance_radius; + + if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { + _acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(), + _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get()); + + } else { + _acceptance_radius = _param_nav_acc_rad.get(); + } + + // Waypoint cruising speed + _cruising_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(); +} + +float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transition_angle, + const float default_acceptance_radius, const float acceptance_radius_gain, + const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) +{ + // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment + float acceptance_radius = default_acceptance_radius; + const float theta = waypoint_transition_angle / 2.f; + const float min_turning_radius = wheel_base / sinf(max_steer_angle); + const float acceptance_radius_temp = min_turning_radius / tanf(theta); + const float acceptance_radius_temp_scaled = acceptance_radius_gain * + acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects + acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, + acceptance_radius_max); + + // Publish updated acceptance radius + position_controller_status_s pos_ctrl_status{}; + pos_ctrl_status.acceptance_radius = acceptance_radius; + pos_ctrl_status.timestamp = _timestamp; + _position_controller_status_pub.publish(pos_ctrl_status); + return acceptance_radius; +} + +float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min, + const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, + const float prev_acc_rad, const float max_decel, const float max_jerk, const int nav_state, + const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed) +{ + // Catch improper values + if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) { + return cruising_speed; + } + + // Cornering slow down effect + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { + const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); + const float cornering_speed = _max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); + + } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + const float cornering_speed = _max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); + + } + + // Straight line speed + if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { + float straight_line_speed{0.f}; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + + } else { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + float cornering_speed = _max_yaw_rate * turning_circle; + cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed); + straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp - acc_rad, cornering_speed); + } + + return math::min(straight_line_speed, cruising_speed); + + } else { + return cruising_speed; + } + +} + +bool AckermannPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp new file mode 100644 index 0000000000..a993be28a5 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * 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 + +// Library includes +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for ackermann position control. + */ +class AckermannPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannPosVelControl. + * @param parent The parent ModuleParams object. + */ + AckermannPosVelControl(ModuleParams *parent); + ~AckermannPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Update global/NED waypoint coordinates and acceptance radius. + */ + void updateWaypointsAndAcceptanceRadius(); + + /** + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. + */ + float updateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius, + float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); + + /** + * @brief Calculate the speed setpoint. During cornering the speed is restricted based on the radius of the corner. + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next corner with the + * desired cornering speed under consideration of the maximum deceleration and jerk. + * @param cruising_speed Cruising speed [m/s]. + * @param miss_speed_min Minimum speed setpoint [m/s]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param nav_state Current nav_state of the rover. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @return Speed setpoint [m/s]. + */ + float calcSpeedSetpoint(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int nav_state, + float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed); + + /** + * @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 _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + 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 _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + + // 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 _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + 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 _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. + float _dt{0.f}; + int _nav_state{0}; + bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. + bool _mission_finished{false}; + bool _prev_param_check_passed{true}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _acceptance_radius{0.5f}; + float _prev_acceptance_radius{0.5f}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + + // Controllers + PID _pid_speed; + SlewRate _speed_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (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, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ra_acc_rad_max, + (ParamFloat) _param_ra_acc_rad_gain, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt new file mode 100644 index 0000000000..caa8fb9d5e --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(AckermannPosVelControl + AckermannPosVelControl.cpp +) + +target_link_libraries(AckermannPosVelControl PUBLIC PID) +target_include_directories(AckermannPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp new file mode 100644 index 0000000000..3ec37eb844 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * 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 "AckermannRateControl.hpp" + +using namespace time_literals; + +AckermannRateControl::AckermannRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void AckermannRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F); +} + +void AckermannRateControl::updateRateControl() +{ + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + // Estimate forward speed based on throttle + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors; + _actuator_motors_sub.copy(&actuator_motors); + _estimated_speed_body_x = math::interpolate(actuator_motors.control[0], -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + _estimated_speed_body_x = fabsf(_estimated_speed_body_x) > _param_ro_speed_th.get() ? _estimated_speed_body_x : 0.f; + } + + if (_vehicle_control_mode.flag_control_manual_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void AckermannRateControl::generateRateSetpoint() +{ + 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; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * math::interpolate + (manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } +} + +void AckermannRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float steering_setpoint{0.f}; + + if (fabsf(_estimated_speed_body_x) > FLT_EPSILON) { + // Set up feasible yaw rate setpoint + float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) / + _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity + float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate); + float constrained_yaw_rate = math::constrain(_rover_rate_setpoint.yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit); + + if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate - + _vehicle_yaw_rate)) { + _yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate); + } + + _yaw_rate_setpoint.update(constrained_yaw_rate, _dt); + + } else { + _yaw_rate_setpoint.setForcedValue(constrained_yaw_rate); + } + + // Feed forward + steering_setpoint = atanf(_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed_body_x); + + // Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability) + if (_estimated_speed_body_x > FLT_EPSILON) { + _pid_yaw_rate.setSetpoint(_yaw_rate_setpoint.getState()); + steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, _dt); + } + + } else { + _pid_yaw_rate.resetIntegral(); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_steering_angle = math::interpolate(steering_setpoint, + -_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool AckermannRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_max_thr_speed"), events::Log::Error, + "Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get()); + } + + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_wheel_base"), events::Log::Error, + "Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get()); + } + + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_max_str_ang"), events::Log::Error, + "Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get()); + } + + } + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp new file mode 100644 index 0000000000..b7e194d70c --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * 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 + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for ackermann rate control. + */ +class AckermannRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannRateControl. + * @param parent The parent ModuleParams object. + */ + AckermannRateControl(ModuleParams *parent); + ~AckermannRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @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 _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_rate_setpoint_s _rover_rate_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_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x] + between [0, 0] and [1, _param_ro_max_thr_speed].*/ + float _max_yaw_rate{0.f}; + float _vehicle_yaw_rate{0.f}; + hrt_abstime _timestamp{0}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_speed_th + ) +}; diff --git a/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt new file mode 100644 index 0000000000..aa80c0734b --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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(AckermannRateControl + AckermannRateControl.cpp +) + +target_link_libraries(AckermannRateControl PUBLIC PID) +target_include_directories(AckermannRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 423b643bdc..93269c29b9 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# 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 @@ -31,8 +31,9 @@ # ############################################################################ -add_subdirectory(RoverAckermannGuidance) -add_subdirectory(RoverAckermannControl) +add_subdirectory(AckermannRateControl) +add_subdirectory(AckermannAttControl) +add_subdirectory(AckermannPosVelControl) px4_add_module( MODULE modules__rover_ackermann @@ -41,11 +42,13 @@ px4_add_module( RoverAckermann.cpp RoverAckermann.hpp DEPENDS - RoverAckermannGuidance - RoverAckermannControl + AckermannRateControl + AckermannAttControl + AckermannPosVelControl px4_work_queue - SlewRate - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) + +set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/rover_control/module.yaml) diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig index 3ba1350659..a6acd00251 100644 --- a/src/modules/rover_ackermann/Kconfig +++ b/src/modules/rover_ackermann/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_ACKERMANN bool "rover_ackermann" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of ackermann drive rovers + Enable support for rover_ackermann diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index 5fea7e5174..c76bbf9ebb 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * 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 @@ -33,14 +33,14 @@ #include "RoverAckermann.hpp" +using namespace time_literals; + RoverAckermann::RoverAckermann() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _rover_ackermann_setpoint_pub.advertise(); - _ax_filter.setAlpha(0.05); - _ay_filter.setAlpha(0.05); - _az_filter.setAlpha(0.05); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); } @@ -53,176 +53,116 @@ bool RoverAckermann::init() void RoverAckermann::updateParams() { ModuleParams::updateParams(); + + if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) { + _servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get()); + } + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverAckermann::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish speed and steering setpoints - hrt_abstime timestamp = hrt_absolute_time(); - - 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)) { - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = NAN; - rover_ackermann_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = manual_control_setpoint.roll; - rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN; - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = NAN; - rover_ackermann_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = NAN; - rover_ackermann_setpoint.lateral_acceleration_setpoint = math::interpolate(manual_control_setpoint.roll, -1.f, 1.f, - -_param_ra_max_lat_accel.get(), _param_ra_max_lat_accel.get()); - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ra_max_speed.get(), _param_ra_max_speed.get()); - rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = NAN; - rover_ackermann_setpoint.lateral_acceleration_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_param_ra_max_lat_accel.get(), _param_ra_max_lat_accel.get()); - - if (fabsf(rover_ackermann_setpoint.lateral_acceleration_setpoint) > FLT_EPSILON - || fabsf(rover_ackermann_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _course_control = false; - - } else { // Course control if the steering input is zero (keep driving on a straight line) - if (!_course_control) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - _course_control = true; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), - 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( - rover_ackermann_setpoint.forward_speed_setpoint) * - vector_scaling * _pos_ctl_course_direction; - // Calculate steering setpoint - const float steering_setpoint = _ackermann_guidance.calcDesiredSteering(_posctl_pure_pursuit, - target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, _param_ra_wheel_base.get(), - rover_ackermann_setpoint.forward_speed_setpoint, _vehicle_yaw, _param_ra_max_steer_angle.get(), _armed); - rover_ackermann_setpoint.lateral_acceleration_setpoint = powf(_vehicle_forward_speed, - 2.f) * tanf(steering_setpoint) / _param_ra_wheel_base.get(); - } - - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _ackermann_guidance.computeGuidance(_vehicle_forward_speed, _vehicle_yaw, _nav_state, _armed); - break; - - default: // Unimplemented nav states will stop the rover - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = NAN; - rover_ackermann_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = 0.f; - rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN; - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - break; - } - - if (!_armed) { - _ackermann_control.resetControllers(); - } - - _ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw, _vehicle_lateral_acceleration); - -} - -void RoverAckermann::updateSubscriptions() { if (_parameter_update_sub.updated()) { updateParams(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - _vehicle_status_sub.copy(&vehicle_status); + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - if (vehicle_status.nav_state != _nav_state) { // Reset on mode change - _ackermann_control.resetControllers(); - _course_control = false; - } + _ackermann_pos_vel_control.updatePosVelControl(); + _ackermann_att_control.updateAttControl(); + _ackermann_rate_control.updateRateControl(); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == 2; + 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(); + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); + generateActuatorSetpoint(); - if (PX4_ISFINITE(vehicle_local_position.ax)) { - _ax_filter.update(vehicle_local_position.ax); - } +} - if (PX4_ISFINITE(vehicle_local_position.ay)) { - _ay_filter.update(vehicle_local_position.ay); - } +void RoverAckermann::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; - if (PX4_ISFINITE(vehicle_local_position.az)) { - _az_filter.update(vehicle_local_position.az); - } - - _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_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; - Vector3f acceleration_in_local_frame(_ax_filter.getState(), _ay_filter.getState(), _az_filter.getState()); - Vector3f acceleration_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(acceleration_in_local_frame); - _vehicle_lateral_acceleration = acceleration_in_body_frame(1); + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); } } +void RoverAckermann::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); + } + + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_motor_setpoint = actuator_motors.control[0]; + } + + if (_vehicle_control_mode.flag_armed) { + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_motor_setpoint, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + if (_actuator_servos_sub.updated()) { + actuator_servos_s actuator_servos{}; + _actuator_servos_sub.copy(&actuator_servos); + _current_servo_setpoint = actuator_servos.control[0]; + } + + if (_param_ra_str_rate_limit.get() > FLT_EPSILON + && _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_servo_setpoint.getState() - _current_servo_setpoint) > fabsf( + _rover_steering_setpoint.normalized_steering_angle - + _current_servo_setpoint)) { + _servo_setpoint.setForcedValue(_current_servo_setpoint); + } + + _servo_setpoint.update(_rover_steering_setpoint.normalized_steering_angle, _dt); + + } else { + _servo_setpoint.setForcedValue(_rover_steering_setpoint.normalized_steering_angle); + } + + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = _servo_setpoint.getState(); + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); +} + int RoverAckermann::task_spawn(int argc, char *argv[]) { RoverAckermann *instance = new RoverAckermann(); diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index b4e5db5b5e..635abff3f8 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * 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 @@ -39,35 +39,28 @@ #include #include #include + +// Libraries #include +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include - - -// Standard library includes -#include -#include -#include +#include +#include +#include +#include +#include +#include // Local includes -#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp" -#include "RoverAckermannControl/RoverAckermannControl.hpp" - -using namespace time_literals; - -// Constants -static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero +#include "AckermannRateControl/AckermannRateControl.hpp" +#include "AckermannAttControl/AckermannAttControl.hpp" +#include "AckermannPosVelControl/AckermannPosVelControl.hpp" class RoverAckermann : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -90,56 +83,65 @@ public: bool init(); - protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); + + /** + * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); // uORB subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; // uORB publications - uORB::Publication _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)}; + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; // Class instances - RoverAckermannGuidance _ackermann_guidance{this}; - RoverAckermannControl _ackermann_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + AckermannRateControl _ackermann_rate_control{this}; + AckermannAttControl _ackermann_att_control{this}; + AckermannPosVelControl _ackermann_pos_vel_control{this}; // Variables - matrix::Quatf _vehicle_attitude_quaternion{}; - int _nav_state{0}; - float _vehicle_forward_speed{0.f}; - float _vehicle_yaw{0.f}; - bool _armed{false}; - bool _course_control{false}; - Vector2f _pos_ctl_course_direction{}; - Vector2f _pos_ctl_start_position_ned{}; - Vector2f _curr_pos_ned{}; - float _vehicle_lateral_acceleration{0.f}; - AlphaFilter _ax_filter; - AlphaFilter _ay_filter; - AlphaFilter _az_filter; + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_servo_setpoint{0.f}; + float _current_motor_setpoint{0.f}; + + // Controllers + SlewRate _servo_setpoint{0.f}; + SlewRate _motor_setpoint{0.f}; // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_lat_accel, - (ParamFloat) _param_pp_lookahd_max - + (ParamInt) _param_r_rev, + (ParamFloat) _param_ra_str_rate_limit, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) - }; diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp deleted file mode 100644 index ad02e79141..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/**************************************************************************** - * - * 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 "RoverAckermannControl.hpp" - -#include - -using namespace matrix; - -RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_ackermann_status_pub.advertise(); -} - -void RoverAckermannControl::updateParams() -{ - ModuleParams::updateParams(); - - _pid_throttle.setGains(_param_ra_speed_p.get(), _param_ra_speed_i.get(), 0.f); - _pid_throttle.setIntegralLimit(1.f); - _pid_throttle.setOutputLimit(1.f); - - _pid_lat_accel.setGains(_param_ra_lat_accel_p.get(), _param_ra_lat_accel_i.get(), 0.f); - _pid_lat_accel.setIntegralLimit(1.f); - _pid_lat_accel.setOutputLimit(1.f); - - // Update slew rates - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) { - _forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get()); - } - - if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { - _steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) / - _param_ra_max_steer_angle.get()); - } -} - -void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_speed, const float vehicle_yaw, - const float vehicle_lateral_acceleration) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5_s) * 1e-6f; - - // Update ackermann setpoint - _rover_ackermann_setpoint_sub.update(&_rover_ackermann_setpoint); - - // Speed control - float forward_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) { - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint, - vehicle_forward_speed, dt, false); - - } else if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint_normalized, - vehicle_forward_speed, dt, true); - - } - - // Closed loop lateral acceleration control (overrides steering setpoint) - if (PX4_ISFINITE(_rover_ackermann_setpoint.lateral_acceleration_setpoint)) { - float vehicle_forward_speed_temp{0.f}; - - if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) { // Use valid measurement if available - vehicle_forward_speed_temp = vehicle_forward_speed; - - } else if (PX4_ISFINITE(forward_speed_normalized) && _param_ra_max_thr_speed.get() > FLT_EPSILON) { - vehicle_forward_speed_temp = math::interpolate(forward_speed_normalized, - -1.f, 1.f, -_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get()); - } - - if (fabsf(vehicle_forward_speed_temp) > FLT_EPSILON) { - float steering_setpoint = atanf(_param_ra_wheel_base.get() * - _rover_ackermann_setpoint.lateral_acceleration_setpoint / powf( - vehicle_forward_speed_temp, 2.f)); - - if (sign(vehicle_forward_speed_temp) == - 1) { // Only do closed loop control when driving forwards (backwards driving is non-minimum phase and can therefor introduce instability) - _pid_lat_accel.setSetpoint(_rover_ackermann_setpoint.lateral_acceleration_setpoint); - steering_setpoint += _pid_lat_accel.update(vehicle_lateral_acceleration, dt); - } - - _rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get()); - - } else { - _rover_ackermann_setpoint.steering_setpoint = 0.f; - } - } - - // Steering control - float steering_normalized{0.f}; - - if (PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint)) { - steering_normalized = math::interpolate(_rover_ackermann_setpoint.steering_setpoint, - -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint - - } else { // Use normalized setpoint - steering_normalized = PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint_normalized) ? math::constrain( - _rover_ackermann_setpoint.steering_setpoint_normalized, -1.f, 1.f) : 0.f; - } - - if (_param_ra_max_steering_rate.get() > FLT_EPSILON - && _param_ra_max_steer_angle.get() > FLT_EPSILON) { // Apply slew rate - _steering_with_rate_limit.update(steering_normalized, dt); - - } else { - _steering_with_rate_limit.setForcedValue(steering_normalized); - } - - // Publish rover Ackermann status (logging) - _rover_ackermann_status.timestamp = _timestamp; - _rover_ackermann_status.measured_forward_speed = vehicle_forward_speed; - _rover_ackermann_status.steering_setpoint_normalized = steering_normalized; - _rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState(); - _rover_ackermann_status.measured_lateral_acceleration = vehicle_lateral_acceleration; - _rover_ackermann_status.pid_throttle_integral = _pid_throttle.getIntegral(); - _rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.getIntegral(); - _rover_ackermann_status_pub.publish(_rover_ackermann_status); - - // Publish to motor - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = forward_speed_normalized; - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - - // Publish to servo - actuator_servos_s actuator_servos{}; - actuator_servos.control[0] = _steering_with_rate_limit.getState(); - actuator_servos.timestamp = _timestamp; - _actuator_servos_pub.publish(actuator_servos); - -} - -float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, - const float vehicle_forward_speed, const float dt, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = _param_ra_max_thr_speed.get() > FLT_EPSILON ? _param_ra_max_thr_speed.get() : 0.f; - } - - // Apply acceleration and deceleration limit - if (fabsf(forward_speed_setpoint) >= fabsf(_forward_speed_setpoint_with_accel_limit.getState())) { - if (_param_ra_max_accel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - _forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / slew_rate_normalization); - _forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - _forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - - } - - } else if (_param_ra_max_decel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - _forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_decel.get() / slew_rate_normalization); - _forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - _forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - } - - // Calculate normalized forward speed setpoint - float forward_speed_normalized{0.f}; - - if (normalized) { - forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState(); - - } else { // Closed loop speed control - _rover_ackermann_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); - - if (_param_ra_max_thr_speed.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(_forward_speed_setpoint_with_accel_limit.getState(), - -_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get(), - -1.f, 1.f); - } - - _pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); - forward_speed_normalized += _pid_throttle.update(vehicle_forward_speed, dt); - } - - return math::constrain(forward_speed_normalized, -1.f, 1.f); - -} - -void RoverAckermannControl::resetControllers() -{ - _pid_throttle.resetIntegral(); - _pid_lat_accel.resetIntegral(); - _forward_speed_setpoint_with_accel_limit.setForcedValue(0.f); - _steering_with_rate_limit.setForcedValue(0.f); - -} diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp deleted file mode 100644 index 7f23caff4c..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * 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 -#include - -// Standard libraries -#include -#include -#include -#include -#include -#include - -using namespace matrix; -using namespace time_literals; - -/** - * @brief Class for ackermann rover guidance. - */ -class RoverAckermannControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverAckermannControl. - * @param parent The parent ModuleParams object. - */ - RoverAckermannControl(ModuleParams *parent); - ~RoverAckermannControl() = default; - - /** - * @brief Compute motor commands based on setpoints - * @param vehicle_forward_speed Measured speed in body x direction. Positiv: forwards, Negativ: backwards [m/s] - * @param vehicle_yaw Measured yaw [rad] - * @param vehicle_lateral_acceleration Measured acceleration in body y direction. Positiv: right, Negativ: left [m/s^s] - */ - void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw, float vehicle_lateral_acceleration); - - /** - * @brief Reset PID controllers and slew rates - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Compute normalized forward speed setpoint by applying slew rates - * to the forward speed setpoint and doing closed loop speed control if enabled. - * @param forward_speed_setpoint Forward speed setpoint [m/s]. - * @param vehicle_forward_speed Actual forward speed [m/s]. - * @param dt Time since last update [s]. - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized forward speed setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float dt, bool normalized); - - // uORB subscriptions - uORB::Subscription _rover_ackermann_setpoint_sub{ORB_ID(rover_ackermann_setpoint)}; - rover_ackermann_setpoint_s _rover_ackermann_setpoint{}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; - uORB::Publication _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)}; - rover_ackermann_status_s _rover_ackermann_status{}; - - // Variables - hrt_abstime _timestamp{0}; - - // Controllers - PID _pid_throttle; // The PID controller for the closed loop speed control - PID _pid_lat_accel; // The PID controller for the closed loop speed control - SlewRate _steering_with_rate_limit{0.f}; - SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_ra_speed_p, - (ParamFloat) _param_ra_speed_i, - (ParamFloat) _param_ra_lat_accel_p, - (ParamFloat) _param_ra_lat_accel_i, - (ParamFloat) _param_ra_max_accel, - (ParamFloat) _param_ra_max_decel, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_thr_speed, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_max_steering_rate, - (ParamFloat) _param_ra_wheel_base, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp deleted file mode 100644 index e8c30083c4..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ /dev/null @@ -1,309 +0,0 @@ -/**************************************************************************** - * - * 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 "RoverAckermannGuidance.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - _rover_ackermann_guidance_status_pub.advertise(); - updateParams(); -} - -void RoverAckermannGuidance::updateParams() -{ - ModuleParams::updateParams(); - - if (_param_ra_wheel_base.get() > FLT_EPSILON && _param_ra_max_lat_accel.get() > FLT_EPSILON - && _param_ra_max_steer_angle.get() > FLT_EPSILON) { - _min_speed = sqrt(_param_ra_wheel_base.get() * _param_ra_max_lat_accel.get() / tanf(_param_ra_max_steer_angle.get())); - } -} - -void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed, - const float vehicle_yaw, const int nav_state, const bool armed) -{ - updateSubscriptions(); - - // Distances to waypoints - const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _prev_wp(0), _prev_wp(1)); - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - - // Catch return to launch - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - // Guidance logic - if (_mission_finished) { // Mission is finished - _desired_steering = 0.f; - _desired_speed = 0.f; - - } else if (_nav_cmd == 93) { // Catch delay command - _desired_speed = 0.f; - - } else { // Regular guidance algorithm - - _desired_speed = calcDesiredSpeed(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp, - _acceptance_radius, - _prev_acceptance_radius, _param_ra_max_decel.get(), _param_ra_max_jerk.get(), nav_state, _waypoint_transition_angle, - _prev_waypoint_transition_angle, _param_ra_max_speed.get()); - - _desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - _param_ra_wheel_base.get(), _desired_speed, vehicle_yaw, _param_ra_max_steer_angle.get(), armed); - - } - - // Publish ackermann controller status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - _rover_ackermann_guidance_status.timestamp = timestamp; - _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); - - // Publish speed and steering setpoints - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = _desired_speed; - rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = NAN; - rover_ackermann_setpoint.lateral_acceleration_setpoint = powf(vehicle_forward_speed, - 2.f) * tanf(_desired_steering) / _param_ra_wheel_base.get(); - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - -} - -void RoverAckermannGuidance::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); - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - - 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 (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypointsAndAcceptanceRadius(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } - - if (_navigator_mission_item_sub.updated()) { - navigator_mission_item_s navigator_mission_item{}; - _navigator_mission_item_sub.copy(&navigator_mission_item); - _nav_cmd = navigator_mission_item.nav_cmd; - } -} - -void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() -{ - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - // Global waypoint coordinates - _prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid - _curr_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if current waypoint is invalid - _next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL - - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) - && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - - } - - if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) - && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - - } - - if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) - && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - - } - - // 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 - _prev_waypoint_transition_angle = _waypoint_transition_angle; - _waypoint_transition_angle = acosf(cosin); - - // Update acceptance radius - _prev_acceptance_radius = _acceptance_radius; - - if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { - _acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(), - _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get()); - - } else { - _acceptance_radius = _param_nav_acc_rad.get(); - } - - // Waypoint cruising speed - _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get()) : _param_ra_max_speed.get(); -} - -float RoverAckermannGuidance::updateAcceptanceRadius(const float waypoint_transition_angle, - const float default_acceptance_radius, const float acceptance_radius_gain, - const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) -{ - // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment - float acceptance_radius = default_acceptance_radius; - const float theta = waypoint_transition_angle / 2.f; - const float min_turning_radius = wheel_base / sinf(max_steer_angle); - const float acceptance_radius_temp = min_turning_radius / tanf(theta); - const float acceptance_radius_temp_scaled = acceptance_radius_gain * - acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects - acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, - acceptance_radius_max); - - // Publish updated acceptance radius - position_controller_status_s pos_ctrl_status{}; - pos_ctrl_status.acceptance_radius = acceptance_radius; - pos_ctrl_status.timestamp = hrt_absolute_time(); - _position_controller_status_pub.publish(pos_ctrl_status); - return acceptance_radius; -} - -float RoverAckermannGuidance::calcDesiredSpeed(const float cruising_speed, const float miss_speed_min, - const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, - const float prev_acc_rad, const float max_decel, const float max_jerk, const int nav_state, - const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed) -{ - // Catch improper values - if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) { - return cruising_speed; - } - - // Cornering slow down effect - if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { - const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); - const float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get()); - return math::constrain(cornering_speed, miss_speed_min, cruising_speed); - - } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { - const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - const float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get()); - return math::constrain(cornering_speed, miss_speed_min, cruising_speed); - - } - - // Straight line speed - if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { - float straight_line_speed{0.f}; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp, 0.f); - - } else { - const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get()); - cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed); - straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp - acc_rad, cornering_speed); - } - - return math::min(straight_line_speed, cruising_speed); - - } else { - return cruising_speed; - } - -} - -float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, - const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, - const float vehicle_yaw, const float max_steering, const bool armed) -{ - const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, - desired_speed); - const float lookahead_distance = pure_pursuit.getLookaheadDistance(); - const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); - // For logging - _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; - _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); - - float desired_steering{0.f}; - - if (!armed) { - return desired_steering; - } - - if (math::abs_t(heading_error) <= M_PI_2_F) { - desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); - - - } else { - desired_steering = atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - - sign(heading_error) * M_PI_2_F)) / lookahead_distance); - } - - return math::constrain(desired_steering, -max_steering, max_steering); - -} diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp deleted file mode 100644 index cf6fa56fc1..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ /dev/null @@ -1,215 +0,0 @@ -/**************************************************************************** - * - * 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 -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard library includes -#include -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for ackermann drive guidance. - */ -class RoverAckermannGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverAckermannGuidance. - * @param parent The parent ModuleParams object. - */ - RoverAckermannGuidance(ModuleParams *parent); - ~RoverAckermannGuidance() = default; - - /** - * @brief Compute and publish speed and steering setpoints based on the mission plan. - * @param vehicle_forward_speed Forward speed of the vehicle [m/s] - * @param vehicle_yaw Yaw of the vehicle [rad] - * @param nav_state Vehicle navigation state. - * @param armed Vehicle arm state. - */ - void computeGuidance(float vehicle_forward_speed, float vehicle_yaw, int nav_state, bool armed); - - /** - * @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to - * reach the point calculated using the pure pursuit algorithm (see PurePursuit.hpp). - * @param pure_pursuit Pure pursuit class instance. - * @param curr_wp_ned Current waypoint in NED frame [m]. - * @param prev_wp_ned Previous waypoint in NED frame [m]. - * @param curr_pos_ned Current position of the vehicle in NED frame [m]. - * @param wheel_base Rover wheelbase [m]. - * @param desired_speed Desired speed for the rover [m/s]. - * @param vehicle_yaw Current yaw of the rover [rad]. - * @param max_steering Maximum steering angle of the rover [rad]. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param armed Vehicle arm status - * @return Steering setpoint for the rover [rad]. - */ - float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering, bool armed); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions - */ - void updateSubscriptions(); - - /** - * @brief Update global/NED waypoint coordinates and acceptance radius. - */ - void updateWaypointsAndAcceptanceRadius(); - - /** - * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment - * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param default_acceptance_radius Default acceptance radius for waypoints [m]. - * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. - * @param acceptance_radius_max Maximum value for the acceptance radius [m]. - * @param wheel_base Rover wheelbase [m]. - * @param max_steer_angle Rover maximum steer angle [rad]. - * @return Updated acceptance radius [m]. - */ - float updateAcceptanceRadius(const float waypoint_transition_angle, float default_acceptance_radius, - float acceptance_radius_gain, - float acceptance_radius_max, float wheel_base, float max_steer_angle); - - - /** - * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse - * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a speed trajectory - * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the - * maximum acceleration and jerk. - * @param cruising_speed Cruising speed [m/s]. - * @param miss_speed_min Minimum speed setpoint [m/s]. - * @param distance_to_prev_wp Distance to the previous waypoint [m]. - * @param distance_to_curr_wp Distance to the current waypoint [m]. - * @param acc_rad Acceptance radius of the current waypoint [m]. - * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. - * @param max_accel Maximum allowed acceleration [m/s^2]. - * @param max_jerk Maximum allowed jerk [m/s^3]. - * @param nav_state Current nav_state of the rover. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param max_speed Maximum speed setpoint [m/s] - * @return Speed setpoint [m/s]. - */ - float calcDesiredSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, - float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state, - float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed); - - // uORB subscriptions - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; - uORB::Subscription _navigator_mission_item_sub{ORB_ID(navigator_mission_item)}; - - // uORB publications - uORB::Publication _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)}; - uORB::Publication _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)}; - uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; - rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; - - // Class instances - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates - PurePursuit _pure_pursuit{this}; // Pure pursuit library - - // Rover variables - float _desired_steering{0.f}; - float _desired_speed{0.f}; - Vector2d _curr_pos{}; - Vector2f _curr_pos_ned{}; - bool _mission_finished{false}; - - // Waypoint variables - Vector2d _home_position{}; - Vector2f _curr_wp_ned{}; - Vector2f _prev_wp_ned{}; - Vector2f _next_wp_ned{}; - Vector2d _curr_wp{}; - Vector2d _prev_wp{}; - Vector2d _next_wp{}; - float _acceptance_radius{0.5f}; - float _prev_acceptance_radius{0.5f}; - float _cruising_speed{0.f}; - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - uint _nav_cmd{0}; - float _min_speed{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_acc_rad_max, - (ParamFloat) _param_ra_acc_rad_gain, - (ParamFloat) _param_ra_p_speed, - (ParamFloat) _param_ra_i_speed, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_jerk, - (ParamFloat) _param_ra_max_decel, - (ParamFloat) _param_ra_max_lat_accel, - (ParamFloat) _param_nav_acc_rad - ) -}; diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml index 0c779f494f..383c9e73b2 100644 --- a/src/modules/rover_ackermann/module.yaml +++ b/src/modules/rover_ackermann/module.yaml @@ -6,26 +6,37 @@ parameters: RA_WHEEL_BASE: description: short: Wheel base - long: Distance from the front to the rear axle + long: Distance from the front to the rear axle. type: float unit: m - min: 0.001 + min: 0 max: 100 increment: 0.001 decimal: 3 - default: 0.5 + default: 0 RA_MAX_STR_ANG: description: short: Maximum steering angle - long: The maximum angle that the rover can steer type: float unit: rad - min: 0.1 + min: 0 max: 1.5708 increment: 0.01 decimal: 2 - default: 0.5236 + default: 0 + + RA_STR_RATE_LIM: + description: + short: Steering rate limit + long: Set to -1 to disable. + type: float + unit: deg/s + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 RA_ACC_RAD_MAX: description: @@ -41,7 +52,7 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 3 + default: -1 RA_ACC_RAD_GAIN: description: @@ -55,142 +66,4 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 - - RA_SPEED_P: - description: - short: Proportional gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 default: 1 - - RA_SPEED_I: - description: - short: Integral gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.001 - decimal: 3 - default: 1 - - RA_MAX_SPEED: - description: - short: Maximum allowed speed - long: | - This is the maximum allowed speed setpoint when driving in a mode that uses - closed loop speed control. - type: float - unit: m/s - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_THR_SPEED: - description: - short: Speed the rover drives at maximum throttle - long: | - This parameter is used to calculate the feedforward term of the closed loop speed control which linearly - maps desired speeds to normalized motor commands [-1. 1]. - A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. - Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. - type: float - unit: m/s - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_ACCEL: - description: - short: Maximum acceleration for the rover - long: | - This is used for the acceleration slew rate. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_DECEL: - description: - short: Maximum deceleration for the rover - long: | - This is used for the deceleration slew rate, the feed-forward term - for the speed controller during missions and the corner slow down effect. - Note: For the corner slow down effect RA_MAX_JERK also has to be set. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_JERK: - description: - short: Maximum jerk - long: | - Limit for forwards acc/deceleration change. - This is used for the corner slow down effect. - Note: RA_MAX_DECEL also has to be set for this to be enabled. - type: float - unit: m/s^3 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_STR_RATE: - description: - short: Maximum steering rate for the rover - type: float - unit: deg/s - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_LAT_ACCEL: - description: - short: Maximum allowed lateral acceleration - long: | - This parameter is used to cap the lateral acceleration and map controller inputs to desired lateral acceleration - in Acro, Stabilized and Position mode. - type: float - unit: m/s^2 - min: 0.01 - max: 1000 - increment: 0.01 - decimal: 2 - default: 0.01 - - RA_LAT_ACCEL_P: - description: - short: Proportional gain for lateral acceleration controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0 - - RA_LAT_ACCEL_I: - description: - short: Integral gain for lateral acceleration controller - type: float - min: 0 - max: 100 - increment: 0.001 - decimal: 3 - default: 0 diff --git a/src/modules/rover_pos_control/rover_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c index 6458fb1fea..020cb43231 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -57,7 +57,7 @@ * @min 0.0 * @decimal 3 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f); @@ -73,7 +73,7 @@ PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f); * @max 50.0 * @decimal 1 * @increment 0.1 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f); @@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f); * @max 50.0 * @decimal 1 * @increment 0.5 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f); @@ -103,7 +103,7 @@ PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f); * @max 0.9 * @decimal 2 * @increment 0.05 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f); @@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f); @@ -133,7 +133,7 @@ PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f); @@ -148,7 +148,7 @@ PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f); @@ -160,7 +160,7 @@ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f); * @max 1 * @value 0 open loop control * @value 1 close the loop with gps speed - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1); @@ -174,7 +174,7 @@ PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f); @@ -188,7 +188,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_I, 3.0f); @@ -202,7 +202,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_I, 3.0f); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.001f); @@ -216,7 +216,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.001f); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f); @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f); @@ -243,7 +243,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f); * @max 40 * @decimal 1 * @increment 0.5 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); @@ -256,7 +256,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); * @max 40 * @decimal 1 * @increment 0.5 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); @@ -271,7 +271,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); * @max 3.14159 * @decimal 3 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); @@ -282,6 +282,6 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); * @min 0.0 * @max 400 * @decimal 1 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f);